pathplayer.cc 11 KB
Newer Older
1
2
3
4
5
//
// Copyright (c) CNRS
// Authors: Joseph Mirabel
//

Joseph Mirabel's avatar
Joseph Mirabel committed
6
#include "hppwidgetsplugin/pathplayer.hh"
Joseph Mirabel's avatar
Joseph Mirabel committed
7

8
9
#include <QProgressDialog>
#include <QSlider>
Joseph Mirabel's avatar
Joseph Mirabel committed
10
11
12
13
14
#if (QT_VERSION < QT_VERSION_CHECK(5,0,0))
# include <QtCore>
#else
# include <QtConcurrent>
#endif
15

Joseph Mirabel's avatar
Joseph Mirabel committed
16
17
18
#include <hpp/corbaserver/common.hh>
#include <hpp/corbaserver/client.hh>

19
20
21
#include "gepetto/gui/mainwindow.hh"
#include <gepetto/gui/windows-manager.hh>
#include <gepetto/gui/osgwidget.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
22
#include <gepetto/gui/action-search-bar.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
23

24
#include "hppwidgetsplugin/conversions.hh"
25
#include "hppwidgetsplugin/ui_pathplayerwidget.h"
Joseph Mirabel's avatar
Joseph Mirabel committed
26
#include "hppwidgetsplugin/joint-action.hh"
27
#include <hppwidgetsplugin/hppwidgetsplugin.hh>
28

