pathplayer.cc 13.2 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
#include "hppwidgetsplugin/pathplayer.hh"
Joseph Mirabel's avatar
Joseph Mirabel committed
2
3
4
5

#include <hpp/corbaserver/common.hh>
#include <hpp/corbaserver/client.hh>

6
7
8
#include "gepetto/gui/mainwindow.hh"
#include <gepetto/gui/windows-manager.hh>
#include <gepetto/gui/osgwidget.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
9
#include <gepetto/gui/action-search-bar.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
10

11
#include "hppwidgetsplugin/conversions.hh"
12
#include "hppwidgetsplugin/ui_pathplayerwidget.h"
Joseph Mirabel's avatar
Joseph Mirabel committed
13
#include "hppwidgetsplugin/joint-action.hh"
14

Joseph Mirabel's avatar
Joseph Mirabel committed
15
16
namespace hpp {
  namespace gui {
Joseph Mirabel's avatar
Joseph Mirabel committed
17
18
19
    using gepetto::gui::MainWindow;
    using gepetto::gui::ActionSearchBar;

Joseph Mirabel's avatar
Joseph Mirabel committed
20
21
22
    PathPlayer::PathPlayer (HppWidgetsPlugin *plugin, QWidget *parent) :
      QWidget (parent)
      , ui_ (new ::Ui::PathPlayerWidget)
23
      , timerId_ (0)
Joseph Mirabel's avatar
Joseph Mirabel committed
24
      , velocity_ (false)
Joseph Mirabel's avatar
Joseph Mirabel committed
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
      , process_ (new QProcess (this))
      , showPOutput_ (new QDialog (this, Qt::Dialog | Qt::WindowCloseButtonHint | Qt::WindowMinMaxButtonsHint))
      , pOutput_ (new QTextBrowser())
      , 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 (record(), SIGNAL (toggled(bool)), this, SLOT (recordToggled(bool)));
      connect (ui_->refreshButton_path, SIGNAL (clicked()), this, SLOT (update()));

      process_->setProcessChannelMode(QProcess::MergedChannels);
      connect (process_, SIGNAL (readyReadStandardOutput ()), SLOT (readyReadProcessOutput()));

      showPOutput_->setModal(false);
      showPOutput_->setLayout(new QHBoxLayout ());
      showPOutput_->layout()->addWidget(pOutput_);
Joseph Mirabel's avatar
Joseph Mirabel committed
45
46

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

