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

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

#include "hpp/gui/mainwindow.h"
7
8
#include <hpp/gui/windows-manager.h>
#include <hpp/gui/osgwidget.h>
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
51
52
53
54
55
56
57
58
59
{
  MainWindow* main = MainWindow::instance();
  if (!pathIndex()->isEnabled())
    main->logError("There is no path. Did you solve a problem ?");
  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
60
  hpp::floatSeqSeq_var waypoints = hpp->problem()->getWaypoints((CORBA::UShort)pid);
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
  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
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
void PathPlayer::displayPath(const std::string jointName)
{
  MainWindow* main = MainWindow::instance();
  if (!pathIndex()->isEnabled())
    main->logError("There is no path. Did you solve a problem ?");
  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);
  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]; }
    }
  wsm->addLines(pn.c_str(), posSeq.in(), colorE);
  hpp->robot()->setCurrentConfig(curCfg.in());
  wsm->addToGroup(pn.c_str(), "hpp-gui");
  wsm->refresh();
}

Joseph Mirabel's avatar
Joseph Mirabel committed
114
115
void PathPlayer::update ()
{
116
  CORBA::Short nbPath = plugin_->client()->problem ()->numberPaths ();
Joseph Mirabel's avatar
Joseph Mirabel committed
117
118
119
120
121
122
123
124
125
126
  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)
127
            pathIndexChanged(0);
Joseph Mirabel's avatar
Joseph Mirabel committed
128
129
130
131
132
133
134
135
136
          pathIndex()->setValue(0);
        }
      pathIndex()->setMaximum(nbPath - 1);
    }
  else {
      pathIndex()->setEnabled(false);
      pathSlider()->setEnabled(false);
      playPause()->setEnabled(false);
      stop()->setEnabled(false);
137
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
138
139
140
141
}

void PathPlayer::pathIndexChanged(int i)
{
Joseph Mirabel's avatar
Joseph Mirabel committed
142
  assert (i >= 0);
143
  pathLength_ = plugin_->client()->problem()->pathLength ((short unsigned int)i);
Joseph Mirabel's avatar
Joseph Mirabel committed
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
  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
172
  MainWindow* main = MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
173
  if (toggled) {
Joseph Mirabel's avatar
Joseph Mirabel committed
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
      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
209
210
211
212
213
214
215
216
217
    }
}

void PathPlayer::pathPulse()
{
  if (playPause()->isChecked()) {
      // Stil playing
      currentParam_ += lengthBetweenRefresh();
      if (currentParam_ > pathLength_) {
Joseph Mirabel's avatar
Joseph Mirabel committed
218
          currentParam_ = pathLength_;
Joseph Mirabel's avatar
Joseph Mirabel committed
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
          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
235
236
237
238
239
void PathPlayer::readyReadProcessOutput()
{
  pOutput_->append(process_->readAll());
}

Joseph Mirabel's avatar
Joseph Mirabel committed
240
241
242
void PathPlayer::updateConfiguration ()
{
  hpp::floatSeq_var config =
243
244
      plugin_->client()->problem()->configAtParam ((short unsigned int)pathIndex()->value(),currentParam_);
  plugin_->client()->robot()->setCurrentConfig (config.in());
245
  MainWindow::instance()->requestApplyCurrentConfiguration();
Joseph Mirabel's avatar
Joseph Mirabel committed
246
247
248
249
250
251
252
253
254
}

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
255
  return (int) (pathSlider()->maximum() * l / pathLength_);
Joseph Mirabel's avatar
Joseph Mirabel committed
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
}

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
{
275
  return ui_->timeSpinBox;
Joseph Mirabel's avatar
Joseph Mirabel committed
276
277
278
279
}

QSpinBox *PathPlayer::pathIndex() const
{
280
  return ui_->pathIndexSpin;
Joseph Mirabel's avatar
Joseph Mirabel committed
281
282
283
284
}

QSlider *PathPlayer::pathSlider() const
{
285
  return ui_->pathSlider;
Joseph Mirabel's avatar
Joseph Mirabel committed
286
287
288
289
}

QPushButton *PathPlayer::playPause() const
{
290
  return ui_->playPauseButton;
Joseph Mirabel's avatar
Joseph Mirabel committed
291
292
293
294
}

QPushButton *PathPlayer::stop() const
{
295
  return ui_->stopButton;
Joseph Mirabel's avatar
Joseph Mirabel committed
296
297
298
299
}

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