Joseph Mirabel's avatar
Joseph Mirabel committed
29
30
namespace hpp {
  namespace gui {
Joseph Mirabel's avatar
Joseph Mirabel committed
31
32
33
    using gepetto::gui::MainWindow;
    using gepetto::gui::ActionSearchBar;

Joseph Mirabel's avatar
Joseph Mirabel committed
34
35
36
    PathPlayer::PathPlayer (HppWidgetsPlugin *plugin, QWidget *parent) :
      QWidget (parent)
      , ui_ (new ::Ui::PathPlayerWidget)
37
      , timerId_ (0)
Joseph Mirabel's avatar
Joseph Mirabel committed
38
      , velocity_ (false)
Joseph Mirabel's avatar
Joseph Mirabel committed
39
40
41
42
43
44
45
46
47
48
      , plugin_ (plugin)
    {
      ui_->setupUi (this);
      pathIndex()->setMaximum(0);
      connect (pathSlider(), SIGNAL (sliderMoved (int)), this, SLOT (pathSliderChanged(int)));
      connect (pathIndex(), SIGNAL (valueChanged(int)), this, SLOT (pathIndexChanged(int)));
      connect (playPause(), SIGNAL (toggled (bool)), this, SLOT (playPauseToggled(bool)));
      connect (stop(), SIGNAL (clicked()), this, SLOT (stopClicked()));
      connect (ui_->refreshButton_path, SIGNAL (clicked()), this, SLOT (update()));

Joseph Mirabel's avatar
Joseph Mirabel committed
49
      initSearchActions();
Joseph Mirabel's avatar
Joseph Mirabel committed
50
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
51
52
53
54

    PathPlayer::~PathPlayer()
    {
      delete ui_;
Joseph Mirabel's avatar
Joseph Mirabel committed
55
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
56

Joseph Mirabel's avatar
Joseph Mirabel committed
57
58
59
60
61
62
63
64
65
66
67
68
69
70
    void PathPlayer::initSearchActions()
    {
      ActionSearchBar* asb = MainWindow::instance()->actionSearchBar();
      JointAction* a;

      a = new JointAction (tr("Display &waypoints of selected path"), plugin_->jointTreeWidget(), this);
      connect (a, SIGNAL (triggered(std::string)), SLOT (displayWaypointsOfPath(std::string)));
      asb->addAction(a);

      a = new JointAction (tr("Display selected &path"), plugin_->jointTreeWidget(), this);
      connect (a, SIGNAL (triggered(std::string)), SLOT (displayPath(std::string)));
      asb->addAction(a);
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
71
72
    void PathPlayer::displayWaypointsOfPath(const std::string jointName)
    {
73
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
74
75
76
77
78
79
80
      if (!pathIndex()->isEnabled()) {
        main->logError("There is no path. Did you solve a problem ?");
        return;
      }
      int pid = pathIndex()->value();
      std::stringstream ss; ss << "path" << pid << "_" << jointName;
      std::string pn = ss.str();
81
82
      const osgVector4 colorN (0.f, 0.f, 1.f, 1.f),
                       colorE (1.f, 0.f, 0.f, 1.f);
83
      gepetto::gui::WindowsManagerPtr_t wsm = main->osg();
Joseph Mirabel's avatar
Joseph Mirabel committed
84
      HppWidgetsPlugin::HppClient* hpp = plugin_->client();
85
      hpp::floatSeq_var times;
86
      hpp::floatSeqSeq_var waypoints = hpp->problem()->getWaypoints((CORBA::ULong)pid, times.out());
Joseph Mirabel's avatar
Joseph Mirabel committed
87
88
89
90
      if (!wsm->getGroup(pn, false)) {
        wsm->createGroup (pn);
        wsm->addToGroup(pn, "hpp-gui");
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
91
      hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
92
93
      graphics::Configuration pos;
      osgVector3 pos1, pos2;
Joseph Mirabel's avatar
Joseph Mirabel committed
94
      for (unsigned int i = 0; i < waypoints->length(); ++i) {
95
96
97
98
        // Make name
        ss.clear(); ss.str(std::string()); ss << pn << "/node" << i;
        std::string xyzName = ss.str();
        // Get positions
Joseph Mirabel's avatar
Joseph Mirabel committed
99
        hpp->robot()->setCurrentConfig(waypoints[i]);
Joseph Mirabel's avatar
Joseph Mirabel committed
100
        hpp::Transform__var t = hpp->robot()->getJointPosition(jointName.c_str());
101
        fromHPP(t, pos);
102
103
        pos1 = pos2; pos2 = pos.position;
        // Create the nodes
Joseph Mirabel's avatar
Joseph Mirabel committed
104
105
        if (wsm->nodeExists(xyzName)) wsm->deleteNode(xyzName, false);
        wsm->addXYZaxis(xyzName, colorN, 0.01f, 0.05f);
106
        wsm->applyConfiguration(xyzName, pos);
Joseph Mirabel's avatar
Joseph Mirabel committed
107
        if  (i > 0) {
108
109
          xyzName.replace(pn.length() + 1, 4, "edge");
          qDebug () << xyzName.c_str();
Joseph Mirabel's avatar
Joseph Mirabel committed
110
          if (wsm->nodeExists(xyzName)) wsm->deleteNode(xyzName, false);
111
          wsm->addLine(xyzName, pos1, pos2, colorE);
112
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
113
114
115
      }
      hpp->robot()->setCurrentConfig(curCfg.in());
      wsm->refresh();
116
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
117
118
119
120
121
122
123
124
125
126
127
128

    void PathPlayer::displayPath(const std::string jointName)
    {
      QFutureWatcher <void>* fw = new QFutureWatcher<void>(this);
      QProgressDialog* pd = new QProgressDialog ("Computing curved path. Please wait...", "Cancel", 0, 100, this);
      connect(this, SIGNAL(displayPath_status(int)), pd, SLOT (setValue(int)));
      pd->setCancelButton(0);
      pd->setRange(0, 100);
      pd->show();
      fw->setFuture(QtConcurrent::run (this, &PathPlayer::displayPath_impl, jointName));
      connect (fw, SIGNAL (finished()), pd, SLOT (deleteLater()));
      connect (fw, SIGNAL (finished()), fw, SLOT (deleteLater()));
Joseph Mirabel's avatar
Joseph Mirabel committed
129
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
130
131
132

    void PathPlayer::displayPath_impl(const std::string jointName)
    {
133
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
134
135
136
137
138
      if (!pathIndex()->isEnabled()) {
        main->logError("There is no path. Did you solve a problem ?");
        emit displayPath_status(100);
        return;
      }
139
      CORBA::ULong pid = (CORBA::ULong) pathIndex()->value();
Joseph Mirabel's avatar
Joseph Mirabel committed
140
141
      std::stringstream ss; ss << "curvedpath_" << pid << "_" << jointName;
      std::string pn = ss.str();
142
      graphics::WindowsManager::Color_t colorE (1.f, 0.f, 0.f, 1.f);
143
      gepetto::gui::WindowsManagerPtr_t wsm = main->osg();
Joseph Mirabel's avatar
Joseph Mirabel committed
144
145
146
147
      HppWidgetsPlugin::HppClient* hpp = plugin_->client();
      hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
      CORBA::Double length = hpp->problem()->pathLength(pid);
      double dt = lengthBetweenRefresh();
148
149
      CORBA::ULong nbPos = (CORBA::ULong)(length / dt) + 1;
      ::osg::Vec3ArrayRefPtr posSeq = new ::osg::Vec3Array;
Joseph Mirabel's avatar
Joseph Mirabel committed
150
151
      float statusStep = 100.f / (float) nbPos;
      emit displayPath_status(0);
152
      for (CORBA::ULong i = 0; i < nbPos; ++i) {
Joseph Mirabel's avatar
Joseph Mirabel committed
153
154
155
        double t = std::min (i * dt, length);
        hpp::floatSeq_var q = hpp->problem()->configAtParam(pid, t);
        hpp->robot()->setCurrentConfig(q);
156
        hpp::Transform__var transform = hpp->robot()->getJointPosition(jointName.c_str());
157
158
        const hpp::Transform__slice* tt = transform.in();
        posSeq->push_back(::osg::Vec3 ((float)tt[0],(float)tt[1],(float)tt[2]));
Joseph Mirabel's avatar
Joseph Mirabel committed
159
160
        emit displayPath_status(qFloor ((float)i * statusStep));
      }
161
      if (wsm->nodeExists(pn)) wsm->deleteNode (pn, true);
162
      wsm->addCurve(pn, posSeq, colorE);
Joseph Mirabel's avatar
Joseph Mirabel committed
163
164
165
166
      hpp->robot()->setCurrentConfig(curCfg.in());
      wsm->addToGroup(pn.c_str(), "hpp-gui");
      wsm->refresh();
      emit displayPath_status (100);
Joseph Mirabel's avatar
Joseph Mirabel committed
167
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
168
169

    void PathPlayer::update ()
Joseph Mirabel's avatar
Joseph Mirabel committed
170
    {
171
      CORBA::Long nbPath = plugin_->client()->problem ()->numberPaths ();
Joseph Mirabel's avatar
Joseph Mirabel committed
172
173
174
175
176
177
178
      if (nbPath > 0)
      {
        pathIndex()->setEnabled(true);
        pathSlider()->setEnabled(true);
        playPause()->setEnabled(true);
        stop()->setEnabled(true);
        if (pathIndex()->maximum() == 0) {
Joseph Mirabel's avatar
Joseph Mirabel committed
179
180
181
          // If path index value is 0, no signal valueChanged will
          // be emitted. Force a value changed.
          if (pathIndex()->value() == 0)
182
            pathIndexChanged(0);
Joseph Mirabel's avatar
Joseph Mirabel committed
183
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
184
        pathIndex()->setMaximum(nbPath - 1);
185
	pathIndex()->setValue(nbPath - 1);
Joseph Mirabel's avatar
Joseph Mirabel committed
186
187
188
189
190
191
192
193
194
      }
      else {
        pathIndex()->setEnabled(false);
        pathSlider()->setEnabled(false);
        playPause()->setEnabled(false);
        stop()->setEnabled(false);
      }
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
195
196
197
198
199
    void PathPlayer::setRobotVelocity (bool set)
    {
      velocity_ = set;
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
200
201
202
203
204
205
206
207
208
209
210
211
    void PathPlayer::pathIndexChanged(int i)
    {
      assert (i >= 0);
      pathLength_ = plugin_->client()->problem()->pathLength ((short unsigned int)i);
      currentParam_= 0;
    }

    void PathPlayer::pathSliderChanged(int value)
    {
      // The user moved the slider manually.
      currentParam_ = sliderToLength(value);
      updateConfiguration();
Joseph Mirabel's avatar
Joseph Mirabel committed
212
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
213
214
215

    void PathPlayer::playPauseToggled(bool toggled)
    {
216
217
218
219
220
221
      if (toggled) {
        if (timerId_ == 0) timerId_ = startTimer(timeBetweenRefresh());
      } else {
        killTimer(timerId_);
        timerId_ = 0;
      }
222
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239

    void PathPlayer::stopClicked()
    {
      // Pause
      playPause()->setChecked(false);
      // Reset current position.
      currentParam_ = 0;
      pathSlider()->setSliderPosition(0);
      updateConfiguration();
    }

    void PathPlayer::pathPulse()
    {
      if (playPause()->isChecked()) {
        // Stil playing
        currentParam_ += lengthBetweenRefresh();
        if (currentParam_ > pathLength_) {
Joseph Mirabel's avatar
Joseph Mirabel committed
240
          currentParam_ = pathLength_;
Joseph Mirabel's avatar
Joseph Mirabel committed
241
242
          playPause()->setChecked(false);
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
243
244
245
246
247
        pathSlider()->setSliderPosition(lengthToSlider(currentParam_));
        updateConfiguration();
      }
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
248
249
250
251
252
253
254
255
256
257
258
259
    void PathPlayer::setCurrentTime (const double& param)
    {
      if (!pathIndex()->isEnabled()) return;
      if (currentParam_ > pathLength_)
        currentParam_ = pathLength_;
      if (0 < currentParam_)
        currentParam_ = 0;
      currentParam_ = param;
      pathSlider()->setSliderPosition(lengthToSlider(currentParam_));
      updateConfiguration();
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
260
261
262
263
264
265
266
267
268
269
    void PathPlayer::timerEvent(QTimerEvent *event)
    {
      if (timerId_ == event->timerId()) {
        pathPulse();
      }
    }

    void PathPlayer::updateConfiguration ()
    {
      hpp::floatSeq_var config =
270
        plugin_->client()->problem()->configAtParam ((CORBA::ULong)pathIndex()->value(),currentParam_);
271
      plugin_->currentConfig() = config.in();
Joseph Mirabel's avatar
Joseph Mirabel committed
272
273
      if (velocity_) {
        config =
274
          plugin_->client()->problem()->derivativeAtParam ((CORBA::ULong)pathIndex()->value(),1,currentParam_);
275
        plugin_->currentVelocity() = config.in();
Joseph Mirabel's avatar
Joseph Mirabel committed
276
      }
277
      gepetto::gui::MainWindow::instance()->requestApplyCurrentConfiguration();
278
      emit appliedConfigAtParam (getCurrentPath(), currentParam_);
Joseph Mirabel's avatar
Joseph Mirabel committed
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
    }

    inline double PathPlayer::sliderToLength(int v) const
    {
      return ((double)v / (double)pathSlider()->maximum()) * pathLength_;
    }

    int PathPlayer::lengthToSlider(double l) const
    {
      return (int) (pathSlider()->maximum() * l / pathLength_);
    }

    inline double PathPlayer::timeToLength(double time) const
    {
      return time * pathLength_ / timeSpinBox()->value();
    }

    inline int PathPlayer::timeBetweenRefresh() const
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
298
299
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
      return main->settings_->refreshRate;
Joseph Mirabel's avatar
Joseph Mirabel committed
300
301
302
303
    }

    double PathPlayer::lengthBetweenRefresh() const
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
304
      return pathLength_ * timeBetweenRefresh() / (1000 * timeSpinBox()->value());
Joseph Mirabel's avatar
Joseph Mirabel committed
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
    }

    QDoubleSpinBox *PathPlayer::timeSpinBox() const
    {
      return ui_->timeSpinBox;
    }

    QSpinBox *PathPlayer::pathIndex() const
    {
      return ui_->pathIndexSpin;
    }

    QSlider *PathPlayer::pathSlider() const
    {
      return ui_->pathSlider;
    }

    QPushButton *PathPlayer::playPause() const
    {
      return ui_->playPauseButton;
    }

    QPushButton *PathPlayer::stop() const
    {
      return ui_->stopButton;
    }

332
333
    int	PathPlayer::getCurrentPath() const
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
334
      if (!pathIndex()->isEnabled()) return -1;
335
336
      return pathIndex()->value();
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
337
338
  } // namespace gui
} // namespace hpp