    PathPlayer::~PathPlayer()
    {
      if (process_ != NULL) {
        if (process_->state() == QProcess::Running)
          process_->kill();
        delete process_;
      }
      delete ui_;
Joseph Mirabel's avatar
Joseph Mirabel committed
57
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
58

Joseph Mirabel's avatar
Joseph Mirabel committed
59
60
61
62
63
64
65
66
67
68
69
70
71
72
    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
73
74
    void PathPlayer::displayWaypointsOfPath(const std::string jointName)
    {
75
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
76
77
78
79
80
81
82
      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();
83
84
      const osgVector4 colorN (0.f, 0.f, 1.f, 1.f),
                       colorE (1.f, 0.f, 0.f, 1.f);
85
      gepetto::gui::WindowsManagerPtr_t wsm = main->osg();
Joseph Mirabel's avatar
Joseph Mirabel committed
86
      HppWidgetsPlugin::HppClient* hpp = plugin_->client();
87
88
      hpp::floatSeq_var times;
      hpp::floatSeqSeq_var waypoints = hpp->problem()->getWaypoints((CORBA::UShort)pid, times.out());
Joseph Mirabel's avatar
Joseph Mirabel committed
89
90
91
92
      if (!wsm->getGroup(pn, false)) {
        wsm->createGroup (pn);
        wsm->addToGroup(pn, "hpp-gui");
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
93
      hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
94
95
      graphics::Configuration pos;
      osgVector3 pos1, pos2;
Joseph Mirabel's avatar
Joseph Mirabel committed
96
      for (unsigned int i = 0; i < waypoints->length(); ++i) {
97
98
99
100
        // 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
101
        hpp->robot()->setCurrentConfig(waypoints[i]);
Joseph Mirabel's avatar
Joseph Mirabel committed
102
        hpp::Transform__var t = hpp->robot()->getJointPosition(jointName.c_str());
103
        fromHPP(t, pos);
104
105
        pos1 = pos2; pos2 = pos.position;
        // Create the nodes
Joseph Mirabel's avatar
Joseph Mirabel committed
106
107
        if (wsm->nodeExists(xyzName)) wsm->deleteNode(xyzName, false);
        wsm->addXYZaxis(xyzName, colorN, 0.01f, 0.05f);
108
        wsm->applyConfiguration(xyzName, pos);
Joseph Mirabel's avatar
Joseph Mirabel committed
109
        if  (i > 0) {
110
111
          xyzName.replace(pn.length() + 1, 4, "edge");
          qDebug () << xyzName.c_str();
Joseph Mirabel's avatar
Joseph Mirabel committed
112
          if (wsm->nodeExists(xyzName)) wsm->deleteNode(xyzName, false);
113
          wsm->addLine(xyzName, pos1, pos2, colorE);
114
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
115
116
117
      }
      hpp->robot()->setCurrentConfig(curCfg.in());
      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
141
142
143
      if (!pathIndex()->isEnabled()) {
        main->logError("There is no path. Did you solve a problem ?");
        emit displayPath_status(100);
        return;
      }
      CORBA::UShort pid = (CORBA::UShort) pathIndex()->value();
      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
149
      HppWidgetsPlugin::HppClient* hpp = plugin_->client();
      hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
      CORBA::Double length = hpp->problem()->pathLength(pid);
      double dt = lengthBetweenRefresh();
150
151
      CORBA::ULong nbPos = (CORBA::ULong)(length / dt) + 1;
      ::osg::Vec3ArrayRefPtr posSeq = new ::osg::Vec3Array;
Joseph Mirabel's avatar
Joseph Mirabel committed
152
153
      float statusStep = 100.f / (float) nbPos;
      emit displayPath_status(0);
154
      for (CORBA::ULong i = 0; i < nbPos; ++i) {
Joseph Mirabel's avatar
Joseph Mirabel committed
155
156
157
158
        double t = std::min (i * dt, length);
        hpp::floatSeq_var q = hpp->problem()->configAtParam(pid, t);
        hpp->robot()->setCurrentConfig(q);
        hpp::Transform__var transform = hpp->robot()->getLinkPosition(jointName.c_str());
159
160
        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
161
162
        emit displayPath_status(qFloor ((float)i * statusStep));
      }
163
      wsm->addCurve(pn, posSeq, colorE);
Joseph Mirabel's avatar
Joseph Mirabel committed
164
165
166
167
      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
168
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
169
170

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

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

Joseph Mirabel's avatar
Joseph Mirabel committed
201
202
203
204
205
206
207
208
209
210
211
212
    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
213
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
214
215
216

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

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

    void PathPlayer::recordToggled(bool toggled)
    {
237
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
238
239
240
241
      if (toggled) {
        QDir tmp ("/tmp");
        tmp.mkpath ("hpp-gui/record"); tmp.cd("hpp-gui/record");
        foreach (QString f, tmp.entryList(QStringList() << "img_0_*.jpeg", QDir::Files))
Joseph Mirabel's avatar
Joseph Mirabel committed
242
          tmp.remove(f);
Joseph Mirabel's avatar
Joseph Mirabel committed
243
244
245
        QString path = tmp.absoluteFilePath("img");
        QString ext = "jpeg";
        main->osg ()->startCapture(
Joseph Mirabel's avatar
Joseph Mirabel committed
246
247
248
            main->centralWidget()->windowID(),
            path.toLocal8Bit().data(),
            ext.toLocal8Bit().data());
Joseph Mirabel's avatar
Joseph Mirabel committed
249
250
251
252
253
        main->log("Saving images to " + path + "_*." + ext);
      } else {
        main->osg()->stopCapture(main->centralWidget()->windowID());
        QString outputFile = QFileDialog::getSaveFileName(this, tr("Save video to"), "untitled.mp4");
        if (!outputFile.isNull()) {
Joseph Mirabel's avatar
Joseph Mirabel committed
254
          if (QFile::exists(outputFile))
Joseph Mirabel's avatar
Joseph Mirabel committed
255
            QFile::remove(outputFile);
Joseph Mirabel's avatar
Joseph Mirabel committed
256
257
258
259
260
          QString avconv = "avconv";

          QStringList args;
          QString input = "/tmp/hpp-gui/record/img_0_%d.jpeg";
          args << "-r" << "50"
Joseph Mirabel's avatar
Joseph Mirabel committed
261
262
263
264
265
            << "-i" << input
            << "-vf" << "scale=trunc(iw/2)*2:trunc(ih/2)*2"
            << "-r" << "25"
            << "-vcodec" << "libx264"
            << outputFile;
Joseph Mirabel's avatar
Joseph Mirabel committed
266
267
268
269
270
271
272
273
          qDebug () << args;

          showPOutput_->setWindowTitle(avconv + " " + args.join(" "));
          pOutput_->clear();
          showPOutput_->resize(main->size() / 2);
          showPOutput_->show();
          process_->start(avconv, args);
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
274
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
275
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
276
277
278
279
280
281
282

    void PathPlayer::pathPulse()
    {
      if (playPause()->isChecked()) {
        // Stil playing
        currentParam_ += lengthBetweenRefresh();
        if (currentParam_ > pathLength_) {
Joseph Mirabel's avatar
Joseph Mirabel committed
283
          currentParam_ = pathLength_;
Joseph Mirabel's avatar
Joseph Mirabel committed
284
285
          playPause()->setChecked(false);
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
286
287
288
289
290
        pathSlider()->setSliderPosition(lengthToSlider(currentParam_));
        updateConfiguration();
      }
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
291
292
293
294
295
296
297
298
299
300
301
302
    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
303
304
305
306
307
308
309
310
311
312
    void PathPlayer::timerEvent(QTimerEvent *event)
    {
      if (timerId_ == event->timerId()) {
        pathPulse();
      }
    }

    void PathPlayer::readyReadProcessOutput()
    {
      pOutput_->append(process_->readAll());
Joseph Mirabel's avatar
Joseph Mirabel committed
313
314
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
315
316
317
318
319
    void PathPlayer::updateConfiguration ()
    {
      hpp::floatSeq_var config =
        plugin_->client()->problem()->configAtParam ((short unsigned int)pathIndex()->value(),currentParam_);
      plugin_->client()->robot()->setCurrentConfig (config.in());
Joseph Mirabel's avatar
Joseph Mirabel committed
320
321
322
323
324
      if (velocity_) {
        config =
          plugin_->client()->problem()->velocityAtParam ((short unsigned int)pathIndex()->value(),currentParam_);
        plugin_->client()->robot()->setCurrentVelocity (config.in());
      }
325
      gepetto::gui::MainWindow::instance()->requestApplyCurrentConfiguration();
326
      emit appliedConfigAtParam (getCurrentPath(), currentParam_);
Joseph Mirabel's avatar
Joseph Mirabel committed
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
    }

    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
346
347
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
      return main->settings_->refreshRate;
Joseph Mirabel's avatar
Joseph Mirabel committed
348
349
350
351
    }

    double PathPlayer::lengthBetweenRefresh() const
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
352
      return pathLength_ * timeBetweenRefresh() / (1000 * timeSpinBox()->value());
Joseph Mirabel's avatar
Joseph Mirabel committed
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
    }

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

    QPushButton *PathPlayer::record() const
    {
      return ui_->recordButton;
Joseph Mirabel's avatar
Joseph Mirabel committed
383
    }
384
385
386

    int	PathPlayer::getCurrentPath() const
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
387
      if (!pathIndex()->isEnabled()) return -1;
388
389
      return pathIndex()->value();
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
390
391
  } // namespace gui
} // namespace hpp