pathplayer.cc 10.1 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>

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

10
#include "hppwidgetsplugin/ui_pathplayerwidget.h"
11
12

PathPlayer::PathPlayer (HppWidgetsPlugin *plugin, QWidget *parent) :
Joseph Mirabel's avatar
Joseph Mirabel committed
13
  QWidget (parent)
14
, ui_ (new Ui::PathPlayerWidget)
Joseph Mirabel's avatar
Joseph Mirabel committed
15
, frameRate_ (25)
Joseph Mirabel's avatar
Joseph Mirabel committed
16
17
18
, process_ (new QProcess (this))
, showPOutput_ (new QDialog (this, Qt::Dialog | Qt::WindowCloseButtonHint | Qt::WindowMinMaxButtonsHint))
, pOutput_ (new QTextBrowser())
19
, plugin_ (plugin)
Joseph Mirabel's avatar
Joseph Mirabel committed
20
{
21
  ui_->setupUi (this);
Joseph Mirabel's avatar
Joseph Mirabel committed
22
23
24
25
26
27
  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)));
28
  connect (ui_->refreshButton_path, SIGNAL (clicked()), this, SLOT (update()));
Joseph Mirabel's avatar
Joseph Mirabel committed
29
30
31
32
33
34
35

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

  showPOutput_->setModal(false);
  showPOutput_->setLayout(new QHBoxLayout ());
  showPOutput_->layout()->addWidget(pOutput_);
36
37
38
39
}

PathPlayer::~PathPlayer()
{
Joseph Mirabel's avatar
Joseph Mirabel committed
40
41
42
43
44
  if (process_ != NULL) {
      if (process_->state() == QProcess::Running)
        process_->kill();
      delete process_;
    }
45
  delete ui_;
Joseph Mirabel's avatar
Joseph Mirabel committed
46
47
}

Joseph Mirabel's avatar
Joseph Mirabel committed
48
void PathPlayer::displayWaypointsOfPath(const std::string jointName)
49
50
{
  MainWindow* main = MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
51
52
53
54
  if (!pathIndex()->isEnabled()) {
      main->logError("There is no path. Did you solve a problem ?");
      return;
    }
55
56
57
58
59
60
61
  int pid = pathIndex()->value();
  std::stringstream ss; ss << "path" << pid << "_" << jointName;
  std::string pn = ss.str();
  float colorN[] = {0.f, 0.f, 1.f, 1.f};
  float colorE[] = {1.f, 0.f, 0.f, 1.f};
  WindowsManagerPtr_t wsm = main->osg();
  HppWidgetsPlugin::HppClient* hpp = plugin_->client();
Joseph Mirabel's avatar
Joseph Mirabel committed
62
  hpp::floatSeqSeq_var waypoints = hpp->problem()->getWaypoints((CORBA::UShort)pid);
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
  wsm->createScene (pn.c_str());
  hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
  for (unsigned int i = 0; i < waypoints->length(); ++i) {
      float pos[7];
      float pos1[3], pos2[3];
      hpp->robot()->setCurrentConfig(waypoints[i]);
      hpp::Transform__var t = hpp->robot()->getLinkPosition(jointName.c_str());
      for (int j = 0; j < 7; ++j) { pos[j] = (float)t.in()[j]; }
      for (int j = 0; j < 3; ++j) { pos1[j] = pos2[j]; }
      for (int j = 0; j < 3; ++j) { pos2[j] = (float)t.in()[j]; }
      QString xyzName = QString::fromStdString(pn).append("/node%1").arg (i);
      wsm->addXYZaxis(xyzName.toLocal8Bit().data(), colorN, 0.01f, 1.f);
      wsm->applyConfiguration(xyzName.toLocal8Bit().data(), pos);
      if  (i > 0) {
          QString lineName = QString::fromStdString(pn).append("/edge%1").arg (i);
          wsm->addLine(lineName.toLocal8Bit().data(), pos1, pos2, colorE);
        }
    }
  hpp->robot()->setCurrentConfig(curCfg.in());
  wsm->addToGroup(pn.c_str(), "hpp-gui");
  wsm->refresh();
}

