pathplayer.cc 12.3 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
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
      , 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
44
45

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

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

Joseph Mirabel's avatar
Joseph Mirabel committed
58
59
60
61
62
63
64
65
66
67
68
69
70
71
    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
72
73
    void PathPlayer::displayWaypointsOfPath(const std::string jointName)
    {
74
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
75
76
77
78
79
80
81
      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();
82
83
      const osgVector4 colorN (0.f, 0.f, 1.f, 1.f),
                       colorE (1.f, 0.f, 0.f, 1.f);
84
      gepetto::gui::WindowsManagerPtr_t wsm = main->osg();
Joseph Mirabel's avatar
Joseph Mirabel committed
85
      HppWidgetsPlugin::HppClient* hpp = plugin_->client();
86
87
      hpp::floatSeq_var times;
      hpp::floatSeqSeq_var waypoints = hpp->problem()->getWaypoints((CORBA::UShort)pid, times.out());
Joseph Mirabel's avatar
Joseph Mirabel committed
88
89
90
91
      if (!wsm->getGroup(pn, false)) {
        wsm->createGroup (pn);
        wsm->addToGroup(pn, "hpp-gui");
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
92
      hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
93
94
      graphics::Configuration pos;
      osgVector3 pos1, pos2;
Joseph Mirabel's avatar
Joseph Mirabel committed
95
      for (unsigned int i = 0; i < waypoints->length(); ++i) {
96
97
98
99
        // 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
100
        hpp->robot()->setCurrentConfig(waypoints[i]);
Joseph Mirabel's avatar
Joseph Mirabel committed
101
        hpp::Transform__var t = hpp->robot()->getJointPosition(jointName.c_str());
102
        fromHPP(t, pos);
103
104
        pos1 = pos2; pos2 = pos.position;
        // Create the nodes
Joseph Mirabel's avatar
Joseph Mirabel committed
105
106
        if (wsm->nodeExists(xyzName)) wsm->deleteNode(xyzName, false);
        wsm->addXYZaxis(xyzName, colorN, 0.01f, 0.05f);
107
        wsm->applyConfiguration(xyzName, pos);
Joseph Mirabel's avatar
Joseph Mirabel committed
108
        if  (i > 0) {
109
110
          xyzName.replace(pn.length() + 1, 4, "edge");
          qDebug () << xyzName.c_str();
Joseph Mirabel's avatar
Joseph Mirabel committed
111
          if (wsm->nodeExists(xyzName)) wsm->deleteNode(xyzName, false);
112
          wsm->addLine(xyzName, pos1, pos2, colorE);
113
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
114
115
116
      }
      hpp->robot()->setCurrentConfig(curCfg.in());
      wsm->refresh();
117
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
118
119
120
121
122
123
124
125
126
127
128
129

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

    void PathPlayer::displayPath_impl(const std::string jointName)
    {
134
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
135
136
137
138
139
140
141
142
      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();
143
      graphics::WindowsManager::Color_t colorE (1.f, 0.f, 0.f, 1.f);
144
      gepetto::gui::WindowsManagerPtr_t wsm = main->osg();
Joseph Mirabel's avatar
Joseph Mirabel committed
145
146
147
148
      HppWidgetsPlugin::HppClient* hpp = plugin_->client();
      hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
      CORBA::Double length = hpp->problem()->pathLength(pid);
      double dt = lengthBetweenRefresh();
149
150
      CORBA::ULong nbPos = (CORBA::ULong)(length / dt) + 1;
      ::osg::Vec3ArrayRefPtr posSeq = new ::osg::Vec3Array;
Joseph Mirabel's avatar
Joseph Mirabel committed
151
152
      float statusStep = 100.f / (float) nbPos;
      emit displayPath_status(0);
153
      for (CORBA::ULong i = 0; i < nbPos; ++i) {
Joseph Mirabel's avatar
Joseph Mirabel committed
154
155
156
157
        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());
158
159
        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
160
161
        emit displayPath_status(qFloor ((float)i * statusStep));
      }
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
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
171
172
173
174
175
176
177
178
      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
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
195
196
197
198
199
200
201
202
203
204
205
206
      }
      else {
        pathIndex()->setEnabled(false);
        pathSlider()->setEnabled(false);
        playPause()->setEnabled(false);
        stop()->setEnabled(false);
      }
    }

    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
207
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
208
209
210

    void PathPlayer::playPauseToggled(bool toggled)
    {
211
212
213
214
215
216
      if (toggled) {
        if (timerId_ == 0) timerId_ = startTimer(timeBetweenRefresh());
      } else {
        killTimer(timerId_);
        timerId_ = 0;
      }
217
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
218
219
220
221
222
223
224
225
226
227
228
229
230

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

    void PathPlayer::recordToggled(bool toggled)
    {
231
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
232
233
234
235
      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
236
          tmp.remove(f);
Joseph Mirabel's avatar
Joseph Mirabel committed
237
238
239
        QString path = tmp.absoluteFilePath("img");
        QString ext = "jpeg";
        main->osg ()->startCapture(
Joseph Mirabel's avatar
Joseph Mirabel committed
240
241
242
            main->centralWidget()->windowID(),
            path.toLocal8Bit().data(),
            ext.toLocal8Bit().data());
Joseph Mirabel's avatar
Joseph Mirabel committed
243
244
245
246
247
        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
248
          if (QFile::exists(outputFile))
Joseph Mirabel's avatar
Joseph Mirabel committed
249
            QFile::remove(outputFile);
Joseph Mirabel's avatar
Joseph Mirabel committed
250
251
252
253
254
          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
255
256
257
258
259
            << "-i" << input
            << "-vf" << "scale=trunc(iw/2)*2:trunc(ih/2)*2"
            << "-r" << "25"
            << "-vcodec" << "libx264"
            << outputFile;
Joseph Mirabel's avatar
Joseph Mirabel committed
260
261
262
263
264
265
266
267
          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
268
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
269
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
270
271
272
273
274
275
276

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

    void PathPlayer::timerEvent(QTimerEvent *event)
    {
      if (timerId_ == event->timerId()) {
        pathPulse();
      }
    }

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

Joseph Mirabel's avatar
Joseph Mirabel committed
297
298
299
300
301
    void PathPlayer::updateConfiguration ()
    {
      hpp::floatSeq_var config =
        plugin_->client()->problem()->configAtParam ((short unsigned int)pathIndex()->value(),currentParam_);
      plugin_->client()->robot()->setCurrentConfig (config.in());
302
      gepetto::gui::MainWindow::instance()->requestApplyCurrentConfiguration();
Joseph Mirabel's avatar
Joseph Mirabel committed
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
    }

    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
322
323
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
      return main->settings_->refreshRate;
Joseph Mirabel's avatar
Joseph Mirabel committed
324
325
326
327
    }

    double PathPlayer::lengthBetweenRefresh() const
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
328
      return pathLength_ * timeBetweenRefresh() / (1000 * timeSpinBox()->value());
Joseph Mirabel's avatar
Joseph Mirabel committed
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
    }

    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
359
    }
360
361
362
363
364

    int	PathPlayer::getCurrentPath() const
    {
      return pathIndex()->value();
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
365
366
  } // namespace gui
} // namespace hpp