pathplayer.cc 7.89 KB
Newer Older
1
#include "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
11
12
#include "ui_pathplayerwidget.h"

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
}

48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
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 ?");
  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();
  hpp::floatSeqSeq_var waypoints = hpp->problem()->getWaypoints(pid);
  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
void PathPlayer::update ()
{
86
  CORBA::Short nbPath = plugin_->client()->problem ()->numberPaths ();
Joseph Mirabel's avatar
Joseph Mirabel committed
87
88
89
90
91
92
93
94
95
96
  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)
97
            pathIndexChanged(0);
Joseph Mirabel's avatar
Joseph Mirabel committed
98
99
100
101
102
103
104
105
106
          pathIndex()->setValue(0);
        }
      pathIndex()->setMaximum(nbPath - 1);
    }
  else {
      pathIndex()->setEnabled(false);
      pathSlider()->setEnabled(false);
      playPause()->setEnabled(false);
      stop()->setEnabled(false);
107
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
108
109
110
111
}

void PathPlayer::pathIndexChanged(int i)
{
Joseph Mirabel's avatar
Joseph Mirabel committed
112
  assert (i >= 0);
113
  pathLength_ = plugin_->client()->problem()->pathLength ((short unsigned int)i);
Joseph Mirabel's avatar
Joseph Mirabel committed
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
  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
142
  MainWindow* main = MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
143
  if (toggled) {
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
172
173
174
175
176
177
178
      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
179
180
181
182
183
184
185
186
187
    }
}

void PathPlayer::pathPulse()
{
  if (playPause()->isChecked()) {
      // Stil playing
      currentParam_ += lengthBetweenRefresh();
      if (currentParam_ > pathLength_) {
Joseph Mirabel's avatar
Joseph Mirabel committed
188
          currentParam_ = pathLength_;
Joseph Mirabel's avatar
Joseph Mirabel committed
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
          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
205
206
207
208
209
void PathPlayer::readyReadProcessOutput()
{
  pOutput_->append(process_->readAll());
}

Joseph Mirabel's avatar
Joseph Mirabel committed
210
211
212
void PathPlayer::updateConfiguration ()
{
  hpp::floatSeq_var config =
213
214
      plugin_->client()->problem()->configAtParam ((short unsigned int)pathIndex()->value(),currentParam_);
  plugin_->client()->robot()->setCurrentConfig (config.in());
215
  MainWindow::instance()->requestApplyCurrentConfiguration();
Joseph Mirabel's avatar
Joseph Mirabel committed
216
217
218
219
220
221
222
223
224
}

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
225
  return (int) (pathSlider()->maximum() * l / pathLength_);
Joseph Mirabel's avatar
Joseph Mirabel committed
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
}

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
{
245
  return ui_->timeSpinBox;
Joseph Mirabel's avatar
Joseph Mirabel committed
246
247
248
249
}

QSpinBox *PathPlayer::pathIndex() const
{
250
  return ui_->pathIndexSpin;
Joseph Mirabel's avatar
Joseph Mirabel committed
251
252
253
254
}

QSlider *PathPlayer::pathSlider() const
{
255
  return ui_->pathSlider;
Joseph Mirabel's avatar
Joseph Mirabel committed
256
257
258
259
}

QPushButton *PathPlayer::playPause() const
{
260
  return ui_->playPauseButton;
Joseph Mirabel's avatar
Joseph Mirabel committed
261
262
263
264
}

QPushButton *PathPlayer::stop() const
{
265
  return ui_->stopButton;
Joseph Mirabel's avatar
Joseph Mirabel committed
266
267
268
269
}

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