solverwidget.cc 13.1 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
#include "hppwidgetsplugin/solverwidget.hh"
2
#include "hppwidgetsplugin/ui_solverwidget.h"
Joseph Mirabel's avatar
Joseph Mirabel committed
3

4
5
6
7
8
9
#include <QComboBox>
#include <QDialogButtonBox>
#include <QDoubleSpinBox>
#include <QFileDialog>
#include <QListWidget>
#include <QMessageBox>
Joseph Mirabel's avatar
Joseph Mirabel committed
10
11
12
13
14
#if (QT_VERSION < QT_VERSION_CHECK(5,0,0))
# include <QtCore>
#else
# include <QtConcurrent>
#endif
15

Joseph Mirabel's avatar
Joseph Mirabel committed
16
17
#include <hpp/corbaserver/client.hh>

18
19
#include "gepetto/gui/mainwindow.hh"
#include "gepetto/gui/windows-manager.hh"
Joseph Mirabel's avatar
Joseph Mirabel committed
20

21
#include "hppwidgetsplugin/roadmap.hh"
22
#include "hppwidgetsplugin/pathplayer.hh"
23

Joseph Mirabel's avatar
Joseph Mirabel committed
24
25
26
27
28
29
30
31
namespace hpp {
  namespace gui {
    namespace {
      void clearQComboBox (QComboBox* c) {
        while (c->count() > 0) c->removeItem(0);
      }
    }

32
33
    using gepetto::gui::MainWindow;

Joseph Mirabel's avatar
Joseph Mirabel committed
34
35
36
37
    SolverWidget::SolverWidget (HppWidgetsPlugin *plugin, QWidget *parent) :
      QWidget (parent),
      ui_ (new ::Ui::SolverWidget),
      plugin_ (plugin),
38
      main_(MainWindow::instance()),
39
      solve_ (plugin, this)
Joseph Mirabel's avatar
Joseph Mirabel committed
40
41
42
43
    {
      ui_->setupUi (this);
      selectButtonSolve(true);

Joseph Mirabel's avatar
Joseph Mirabel committed
44
45
      connect(planner(), SIGNAL (activated(const QString&)), this, SLOT (selectPathPlanner(const QString&)));
      connect(ui_->pathOptimizerButton, SIGNAL (clicked()), this, SLOT (openPathOptimizerSelector()));
hdallard's avatar
hdallard committed
46
47
      connect(projector(), SIGNAL (activated(const QString&)), this, SLOT (selectPathProjector(const QString&)));
      connect(validation(), SIGNAL (activated(const QString&)), this, SLOT (selectPathValidation(const QString&)));
48
      connect(steeringMethod(), SIGNAL(activated(const QString&)), this, SLOT(selectSteeringMethod(const QString&)));
Joseph Mirabel's avatar
Joseph Mirabel committed
49
50
51
      connect(ui_->pushButtonSolve, SIGNAL (clicked ()), this, SLOT (solve ()));
      connect(ui_->pushButtonInterrupt, SIGNAL (clicked ()), this, SLOT (interrupt ()));
      connect(ui_->pushButtonSolveAndDisplay, SIGNAL (clicked ()),
52
          SLOT (solveAndDisplay ()));
53
      connect(&solve_.watcher, SIGNAL (finished()), SLOT(solveDone ()));
Joseph Mirabel's avatar
Joseph Mirabel committed
54
55
      connect(ui_->loadRoadmap, SIGNAL (clicked()), SLOT (loadRoadmap()));
      connect(ui_->saveRoadmap, SIGNAL (clicked()), SLOT (saveRoadmap()));
56
      connect(ui_->clearRoadmap, SIGNAL (clicked()), SLOT (clearRoadmap()));
57
      connect(ui_->optimizeButton, SIGNAL(clicked()), SLOT(optimizePath()));
58
59
60
61
62
63
64
65
66
67
68
69
70
71

      // Settings of the DoubleSpinBox for discontinuity
      ui_->pathProjectorDiscontinuity->setMinimum(0);
      ui_->pathProjectorDiscontinuity->setValue(0.2);
      ui_->pathProjectorDiscontinuity->setSingleStep(0.1);
      connect(ui_->pathProjectorDiscontinuity, SIGNAL(valueChanged(double)),
	      this, SLOT(discontinuityChanged(double)));

      // Settings of the DoubleSpinBox for penetration
      ui_->pathValidationPenetration->setMinimum(0);
      ui_->pathValidationPenetration->setValue(0.05);
      ui_->pathValidationPenetration->setSingleStep(0.01);
      connect(ui_->pathValidationPenetration, SIGNAL(valueChanged(double)),
	      this, SLOT(penetrationChanged(double)));
Joseph Mirabel's avatar
Joseph Mirabel committed
72
73
74
75
76
77
78
    }