Joseph Mirabel's avatar
Joseph Mirabel committed
86
void PathPlayer::displayPath(const std::string jointName)
Joseph Mirabel's avatar
Joseph Mirabel committed
87
88
89
90
91
92
93
94
95
96
97
98
99
{
  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()));
}

void PathPlayer::displayPath_impl(const std::string jointName)
Joseph Mirabel's avatar
Joseph Mirabel committed
100
101
{
  MainWindow* main = MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
102
103
104
105
106
  if (!pathIndex()->isEnabled()) {
      main->logError("There is no path. Did you solve a problem ?");
      emit displayPath_status(100);
      return;
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
107
108
109
110
111
112
113
114
115
116
117
118
  CORBA::UShort pid = (CORBA::UShort) pathIndex()->value();
  std::stringstream ss; ss << "curvedpath_" << pid << "_" << jointName;
  std::string pn = ss.str();
  float colorE[] = {1.f, 0.f, 0.f, 1.f};
  WindowsManagerPtr_t wsm = main->osg();
  HppWidgetsPlugin::HppClient* hpp = plugin_->client();
  hpp::floatSeq_var curCfg = hpp->robot()->getCurrentConfig();
  CORBA::Double length = hpp->problem()->pathLength(pid);
  double dt = lengthBetweenRefresh();
  std::size_t nbPos = (std::size_t)(length / dt) + 1;
  gepetto::corbaserver::PositionSeq_var posSeq = new gepetto::corbaserver::PositionSeq (nbPos);
  posSeq->length(nbPos);
Joseph Mirabel's avatar
Joseph Mirabel committed
119
120
  float statusStep = 100.f / (float) nbPos;
  emit displayPath_status(0);
Joseph Mirabel's avatar
Joseph Mirabel committed
121
122
123
124
125
126
  for (std::size_t i = 0; i < nbPos; ++i) {
      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());
      for (int j = 0; j < 3; ++j) { posSeq[i][j] = (float)transform.in()[j]; }
Joseph Mirabel's avatar
Joseph Mirabel committed
127
      emit displayPath_status(qFloor ((float)i * statusStep));
Joseph Mirabel's avatar
Joseph Mirabel committed
128
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
129
  wsm->addCurve(pn.c_str(), posSeq.in(), colorE);
Joseph Mirabel's avatar
Joseph Mirabel committed
130
131
132
  hpp->robot()->setCurrentConfig(curCfg.in());
  wsm->addToGroup(pn.c_str(), "hpp-gui");
  wsm->refresh();
Joseph Mirabel's avatar
Joseph Mirabel committed
133
  emit displayPath_status (100);
Joseph Mirabel's avatar
Joseph Mirabel committed
134
135
}

Joseph Mirabel's avatar
Joseph Mirabel committed
136
137
void PathPlayer::update ()
{
138
  CORBA::Short nbPath = plugin_->client()->problem ()->numberPaths ();
Joseph Mirabel's avatar
Joseph Mirabel committed
139
140
141
142
143
144
145
146
147
148
  if (nbPath > 0)
    {
      pathIndex()->setEnabled(true);
      pathSlider()->setEnabled(true);
      playPause()->setEnabled(true);
      stop()->setEnabled(true);
      if (pathIndex()->maximum() == 0) {
          // If path index value is 0, no signal valueChanged will
          // be emitted. Force a value changed.
          if (pathIndex()->value() == 0)
149
            pathIndexChanged(0);
Joseph Mirabel's avatar
Joseph Mirabel committed
150
151
152
153
154
155
156
157
158
          pathIndex()->setValue(0);
        }
      pathIndex()->setMaximum(nbPath - 1);
    }
  else {
      pathIndex()->setEnabled(false);
      pathSlider()->setEnabled(false);
      playPause()->setEnabled(false);
      stop()->setEnabled(false);
159
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
160
161
162
163
}

void PathPlayer::pathIndexChanged(int i)
{
Joseph Mirabel's avatar
Joseph Mirabel committed
164
  assert (i >= 0);
165
  pathLength_ = plugin_->client()->problem()->pathLength ((short unsigned int)i);
Joseph Mirabel's avatar
Joseph Mirabel committed
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
  currentParam_= 0;
}

void PathPlayer::pathSliderChanged(int value)
{
  // The user moved the slider manually.
  currentParam_ = sliderToLength(value);
  updateConfiguration();
}

void PathPlayer::playPauseToggled(bool toggled)
{
  if (toggled) timerId_ = startTimer(0);
  else killTimer(timerId_);
}

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

void PathPlayer::recordToggled(bool toggled)
{
Joseph Mirabel's avatar
Joseph Mirabel committed
194
  MainWindow* main = MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
195
  if (toggled) {
Joseph Mirabel's avatar
Joseph Mirabel committed
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
      QDir tmp ("/tmp");
      tmp.mkpath ("hpp-gui/record"); tmp.cd("hpp-gui/record");
      foreach (QString f, tmp.entryList(QStringList() << "img_0_*.jpeg", QDir::Files))
          tmp.remove(f);
      QString path = tmp.absoluteFilePath("img");
      QString ext = "jpeg";
      main->osg ()->startCapture(
            main->centralWidget()->windowID(),
            path.toLocal8Bit().data(),
            ext.toLocal8Bit().data());
      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()) {
          if (QFile::exists(outputFile))
               QFile::remove(outputFile);
          QString avconv = "avconv";

          QStringList args;
          QString input = "/tmp/hpp-gui/record/img_0_%d.jpeg";
          args << "-r" << "50"
               << "-i" << input
               << "-vf" << "scale=trunc(iw/2)*2:trunc(ih/2)*2"
               << "-r" << "25"
               << "-vcodec" << "libx264"
               << outputFile;
          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
231
232
233
234
235
236
237
238
239
    }
}

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
243
244
245
246
247
248
249
250
251
252
253
254
255
256
          playPause()->setChecked(false);
        }
      else
        timerId_ = startTimer(timeBetweenRefresh());
      pathSlider()->setSliderPosition(lengthToSlider(currentParam_));
      updateConfiguration();
    }
}

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

