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
92
93
94
      // Temporary object to avoid dynamic allocation.
      // Arguments are max, length, storage, take ownership.
      char* tmps[1];
      hpp::Names_t names (1, 1, tmps, false);
95
96
      graphics::Configuration pos;
      osgVector3 pos1, pos2;
Joseph Mirabel's avatar
Joseph Mirabel committed
97
      for (unsigned int i = 0; i < waypoints->length(); ++i) {
98
99
100
101
        // 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
102
103
104
        names[0] = jointName.c_str();
        hpp::TransformSeq_var Ts = hpp->robot()->getJointsPosition(waypoints[i], names);
        fromHPP(Ts[0], pos);
105
106
        pos1 = pos2; pos2 = pos.position;
        // Create the nodes
Joseph Mirabel's avatar
Joseph Mirabel committed
107
108
        if (wsm->nodeExists(xyzName)) wsm->deleteNode(xyzName, false);
        wsm->addXYZaxis(xyzName, colorN, 0.01f, 0.05f);
109
        wsm->applyConfiguration(xyzName, pos);
Joseph Mirabel's avatar
Joseph Mirabel committed
110
        if  (i > 0) {
111
112
          xyzName.replace(pn.length() + 1, 4, "edge");
          qDebug () << xyzName.c_str();
Joseph Mirabel's avatar
Joseph Mirabel committed
113
          if (wsm->nodeExists(xyzName)) wsm->deleteNode(xyzName, false);
114
          wsm->addLine(xyzName, pos1, pos2, colorE);
115
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
116
117
      }
      wsm->refresh();
118
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
119
120
121
122
123
124
125
126
127
128
129
130

    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
131
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
132
133
134

    void PathPlayer::displayPath_impl(const std::string jointName)
    {
135
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
136
137
138
139
140
      if (!pathIndex()->isEnabled()) {
        main->logError("There is no path. Did you solve a problem ?");
        emit displayPath_status(100);
        return;
      }
141
      CORBA::ULong pid = (CORBA::ULong) pathIndex()->value();
Joseph Mirabel's avatar
Joseph Mirabel committed
142
143
      std::stringstream ss; ss << "curvedpath_" << pid << "_" << jointName;
      std::string pn = ss.str();
144
      graphics::WindowsManager::Color_t colorE (1.f, 0.f, 0.f, 1.f);
145
      gepetto::gui::WindowsManagerPtr_t wsm = main->osg();
Joseph Mirabel's avatar
Joseph Mirabel committed
146
147
148
      HppWidgetsPlugin::HppClient* hpp = plugin_->client();
      CORBA::Double length = hpp->problem()->pathLength(pid);
      double dt = lengthBetweenRefresh();
149
      CORBA::ULong nbPos = (CORBA::ULong)(length / dt) + 1;
Joseph Mirabel's avatar
Joseph Mirabel committed
150
      osgVector3 pos;
151
      ::osg::Vec3ArrayRefPtr posSeq = new ::osg::Vec3Array;
Joseph Mirabel's avatar
Joseph Mirabel committed
152
153
154
155
      // Temporary object to avoid dynamic allocation.
      // Arguments are max, length, storage, take ownership.
      char* tmps[1];
      hpp::Names_t names (1, 1, tmps, false);
Joseph Mirabel's avatar
Joseph Mirabel committed
156
157
      float statusStep = 100.f / (float) nbPos;
      emit displayPath_status(0);
158
      for (CORBA::ULong i = 0; i < nbPos; ++i) {
Joseph Mirabel's avatar
Joseph Mirabel committed
159
160
        double t = std::min (i * dt, length);
        hpp::floatSeq_var q = hpp->problem()->configAtParam(pid, t);
Joseph Mirabel's avatar
Joseph Mirabel committed
161
162
163
164
        names[0] = jointName.c_str();
        hpp::TransformSeq_var Ts = hpp->robot()->getJointsPosition(q.in(), names);
        fromHPP(Ts[0], pos);
        posSeq->push_back(pos);
Joseph Mirabel's avatar
Joseph Mirabel committed
165
166
        emit displayPath_status(qFloor ((float)i * statusStep));
      }
167
      if (wsm->nodeExists(pn)) wsm->deleteNode (pn, true);
168
      wsm->addCurve(pn, posSeq, colorE);
Joseph Mirabel's avatar
Joseph Mirabel committed
169
170
171
      wsm->addToGroup(pn.c_str(), "hpp-gui");
      wsm->refresh();
      emit displayPath_status (100);
Joseph Mirabel's avatar
Joseph Mirabel committed
172
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
173
174

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

Joseph Mirabel's avatar
Joseph Mirabel committed
200
201
202
203
204
    void PathPlayer::setRobotVelocity (bool set)
    {
      velocity_ = set;
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
205
206
207
208
209
210
211
212
213
214
215
216
    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
217
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
218
219
220

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

    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
245
          currentParam_ = pathLength_;
Joseph Mirabel's avatar
Joseph Mirabel committed
246
247
          playPause()->setChecked(false);
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
248
249
250
251
252
        pathSlider()->setSliderPosition(lengthToSlider(currentParam_));
        updateConfiguration();
      }
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
253
254
255
256
257
258
259
260
261
262
263
264
    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
265
266
267
268
269
270
271
272
273
274
    void PathPlayer::timerEvent(QTimerEvent *event)
    {
      if (timerId_ == event->timerId()) {
        pathPulse();
      }
    }

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

    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
303
304
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
      return main->settings_->refreshRate;
Joseph Mirabel's avatar
Joseph Mirabel committed
305
306
307
308
    }

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

    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;
    }

337
338
    int	PathPlayer::getCurrentPath() const
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
339
      if (!pathIndex()->isEnabled()) return -1;
340
341
      return pathIndex()->value();
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
342
343
  } // namespace gui
} // namespace hpp