GCC Code Coverage Report


Directory: plugins/
File: plugins/hppwidgetsplugin/solverwidget.cc
Date: 2024-12-13 15:51:58
Exec Total Coverage
Lines: 0 217 0.0%
Branches: 0 460 0.0%

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