Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) CNRS |
3 |
|
|
// Authors: Joseph Mirabel and Heidy Dallard |
4 |
|
|
// |
5 |
|
|
|
6 |
|
|
#include "hppwidgetsplugin/solverwidget.hh" |
7 |
|
|
|
8 |
|
|
#include <QComboBox> |
9 |
|
|
#include <QDialogButtonBox> |
10 |
|
|
#include <QDoubleSpinBox> |
11 |
|
|
#include <QFileDialog> |
12 |
|
|
#include <QListWidget> |
13 |
|
|
#include <QMessageBox> |
14 |
|
|
|
15 |
|
|
#include "hppwidgetsplugin/ui_solverwidget.h" |
16 |
|
|
#if (QT_VERSION < QT_VERSION_CHECK(5, 0, 0)) |
17 |
|
|
#include <QtCore> |
18 |
|
|
#else |
19 |
|
|
#include <QtConcurrent> |
20 |
|
|
#endif |
21 |
|
|
|
22 |
|
|
#include <hpp/corbaserver/client.hh> |
23 |
|
|
|
24 |
|
|
#include "gepetto/gui/mainwindow.hh" |
25 |
|
|
#include "gepetto/gui/windows-manager.hh" |
26 |
|
|
#include "hppwidgetsplugin/pathplayer.hh" |
27 |
|
|
#include "hppwidgetsplugin/roadmap.hh" |
28 |
|
|
|
29 |
|
|
namespace hpp { |
30 |
|
|
namespace gui { |
31 |
|
|
namespace { |
32 |
|
✗ |
void clearQComboBox(QComboBox* c) { |
33 |
|
✗ |
while (c->count() > 0) c->removeItem(0); |
34 |
|
|
} |
35 |
|
|
} // namespace |
36 |
|
|
|
37 |
|
|
using gepetto::gui::MainWindow; |
38 |
|
|
|
39 |
|
✗ |
SolverWidget::SolverWidget(HppWidgetsPlugin* plugin, QWidget* parent) |
40 |
|
|
: QWidget(parent), |
41 |
|
✗ |
ui_(new ::Ui::SolverWidget), |
42 |
|
✗ |
plugin_(plugin), |
43 |
|
✗ |
main_(MainWindow::instance()), |
44 |
|
✗ |
solve_(plugin, this) { |
45 |
|
✗ |
ui_->setupUi(this); |
46 |
|
✗ |
selectButtonSolve(true); |
47 |
|
|
|
48 |
|
✗ |
connect(planner(), SIGNAL(activated(const QString&)), this, |
49 |
|
|
SLOT(selectPathPlanner(const QString&))); |
50 |
|
✗ |
connect(ui_->pathOptimizerButton, SIGNAL(clicked()), this, |
51 |
|
|
SLOT(openPathOptimizerSelector())); |
52 |
|
✗ |
connect(projector(), SIGNAL(activated(const QString&)), this, |
53 |
|
|
SLOT(selectPathProjector(const QString&))); |
54 |
|
✗ |
connect(validation(), SIGNAL(activated(const QString&)), this, |
55 |
|
|
SLOT(selectPathValidation(const QString&))); |
56 |
|
✗ |
connect(steeringMethod(), SIGNAL(activated(const QString&)), this, |
57 |
|
|
SLOT(selectSteeringMethod(const QString&))); |
58 |
|
✗ |
connect(ui_->pushButtonSolve, SIGNAL(clicked()), this, SLOT(solve())); |
59 |
|
✗ |
connect(ui_->pushButtonInterrupt, SIGNAL(clicked()), this, SLOT(interrupt())); |
60 |
|
✗ |
connect(ui_->pushButtonSolveAndDisplay, SIGNAL(clicked()), |
61 |
|
|
SLOT(solveAndDisplay())); |
62 |
|
✗ |
connect(&solve_.watcher, SIGNAL(finished()), SLOT(solveDone())); |
63 |
|
✗ |
connect(ui_->loadRoadmap, SIGNAL(clicked()), SLOT(loadRoadmap())); |
64 |
|
✗ |
connect(ui_->saveRoadmap, SIGNAL(clicked()), SLOT(saveRoadmap())); |
65 |
|
✗ |
connect(ui_->clearRoadmap, SIGNAL(clicked()), SLOT(clearRoadmap())); |
66 |
|
✗ |
connect(ui_->optimizeButton, SIGNAL(clicked()), SLOT(optimizePath())); |
67 |
|
|
|
68 |
|
|
// Settings of the DoubleSpinBox for discontinuity |
69 |
|
✗ |
ui_->pathProjectorDiscontinuity->setMinimum(0); |
70 |
|
✗ |
ui_->pathProjectorDiscontinuity->setValue(0.2); |
71 |
|
✗ |
ui_->pathProjectorDiscontinuity->setSingleStep(0.1); |
72 |
|
✗ |
connect(ui_->pathProjectorDiscontinuity, SIGNAL(valueChanged(double)), this, |
73 |
|
|
SLOT(discontinuityChanged(double))); |
74 |
|
|
|
75 |
|
|
// Settings of the DoubleSpinBox for penetration |
76 |
|
✗ |
ui_->pathValidationPenetration->setMinimum(0); |
77 |
|
✗ |
ui_->pathValidationPenetration->setValue(0.05); |
78 |
|
✗ |
ui_->pathValidationPenetration->setSingleStep(0.01); |
79 |
|
✗ |
connect(ui_->pathValidationPenetration, SIGNAL(valueChanged(double)), this, |
80 |
|
|
SLOT(penetrationChanged(double))); |
81 |
|
|
} |
82 |
|
|
|
83 |
|
✗ |
SolverWidget::~SolverWidget() { delete ui_; } |
84 |
|
|
|
85 |
|
✗ |
void SolverWidget::setSelected(QComboBox* cb, const QString& what) { |
86 |
|
|
hpp::Names_t_var names = |
87 |
|
✗ |
plugin_->client()->problem()->getSelected(what.toStdString().c_str()); |
88 |
|
✗ |
cb->setCurrentIndex(cb->findText(names[0].in())); |
89 |
|
|
} |
90 |
|
|
|
91 |
|
✗ |
void SolverWidget::update(Select s) { |
92 |
|
✗ |
hpp::Names_t_var names; |
93 |
|
✗ |
switch (s) { |
94 |
|
✗ |
case All: |
95 |
|
|
case Planner: |
96 |
|
✗ |
clearQComboBox(planner()); |
97 |
|
✗ |
names = plugin_->client()->problem()->getAvailable("PathPlanner"); |
98 |
|
✗ |
for (CORBA::ULong i = 0; i < names->length(); ++i) |
99 |
|
✗ |
planner()->addItem(QString::fromLocal8Bit(names[i])); |
100 |
|
✗ |
setSelected(planner(), "PathPlanner"); |
101 |
|
✗ |
if (s == Planner) break; |
102 |
|
|
// fall through |
103 |
|
|
case Optimizer: |
104 |
|
✗ |
names = plugin_->client()->problem()->getAvailable("PathOptimizer"); |
105 |
|
✗ |
optimizers_.clear(); |
106 |
|
✗ |
for (CORBA::ULong i = 0; i < names->length(); ++i) |
107 |
|
✗ |
optimizers_ << QString::fromLocal8Bit(names[i]); |
108 |
|
✗ |
if (s == Optimizer) break; |
109 |
|
|
// fall through |
110 |
|
|
case Validation: |
111 |
|
✗ |
clearQComboBox(validation()); |
112 |
|
✗ |
names = plugin_->client()->problem()->getAvailable("PathValidation"); |
113 |
|
✗ |
for (CORBA::ULong i = 0; i < names->length(); ++i) |
114 |
|
✗ |
validation()->addItem(QString::fromLocal8Bit(names[i]), QVariant(0.2)); |
115 |
|
✗ |
setSelected(validation(), "PathValidation"); |
116 |
|
✗ |
if (s == Validation) break; |
117 |
|
|
// fall through |
118 |
|
|
case Projector: |
119 |
|
✗ |
clearQComboBox(projector()); |
120 |
|
✗ |
names = plugin_->client()->problem()->getAvailable("PathProjector"); |
121 |
|
✗ |
for (CORBA::ULong i = 0; i < names->length(); ++i) |
122 |
|
✗ |
projector()->addItem(QString::fromLocal8Bit(names[i]), QVariant(0.2)); |
123 |
|
✗ |
setSelected(projector(), "PathProjector"); |
124 |
|
✗ |
if (s == Projector) break; |
125 |
|
|
// fall through |
126 |
|
|
case SteeringMethod: |
127 |
|
✗ |
clearQComboBox(steeringMethod()); |
128 |
|
✗ |
names = plugin_->client()->problem()->getAvailable("SteeringMethod"); |
129 |
|
✗ |
for (CORBA::ULong i = 0; i < names->length(); ++i) |
130 |
|
✗ |
steeringMethod()->addItem(QString::fromLocal8Bit(names[i])); |
131 |
|
✗ |
setSelected(steeringMethod(), "SteeringMethod"); |
132 |
|
✗ |
if (s == SteeringMethod) break; |
133 |
|
|
// fall through |
134 |
|
|
} |
135 |
|
|
} |
136 |
|
|
|
137 |
|
✗ |
void SolverWidget::selectPathPlanner(const QString& text) { |
138 |
|
✗ |
plugin_->client()->problem()->selectPathPlanner(text.toStdString().c_str()); |
139 |
|
|
} |
140 |
|
|
|
141 |
|
✗ |
void SolverWidget::selectSteeringMethod(const QString& text) { |
142 |
|
✗ |
plugin_->client()->problem()->selectSteeringMethod( |
143 |
|
✗ |
text.toStdString().c_str()); |
144 |
|
|
} |
145 |
|
|
|
146 |
|
✗ |
void SolverWidget::selectPathOptimizers(const QStringList& list) { |
147 |
|
✗ |
plugin_->client()->problem()->clearPathOptimizers(); |
148 |
|
✗ |
foreach (QString s, list) |
149 |
|
✗ |
plugin_->client()->problem()->addPathOptimizer(s.toStdString().c_str()); |
150 |
|
|
} |
151 |
|
|
|
152 |
|
✗ |
void SolverWidget::selectPathProjector(const QString& name) { |
153 |
|
✗ |
plugin_->client()->problem()->selectPathProjector( |
154 |
|
✗ |
name.toStdString().c_str(), projectorDiscontinuity()->value()); |
155 |
|
|
} |
156 |
|
|
|
157 |
|
✗ |
void SolverWidget::selectPathValidation(const QString& name) { |
158 |
|
✗ |
plugin_->client()->problem()->selectPathValidation( |
159 |
|
✗ |
name.toStdString().c_str(), validationPenetration()->value()); |
160 |
|
|
} |
161 |
|
|
|
162 |
|
✗ |
void SolverWidget::discontinuityChanged(double value) { |
163 |
|
|
Q_UNUSED(value); |
164 |
|
✗ |
selectPathProjector(projector()->currentText()); |
165 |
|
|
} |
166 |
|
|
|
167 |
|
✗ |
void SolverWidget::penetrationChanged(double value) { |
168 |
|
|
Q_UNUSED(value); |
169 |
|
✗ |
selectPathValidation(validation()->currentText()); |
170 |
|
|
} |
171 |
|
|
|
172 |
|
✗ |
void SolverWidget::openPathOptimizerSelector() { |
173 |
|
✗ |
QDialog dialog(this); |
174 |
|
|
// Use a layout allowing to have a label next to each field |
175 |
|
✗ |
QVBoxLayout lay(&dialog); |
176 |
|
|
|
177 |
|
|
// Add some text above the fields |
178 |
|
✗ |
lay.addWidget(new QLabel("Select path optimizers:")); |
179 |
|
|
|
180 |
|
|
// Add the lineEdits with their respective labels |
181 |
|
✗ |
QListWidget* list = new QListWidget(&dialog); |
182 |
|
✗ |
list->addItems(optimizers_); |
183 |
|
✗ |
list->setSelectionMode(QAbstractItemView::ExtendedSelection); |
184 |
|
✗ |
lay.addWidget(list); |
185 |
|
|
|
186 |
|
|
hpp::Names_t_var names = |
187 |
|
✗ |
plugin_->client()->problem()->getSelected("PathOptimizer"); |
188 |
|
✗ |
for (unsigned i = 0; i < names->length(); ++i) { |
189 |
|
✗ |
int index = optimizers_.indexOf(QRegExp(names[i].in())); |
190 |
|
|
|
191 |
|
✗ |
list->item(index)->setSelected(true); |
192 |
|
|
} |
193 |
|
|
|
194 |
|
|
QDialogButtonBox buttonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, |
195 |
|
✗ |
Qt::Horizontal, &dialog); |
196 |
|
✗ |
lay.addWidget(&buttonBox); |
197 |
|
✗ |
QObject::connect(&buttonBox, SIGNAL(accepted()), &dialog, SLOT(accept())); |
198 |
|
✗ |
QObject::connect(&buttonBox, SIGNAL(rejected()), &dialog, SLOT(reject())); |
199 |
|
|
|
200 |
|
|
// Show the dialog as modal |
201 |
|
✗ |
if (dialog.exec() == QDialog::Accepted) { |
202 |
|
|
// If the user didn't dismiss the dialog, do something with the fields |
203 |
|
✗ |
QStringList selected; |
204 |
|
✗ |
foreach (QListWidgetItem* item, list->selectedItems()) |
205 |
|
✗ |
selected << item->text(); |
206 |
|
✗ |
qDebug() << "Selected path optimizers: " << selected; |
207 |
|
✗ |
selectPathOptimizers(selected); |
208 |
|
|
} |
209 |
|
|
} |
210 |
|
|
|
211 |
|
✗ |
void SolverWidget::solve() { |
212 |
|
✗ |
if (solve_.status.isRunning()) { |
213 |
|
✗ |
main_->logError("The solver is already running."); |
214 |
|
✗ |
return; |
215 |
|
|
} |
216 |
|
✗ |
solve_.stepByStep = false; |
217 |
|
✗ |
solve_.status = QtConcurrent::run(&solve_, &Solve::solve); |
218 |
|
✗ |
solve_.watcher.setFuture(solve_.status); |
219 |
|
✗ |
main_->logJobStarted(0, "solve problem."); |
220 |
|
✗ |
selectButtonSolve(false); |
221 |
|
|
} |
222 |
|
|
|
223 |
|
✗ |
void SolverWidget::solveAndDisplay() { |
224 |
|
✗ |
if (solve_.status.isRunning()) { |
225 |
|
✗ |
main_->logError("The solver is already running."); |
226 |
|
✗ |
return; |
227 |
|
|
} |
228 |
|
✗ |
solve_.stepByStep = true; |
229 |
|
✗ |
solve_.interrupt = false; |
230 |
|
✗ |
solve_.status = QtConcurrent::run(&solve_, &Solve::solveAndDisplay); |
231 |
|
✗ |
solve_.watcher.setFuture(solve_.status); |
232 |
|
✗ |
selectButtonSolve(false); |
233 |
|
|
} |
234 |
|
|
|
235 |
|
✗ |
void SolverWidget::solveDone() { |
236 |
|
✗ |
if (solve_.done()) emit problemSolved(); |
237 |
|
|
} |
238 |
|
|
|
239 |
|
✗ |
bool SolverWidget::Solve::done() { |
240 |
|
✗ |
qDebug() << "Solve done"; |
241 |
|
✗ |
parent->selectButtonSolve(true); |
242 |
|
✗ |
if (isSolved) { |
243 |
|
✗ |
QMessageBox::information(parent, "Problem solver", "Problem is solved."); |
244 |
|
✗ |
parent->main_->logJobDone(0, "Problem solved."); |
245 |
|
✗ |
return true; |
246 |
|
|
} |
247 |
|
✗ |
return false; |
248 |
|
|
} |
249 |
|
|
|
250 |
|
✗ |
void SolverWidget::interrupt() { |
251 |
|
✗ |
if (solve_.stepByStep) { |
252 |
|
✗ |
solve_.interrupt = true; |
253 |
|
|
} else { |
254 |
|
✗ |
plugin_->client()->problem()->interruptPathPlanning(); |
255 |
|
|
} |
256 |
|
✗ |
solve_.status.waitForFinished(); |
257 |
|
✗ |
selectButtonSolve(true); |
258 |
|
|
} |
259 |
|
|
|
260 |
|
✗ |
void SolverWidget::loadRoadmap() { |
261 |
|
|
QString file = |
262 |
|
✗ |
QFileDialog::getOpenFileName(this, tr("Select a roadmap file")); |
263 |
|
✗ |
if (file.isNull()) return; |
264 |
|
|
try { |
265 |
|
✗ |
plugin_->client()->problem()->loadRoadmap(file.toLocal8Bit().data()); |
266 |
|
✗ |
} catch (const hpp::Error& e) { |
267 |
|
✗ |
MainWindow::instance()->logError(QString::fromLocal8Bit(e.msg)); |
268 |
|
|
} |
269 |
|
|
} |
270 |
|
|
|
271 |
|
✗ |
void SolverWidget::saveRoadmap() { |
272 |
|
✗ |
QString file = QFileDialog::getSaveFileName(this, tr("Select a destination")); |
273 |
|
✗ |
if (file.isNull()) return; |
274 |
|
|
try { |
275 |
|
✗ |
plugin_->client()->problem()->saveRoadmap(file.toLocal8Bit().data()); |
276 |
|
✗ |
} catch (const hpp::Error& e) { |
277 |
|
✗ |
MainWindow::instance()->logError(QString::fromLocal8Bit(e.msg)); |
278 |
|
|
} |
279 |
|
|
} |
280 |
|
|
|
281 |
|
✗ |
void SolverWidget::clearRoadmap() { |
282 |
|
|
try { |
283 |
|
✗ |
plugin_->client()->problem()->resetRoadmap(); |
284 |
|
✗ |
} catch (const hpp::Error& e) { |
285 |
|
✗ |
MainWindow::instance()->logError(QString::fromLocal8Bit(e.msg)); |
286 |
|
|
} |
287 |
|
|
} |
288 |
|
|
|
289 |
|
✗ |
void SolverWidget::optimizePath() { |
290 |
|
✗ |
if (solve_.status.isRunning()) { |
291 |
|
✗ |
main_->logError("The solver is already running."); |
292 |
|
✗ |
return; |
293 |
|
|
} |
294 |
|
✗ |
solve_.stepByStep = false; |
295 |
|
✗ |
solve_.status = QtConcurrent::run(&solve_, &Solve::optimize, |
296 |
|
✗ |
plugin_->pathPlayer()->getCurrentPath()); |
297 |
|
✗ |
solve_.watcher.setFuture(solve_.status); |
298 |
|
✗ |
main_->logJobStarted(0, "Optimize path."); |
299 |
|
✗ |
selectButtonSolve(false); |
300 |
|
|
} |
301 |
|
|
|
302 |
|
✗ |
void SolverWidget::Solve::solve() { |
303 |
|
✗ |
isSolved = false; |
304 |
|
✗ |
HppWidgetsPlugin::HppClient* hpp = plugin->client(); |
305 |
|
|
try { |
306 |
|
✗ |
hpp::intSeq_var time = hpp->problem()->solve(); |
307 |
|
✗ |
isSolved = true; |
308 |
|
✗ |
} catch (hpp::Error const& e) { |
309 |
|
✗ |
parent->main_->logJobFailed(0, QString(e.msg)); |
310 |
|
|
} |
311 |
|
|
} |
312 |
|
|
|
313 |
|
✗ |
void SolverWidget::Solve::optimize(const int pathId) { |
314 |
|
✗ |
isSolved = false; |
315 |
|
✗ |
HppWidgetsPlugin::HppClient* hpp = plugin->client(); |
316 |
|
|
try { |
317 |
|
✗ |
hpp->problem()->optimizePath((CORBA::UShort)pathId); |
318 |
|
✗ |
isSolved = true; |
319 |
|
✗ |
} catch (hpp::Error const& e) { |
320 |
|
✗ |
parent->main_->logJobFailed(0, QString(e.msg)); |
321 |
|
|
} |
322 |
|
|
} |
323 |
|
|
|
324 |
|
✗ |
void SolverWidget::Solve::solveAndDisplay() { |
325 |
|
✗ |
isSolved = false; |
326 |
|
✗ |
HppWidgetsPlugin::HppClient* hpp = plugin->client(); |
327 |
|
✗ |
std::string jn = plugin->getSelectedJoint(); |
328 |
|
✗ |
if (jn.empty()) { |
329 |
|
✗ |
QMessageBox::information( |
330 |
|
✗ |
parent, "Problem solver", |
331 |
|
|
"Please, select a joint in the joint tree window."); |
332 |
|
✗ |
return; |
333 |
|
|
} |
334 |
|
✗ |
isSolved = hpp->problem()->prepareSolveStepByStep(); |
335 |
|
✗ |
Roadmap* r = plugin->createRoadmap(jn); |
336 |
|
✗ |
while (!isSolved) { |
337 |
|
✗ |
isSolved = hpp->problem()->executeOneStep(); |
338 |
|
✗ |
r->displayRoadmap(); |
339 |
|
✗ |
if (interrupt) break; |
340 |
|
|
} |
341 |
|
✗ |
if (isSolved) hpp->problem()->finishSolveStepByStep(); |
342 |
|
✗ |
delete r; |
343 |
|
|
} |
344 |
|
|
|
345 |
|
✗ |
void SolverWidget::selectButtonSolve(bool solve) { |
346 |
|
✗ |
if (solve) { |
347 |
|
✗ |
ui_->pushButtonInterrupt->setVisible(false); |
348 |
|
✗ |
ui_->pushButtonSolve->setVisible(true); |
349 |
|
✗ |
ui_->pushButtonSolveAndDisplay->setVisible(true); |
350 |
|
✗ |
ui_->optimizeButton->setVisible(true); |
351 |
|
|
} else { |
352 |
|
✗ |
ui_->pushButtonInterrupt->setVisible(true); |
353 |
|
✗ |
ui_->pushButtonSolve->setVisible(false); |
354 |
|
✗ |
ui_->pushButtonSolveAndDisplay->setVisible(false); |
355 |
|
✗ |
ui_->optimizeButton->setVisible(false); |
356 |
|
|
} |
357 |
|
|
} |
358 |
|
|
|
359 |
|
✗ |
QComboBox* SolverWidget::planner() { return ui_->pathPlannerComboBox; } |
360 |
|
|
|
361 |
|
✗ |
QComboBox* SolverWidget::projector() { return ui_->pathProjectorComboBox; } |
362 |
|
|
|
363 |
|
✗ |
QComboBox* SolverWidget::validation() { return ui_->pathValidationComboBox; } |
364 |
|
|
|
365 |
|
✗ |
QComboBox* SolverWidget::steeringMethod() { |
366 |
|
✗ |
return ui_->steeringMethodComboBox; |
367 |
|
|
} |
368 |
|
|
|
369 |
|
✗ |
QDoubleSpinBox* SolverWidget::projectorDiscontinuity() { |
370 |
|
✗ |
return ui_->pathProjectorDiscontinuity; |
371 |
|
|
} |
372 |
|
|
|
373 |
|
✗ |
QDoubleSpinBox* SolverWidget::validationPenetration() { |
374 |
|
✗ |
return ui_->pathValidationPenetration; |
375 |
|
|
} |
376 |
|
|
} // namespace gui |
377 |
|
|
} // namespace hpp |
378 |
|
|
|