    SolverWidget::~SolverWidget()
    {
      delete ui_;
    }

79
80
81
82
83
84
    void SolverWidget::setSelected(QComboBox *cb, const QString &what)
    {
      hpp::Names_t_var names = plugin_->client()->problem()->getSelected(what.toStdString().c_str());
      cb->setCurrentIndex(cb->findText(names[0].in()));
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
85
    void SolverWidget::update (Select s) {
Joseph Mirabel's avatar
Joseph Mirabel committed
86
      hpp::Names_t_var names;
Joseph Mirabel's avatar
Joseph Mirabel committed
87
88
89
90
      switch (s) {
        case All:
        case Planner:
          clearQComboBox(planner());
Joseph Mirabel's avatar
Joseph Mirabel committed
91
92
93
          names = plugin_->client()->problem()->getAvailable("PathPlanner");
          for (CORBA::ULong i = 0; i < names->length(); ++i)
            planner()->addItem(QString::fromLocal8Bit(names[i]));
94
          setSelected(planner(), "PathPlanner");
Joseph Mirabel's avatar
Joseph Mirabel committed
95
96
          if (s == Planner) break;
        case Optimizer:
Joseph Mirabel's avatar
Joseph Mirabel committed
97
          names = plugin_->client()->problem()->getAvailable("PathOptimizer");
Joseph Mirabel's avatar
Joseph Mirabel committed
98
          optimizers_.clear();
Joseph Mirabel's avatar
Joseph Mirabel committed
99
          for (CORBA::ULong i = 0; i < names->length(); ++i)
Joseph Mirabel's avatar
Joseph Mirabel committed
100
            optimizers_ << QString::fromLocal8Bit(names[i]);
Joseph Mirabel's avatar
Joseph Mirabel committed
101
          if (s == Optimizer) break;
102
103
104
105
106
        case Validation:
	  clearQComboBox(validation());
          names = plugin_->client()->problem()->getAvailable("PathValidation");
          for (CORBA::ULong i = 0; i < names->length(); ++i)
            validation()->addItem(QString::fromLocal8Bit(names[i]), QVariant (0.2));
107
          setSelected(validation(), "PathValidation");
108
          if (s == Validation) break;
Joseph Mirabel's avatar
Joseph Mirabel committed
109
110
        case Projector:
          clearQComboBox(projector());
Joseph Mirabel's avatar
Joseph Mirabel committed
111
112
113
          names = plugin_->client()->problem()->getAvailable("PathProjector");
          for (CORBA::ULong i = 0; i < names->length(); ++i)
            projector()->addItem(QString::fromLocal8Bit(names[i]), QVariant (0.2));
114
          setSelected(projector(), "PathProjector");
Joseph Mirabel's avatar
Joseph Mirabel committed
115
          if (s == Projector) break;
116
117
118
119
120
        case SteeringMethod:
          clearQComboBox(steeringMethod());
          names = plugin_->client()->problem()->getAvailable("SteeringMethod");
          for (CORBA::ULong i = 0; i < names->length(); ++i)
            steeringMethod()->addItem(QString::fromLocal8Bit(names[i]));
121
          setSelected(steeringMethod(), "SteeringMethod");
122
          if (s == SteeringMethod) break;
Joseph Mirabel's avatar
Joseph Mirabel committed
123
124
125
126
127
128
129
      }
    }

    void SolverWidget::selectPathPlanner (const QString& text) {
      plugin_->client()->problem()->selectPathPlanner (text.toStdString().c_str());
    }

130
131
132
133
    void SolverWidget::selectSteeringMethod (const QString& text) {
      plugin_->client()->problem()->selectSteeringMethod (text.toStdString().c_str());
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
134
    void SolverWidget::selectPathOptimizers (const QStringList& list) {
Joseph Mirabel's avatar
Joseph Mirabel committed
135
      plugin_->client()->problem()->clearPathOptimizers();
Joseph Mirabel's avatar
Joseph Mirabel committed
136
137
      foreach(QString s, list)
        plugin_->client()->problem()->addPathOptimizer (s.toStdString().c_str());
Joseph Mirabel's avatar
Joseph Mirabel committed
138
139
    }

hdallard's avatar
hdallard committed
140
    void SolverWidget::selectPathProjector (const QString& name) {
Joseph Mirabel's avatar
Joseph Mirabel committed
141
      plugin_->client()->problem()->selectPathProjector (
hdallard's avatar
hdallard committed
142
          name.toStdString().c_str(),
143
	  projectorDiscontinuity()->value());
Joseph Mirabel's avatar
Joseph Mirabel committed
144
145
    }

hdallard's avatar
hdallard committed
146
    void SolverWidget::selectPathValidation (const QString& name) {
147
      plugin_->client()->problem()->selectPathValidation (
hdallard's avatar
hdallard committed
148
      name.toStdString().c_str(),
149
150
151
152
153
154
          validationPenetration()->value());
    }

    void SolverWidget::discontinuityChanged(double value)
    {
      Q_UNUSED(value);
hdallard's avatar
hdallard committed
155
      selectPathProjector(projector()->currentText());
156
157
158
159
160
    }

    void SolverWidget::penetrationChanged(double value)
    {
      Q_UNUSED(value);
hdallard's avatar
hdallard committed
161
      selectPathValidation(validation()->currentText());
162
163
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
    void SolverWidget::openPathOptimizerSelector ()
    {
      QDialog dialog(this);
      // Use a layout allowing to have a label next to each field
      QVBoxLayout lay(&dialog);

      // Add some text above the fields
      lay.addWidget(new QLabel("Select path optimizers:"));

      // Add the lineEdits with their respective labels
      QListWidget* list = new QListWidget (&dialog);
      list->addItems(optimizers_);
      list->setSelectionMode(QAbstractItemView::ExtendedSelection);
      lay.addWidget(list);

179
180
181
182
183
184
185
      hpp::Names_t_var names = plugin_->client()->problem()->getSelected("PathOptimizer");
      for (unsigned i = 0; i < names->length(); ++i) {
        int index = optimizers_.indexOf(QRegExp(names[i].in()));

        list->item(index)->setSelected(true);
      }

Joseph Mirabel's avatar
Joseph Mirabel committed
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
      QDialogButtonBox buttonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel,
          Qt::Horizontal, &dialog);
      lay.addWidget(&buttonBox);
      QObject::connect(&buttonBox, SIGNAL(accepted()), &dialog, SLOT(accept()));
      QObject::connect(&buttonBox, SIGNAL(rejected()), &dialog, SLOT(reject()));

      // Show the dialog as modal
      if (dialog.exec() == QDialog::Accepted) {
          // If the user didn't dismiss the dialog, do something with the fields
          QStringList selected;
          foreach (QListWidgetItem* item, list->selectedItems())
              selected << item->text();
          qDebug () << "Selected path optimizers: " << selected;
          selectPathOptimizers(selected);
        }
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
203
204
    void SolverWidget::solve()
    {
205
206
207
208
209
210
211
212
      if (solve_.status.isRunning ()) {
        main_->logError("The solver is already running.");
        return;
      }
      solve_.stepByStep = false;
      solve_.status = QtConcurrent::run(&solve_, &Solve::solve);
      solve_.watcher.setFuture (solve_.status);
      main_->logJobStarted(0, "solve problem.");
Joseph Mirabel's avatar
Joseph Mirabel committed
213
214
215
216
217
      selectButtonSolve(false);
    }

    void SolverWidget::solveAndDisplay()
    {
218
219
220
221
222
223
224
225
      if (solve_.status.isRunning ()) {
        main_->logError("The solver is already running.");
        return;
      }
      solve_.stepByStep = true;
      solve_.interrupt = false;
      solve_.status = QtConcurrent::run (&solve_, &Solve::solveAndDisplay);
      solve_.watcher.setFuture (solve_.status);
Joseph Mirabel's avatar
Joseph Mirabel committed
226
227
228
      selectButtonSolve (false);
    }

229
    void SolverWidget::solveDone()
Joseph Mirabel's avatar
Joseph Mirabel committed
230
    {
231
      if (solve_.done())
Joseph Mirabel's avatar
Joseph Mirabel committed
232
        emit problemSolved ();
233
234
235
236
237
238
239
240
241
242
    }

    bool SolverWidget::Solve::done()
    {
      qDebug () << "Solve done";
      parent->selectButtonSolve (true);
      if (isSolved) {
        QMessageBox::information(parent, "Problem solver", "Problem is solved.");
        parent->main_->logJobDone(0, "Problem solved.");
        return true;
Joseph Mirabel's avatar
Joseph Mirabel committed
243
      }
244
      return false;
Joseph Mirabel's avatar
Joseph Mirabel committed
245
246
247
248
    }

    void SolverWidget::interrupt()
    {
249
250
      if (solve_.stepByStep) {
        solve_.interrupt = true;
Joseph Mirabel's avatar
Joseph Mirabel committed
251
252
253
      } else {
        plugin_->client()->problem()->interruptPathPlanning();
      }
254
      solve_.status.waitForFinished ();
255
      selectButtonSolve(true);
Joseph Mirabel's avatar
Joseph Mirabel committed
256
257
258
259
260
261
262
263
264
    }

    void SolverWidget::loadRoadmap()
    {
      QString file = QFileDialog::getOpenFileName(this, tr ("Select a roadmap file"));
      if (file.isNull()) return;
      try {
        plugin_->client()->problem()->readRoadmap (file.toLocal8Bit().data());
      } catch (const hpp::Error& e) {
265
        MainWindow::instance()->logError(QString::fromLocal8Bit(e.msg));
Joseph Mirabel's avatar
Joseph Mirabel committed
266
267
268
269
270
271
272
273
274
275
      }
    }

    void SolverWidget::saveRoadmap()
    {
      QString file = QFileDialog::getSaveFileName(this, tr("Select a destination"));
      if (file.isNull()) return;
      try {
        plugin_->client()->problem()->saveRoadmap (file.toLocal8Bit().data());
      } catch (const hpp::Error& e) {
276
        MainWindow::instance()->logError(QString::fromLocal8Bit(e.msg));
Joseph Mirabel's avatar
Joseph Mirabel committed
277
278
279
      }
    }

280
281
282
283
284
285
286
287
288
    void SolverWidget::clearRoadmap()
    {
      try {
        plugin_->client()->problem()->resetRoadmap ();
      } catch (const hpp::Error& e) {
        MainWindow::instance()->logError(QString::fromLocal8Bit(e.msg));
      }
    }

289
290
    void SolverWidget::optimizePath()
    {
291
292
293
294
295
296
297
298
299
      if (solve_.status.isRunning ()) {
        main_->logError("The solver is already running.");
        return;
      }
      solve_.stepByStep = false;
      solve_.status = QtConcurrent::run(&solve_,
          &Solve::optimize, plugin_->pathPlayer()->getCurrentPath());
      solve_.watcher.setFuture (solve_.status);
      main_->logJobStarted(0, "Optimize path.");
300
301
302
      selectButtonSolve(false);
    }

303
    void SolverWidget::Solve::solve()
Joseph Mirabel's avatar
Joseph Mirabel committed
304
    {
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
      isSolved = false;
      HppWidgetsPlugin::HppClient* hpp = plugin->client();
      try {
        hpp::intSeq_var time = hpp->problem()->solve();
        isSolved = true;
      } catch (hpp::Error const& e) {
        parent->main_->logJobFailed(0, QString(e.msg));
      }
    }

    void SolverWidget::Solve::optimize(const int pathId)
    {
      isSolved = false;
      HppWidgetsPlugin::HppClient* hpp = plugin->client();
      try {
        hpp->problem()->optimizePath((CORBA::UShort)pathId);
        isSolved = true;
      } catch (hpp::Error const& e) {
        parent->main_->logJobFailed(0, QString(e.msg));
Joseph Mirabel's avatar
Joseph Mirabel committed
324
325
326
      }
    }

327
    void SolverWidget::Solve::solveAndDisplay()
Joseph Mirabel's avatar
Joseph Mirabel committed
328
    {
329
      isSolved = false;
Joseph Mirabel's avatar
Joseph Mirabel committed
330
331
332
      HppWidgetsPlugin::HppClient* hpp = plugin->client();
      std::string jn = plugin->getSelectedJoint();
      if (jn.empty()) {
333
334
	QMessageBox::information(parent, "Problem solver",
				 "Please, select a joint in the joint tree window.");
Joseph Mirabel's avatar
Joseph Mirabel committed
335
336
337
        return;
      }
      isSolved = hpp->problem()->prepareSolveStepByStep();
338
      Roadmap* r = plugin->createRoadmap(jn);
Joseph Mirabel's avatar
Joseph Mirabel committed
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
      while (!isSolved) {
        isSolved = hpp->problem()->executeOneStep();
        r->displayRoadmap();
        if (interrupt) break;
      }
      if (isSolved)
        hpp->problem()->finishSolveStepByStep();
      delete r;
    }

    void SolverWidget::selectButtonSolve(bool solve)
    {
      if (solve) {
        ui_->pushButtonInterrupt->setVisible(false);
        ui_->pushButtonSolve->setVisible(true);
        ui_->pushButtonSolveAndDisplay->setVisible(true);
355
	ui_->optimizeButton->setVisible(true);
Joseph Mirabel's avatar
Joseph Mirabel committed
356
357
358
359
      } else {
        ui_->pushButtonInterrupt->setVisible(true);
        ui_->pushButtonSolve->setVisible(false);
        ui_->pushButtonSolveAndDisplay->setVisible(false);
360
	ui_->optimizeButton->setVisible(false);
Joseph Mirabel's avatar
Joseph Mirabel committed
361
362
363
364
365
366
367
368
369
370
371
372
      }
    }

    QComboBox *SolverWidget::planner()
    {
      return ui_->pathPlannerComboBox;
    }

    QComboBox *SolverWidget::projector()
    {
      return ui_->pathProjectorComboBox;
    }
373
374
375
376
377

    QComboBox *SolverWidget::validation()
    {
      return ui_->pathValidationComboBox;
    }
378

379
380
381
382
383
    QComboBox *SolverWidget::steeringMethod()
    {
      return ui_->steeringMethodComboBox;
    }

384
385
386
387
388
389
390
391
392
    QDoubleSpinBox *SolverWidget::projectorDiscontinuity()
    {
      return ui_->pathProjectorDiscontinuity;
    }

    QDoubleSpinBox *SolverWidget::validationPenetration()
    {
      return ui_->pathValidationPenetration;
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
393
394
  } // namespace gui
} // namespace hpp