Joseph Mirabel's avatar
Joseph Mirabel committed
257
258
259
260
261
void PathPlayer::readyReadProcessOutput()
{
  pOutput_->append(process_->readAll());
}

Joseph Mirabel's avatar
Joseph Mirabel committed
262
263
264
void PathPlayer::updateConfiguration ()
{
  hpp::floatSeq_var config =
265
266
      plugin_->client()->problem()->configAtParam ((short unsigned int)pathIndex()->value(),currentParam_);
  plugin_->client()->robot()->setCurrentConfig (config.in());
267
  MainWindow::instance()->requestApplyCurrentConfiguration();
Joseph Mirabel's avatar
Joseph Mirabel committed
268
269
270
271
272
273
274
275
276
}

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

int PathPlayer::lengthToSlider(double l) const
{
Joseph Mirabel's avatar
Joseph Mirabel committed
277
  return (int) (pathSlider()->maximum() * l / pathLength_);
Joseph Mirabel's avatar
Joseph Mirabel committed
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
}

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

inline int PathPlayer::timeBetweenRefresh() const
{
  return 1000/frameRate_;
}

double PathPlayer::lengthBetweenRefresh() const
{
  return pathLength_ / (timeSpinBox()->value() * frameRate_);
}

QDoubleSpinBox *PathPlayer::timeSpinBox() const
{
297
  return ui_->timeSpinBox;
Joseph Mirabel's avatar
Joseph Mirabel committed
298
299
300
301
}

QSpinBox *PathPlayer::pathIndex() const
{
302
  return ui_->pathIndexSpin;
Joseph Mirabel's avatar
Joseph Mirabel committed
303
304
305
306
}

QSlider *PathPlayer::pathSlider() const
{
307
  return ui_->pathSlider;
Joseph Mirabel's avatar
Joseph Mirabel committed
308
309
310
311
}

QPushButton *PathPlayer::playPause() const
{
312
  return ui_->playPauseButton;
Joseph Mirabel's avatar
Joseph Mirabel committed
313
314
315
316
}

QPushButton *PathPlayer::stop() const
{
317
  return ui_->stopButton;
Joseph Mirabel's avatar
Joseph Mirabel committed
318
319
320
321
}

QPushButton *PathPlayer::record() const
{
322
  return ui_->recordButton;
Joseph Mirabel's avatar
Joseph Mirabel committed
323
}