GCC Code Coverage Report


Directory: ./
File: plugins/hppmonitoringplugin/hppmonitoringplugin.cc
Date: 2024-12-13 15:45:52
Exec Total Coverage
Lines: 0 222 0.0%
Branches: 0 366 0.0%

Line Branch Exec Source
1 // BSD 2-Clause License
2
3 // Copyright (c) 2015 - 2018, hpp-plot
4 // Authors: Heidy Dallard, Joseph Mirabel
5 // All rights reserved.
6
7 // Redistribution and use in source and binary forms, with or without
8 // modification, are permitted provided that the following conditions
9 // are met:
10
11 // * Redistributions of source code must retain the above copyright
12 // notice, this list of conditions and the following disclaimer.
13
14 // * Redistributions in binary form must reproduce the above copyright
15 // notice, this list of conditions and the following disclaimer in
16 // the documentation and/or other materials provided with the
17 // distribution.
18
19 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22 // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23 // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
24 // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26 // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
27 // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
28 // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
30 // OF THE POSSIBILITY OF SUCH DAMAGE.
31
32 #include "hppmonitoringplugin.hh"
33
34 #include <QDockWidget>
35 #include <QHBoxLayout>
36 #include <QtGlobal>
37 #include <limits>
38 #if (QT_VERSION >= QT_VERSION_CHECK(5, 0, 0))
39 #include <QtConcurrent>
40 #else
41 #include <QtCore>
42 #endif
43
44 #include <gepetto/gui/mainwindow.hh>
45
46 using gepetto::gui::MainWindow;
47
48 namespace hpp {
49 namespace plot {
50 HppMonitoringPlugin::HppMonitoringPlugin()
51 : cgWidget_(NULL), manip_(NULL), basic_(NULL), hppPlugin_(NULL) {}
52
53 HppMonitoringPlugin::~HppMonitoringPlugin() {
54 MainWindow* main = MainWindow::instance();
55 foreach (QDockWidget* dock, docks_) {
56 main->removeDockWidget(dock);
57 delete dock;
58 }
59 docks_.clear();
60 closeConnection();
61 }
62
63 void HppMonitoringPlugin::init() {
64 openConnection();
65
66 MainWindow* main = MainWindow::instance();
67 QDockWidget* dock;
68
69 // Constraint graph widget
70 dock = new QDockWidget("Constraint &Graph", main);
71 dock->setObjectName("hppmonitoringplugin.constraintgraph");
72 cgWidget_ = new hpp::plot::HppManipulationGraphWidget(manip_, main);
73 dock->setWidget(cgWidget_);
74 main->insertDockWidget(dock, Qt::RightDockWidgetArea, Qt::Horizontal);
75 dock->toggleViewAction()->setShortcut(gepetto::gui::DockKeyShortcutBase +
76 Qt::Key_G);
77 main->registerShortcut(dock->windowTitle(), "Toggle view",
78 dock->toggleViewAction());
79
80 hpp::plot::GraphAction* a = new hpp::plot::GraphAction(cgWidget_);
81 a->setShortcut(Qt::Key_C);
82 a->setText("Generate from &current config");
83 connect(a, SIGNAL(activated(hpp::ID)), SLOT(projectCurrentConfigOn(hpp::ID)));
84 cgWidget_->addNodeContextMenuAction(a);
85 cgWidget_->addAction(a);
86 main->registerShortcut(dock->windowTitle(), a);
87
88 a = new hpp::plot::GraphAction(cgWidget_);
89 a->setShortcut(Qt::Key_G);
90 a->setText("&Generate config");
91 connect(a, SIGNAL(activated(hpp::ID)), SLOT(projectRandomConfigOn(hpp::ID)));
92 cgWidget_->addNodeContextMenuAction(a);
93 cgWidget_->addAction(a);
94 main->registerShortcut(dock->windowTitle(), a);
95
96 a = new hpp::plot::GraphAction(cgWidget_);
97 a->setShortcut(Qt::Key_T);
98 a->setText("Set as &target state");
99 connect(a, SIGNAL(activated(hpp::ID)), SLOT(setTargetState(hpp::ID)));
100 cgWidget_->addNodeContextMenuAction(a);
101 cgWidget_->addAction(a);
102 main->registerShortcut(dock->windowTitle(), a);
103
104 a = new hpp::plot::GraphAction(cgWidget_);
105 // a->setShortcut(Qt::Key_C);
106 a->setText("Display &node constraints");
107 cgWidget_->connect(a, SIGNAL(activated(hpp::ID)),
108 SLOT(displayNodeConstraint(hpp::ID)));
109 cgWidget_->addNodeContextMenuAction(a);
110 // cgWidget_->addAction(a);
111 // main->registerShortcut(dock->windowTitle(), a);
112
113 a = new hpp::plot::GraphAction(cgWidget_);
114 a->setShortcut(Qt::Key_E);
115 a->setText("&Extend current config");
116 connect(a, SIGNAL(activated(hpp::ID)),
117 SLOT(extendFromCurrentToCurrentConfigOn(hpp::ID)));
118 cgWidget_->addEdgeContextMenuAction(a);
119 cgWidget_->addAction(a);
120 main->registerShortcut(dock->windowTitle(), a);
121
122 a = new hpp::plot::GraphAction(cgWidget_);
123 a->setShortcut(Qt::Key_R);
124 a->setText("&Extend current config to random config");
125 connect(a, SIGNAL(activated(hpp::ID)),
126 SLOT(extendFromCurrentToRandomConfigOn(hpp::ID)));
127 cgWidget_->addEdgeContextMenuAction(a);
128 cgWidget_->addAction(a);
129 main->registerShortcut(dock->windowTitle(), a);
130
131 a = new hpp::plot::GraphAction(cgWidget_);
132 // a->setShortcut(Qt::Key_C);
133 a->setText("Display edge &constraints");
134 cgWidget_->connect(a, SIGNAL(activated(hpp::ID)),
135 SLOT(displayEdgeConstraint(hpp::ID)));
136 cgWidget_->addEdgeContextMenuAction(a);
137 cgWidget_->addAction(a);
138 main->registerShortcut(dock->windowTitle(), a);
139
140 a = new hpp::plot::GraphAction(cgWidget_);
141 // a->setShortcut(Qt::Key_T);
142 a->setText("Display edge &target constraints");
143 cgWidget_->connect(a, SIGNAL(activated(hpp::ID)),
144 SLOT(displayEdgeTargetConstraint(hpp::ID)));
145 cgWidget_->addEdgeContextMenuAction(a);
146 // cgWidget_->addAction(a);
147 // main->registerShortcut(dock->windowTitle(), a);
148
149 connect(main, SIGNAL(refresh()), cgWidget_, SLOT(updateGraph()));
150 connect(main, SIGNAL(applyCurrentConfiguration()),
151 SLOT(applyCurrentConfiguration()));
152
153 main->connectSignal(SIGNAL(appliedConfigAtParam(int, double)),
154 SLOT(appliedConfigAtParam(int, double)), this);
155 }
156
157 QString HppMonitoringPlugin::name() const {
158 return QString("Monitoring for hpp-manipulation-corba");
159 }
160
161 static QString getHppIIOPurl() {
162 auto* settings = gepetto::gui::MainWindow::instance()->settings_;
163 QString host("localhost"), port("13331");
164
165 QByteArray env = qgetenv("HPP_HOST");
166 if (!env.isNull()) host = env;
167 env = qgetenv("HPP_PORT");
168 if (!env.isNull()) port = env;
169
170 host = settings->getSetting("hpp/host", host).toString();
171 port = settings->getSetting("hpp/port", port).toString();
172 return QString("corbaloc:iiop:%1:%2").arg(host).arg(port);
173 }
174
175 static QString getHppContext() {
176 QString context =
177 gepetto::gui::MainWindow::instance()
178 ->settings_->getSetting("hpp/context", QString("corbaserver"))
179 .toString();
180 return context;
181 }
182
183 void HppMonitoringPlugin::openConnection() {
184 closeConnection();
185 basic_ = new hpp::corbaServer::Client(0, 0);
186 manip_ = new hpp::corbaServer::manipulation::Client(0, 0);
187 QByteArray iiop = getHppIIOPurl().toLatin1();
188 QByteArray context = getHppContext().toLatin1();
189 try {
190 basic_->connect(iiop.constData(), context.constData());
191 manip_->connect(iiop.constData(), context.constData());
192 hpp::Names_t_var for_deletion = manip_->problem()->getAvailable("type");
193 } catch (const CORBA::Exception& e) {
194 QString error("Could not find the manipulation server. Is it running ?");
195 error += "\n";
196 error += e._name();
197 error += " : ";
198 error += e._rep_id();
199
200 gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
201 if (main != NULL)
202 main->logError(error);
203 else
204 qDebug() << error;
205
206 closeConnection();
207 return;
208 }
209 if (cgWidget_) cgWidget_->client(manip_);
210 }
211
212 void HppMonitoringPlugin::closeConnection() {
213 if (basic_) delete basic_;
214 basic_ = NULL;
215 if (manip_) delete manip_;
216 manip_ = NULL;
217 }
218
219 bool HppMonitoringPlugin::corbaException(int jobId,
220 const CORBA::Exception& excep) const {
221 try {
222 const hpp::Error& error = dynamic_cast<const hpp::Error&>(excep);
223 MainWindow::instance()->logJobFailed(jobId, QString(error.msg));
224 return true;
225 } catch (const std::bad_cast&) {
226 // dynamic_cast failed.
227 }
228 return false;
229 }
230
231 void HppMonitoringPlugin::projectRandomConfigOn(hpp::ID idNode) {
232 QFutureWatcher<bool>* fw = new QFutureWatcher<bool>(this);
233 QDialog* d = new QDialog(NULL, Qt::Dialog);
234 QLabel* l = new QLabel("Projecting...");
235 d->setLayout(new QHBoxLayout);
236 d->layout()->addWidget(l);
237 connect(this, SIGNAL(projectionStatus(QString)), l, SLOT(setText(QString)));
238 d->show();
239 fw->setFuture(QtConcurrent::run(
240 this, &HppMonitoringPlugin::projectRandomConfigOn_impl, idNode));
241 connect(fw, SIGNAL(finished()), d, SLOT(deleteLater()));
242 connect(fw, SIGNAL(finished()), fw, SLOT(deleteLater()));
243 }
244
245 bool HppMonitoringPlugin::projectRandomConfigOn_impl(hpp::ID idNode) {
246 if (manip_ == NULL) return false;
247 hpp::floatSeq_var qRand;
248 hpp::floatSeq_var res;
249 ::CORBA::Double error, minError = std::numeric_limits<double>::infinity();
250 int i = 0;
251 try {
252 do {
253 qRand = basic_->robot()->shootRandomConfig();
254 i++;
255 bool success = manip_->graph()->applyNodeConstraints(idNode, qRand.in(),
256 res.out(), error);
257 if (success) {
258 setCurrentConfig(res.in());
259 return true;
260 }
261 if (error < minError) {
262 minError = error;
263 }
264 if (i >= 20) {
265 qDebug() << "Projection failed after 20 trials.";
266 break;
267 }
268 emit projectionStatus(
269 QString("Tried %1 times. Minimal residual error is %2")
270 .arg(i)
271 .arg(minError));
272 } while (true);
273 } catch (const hpp::Error& e) {
274 MainWindow::instance()->logError(e.msg.in());
275 }
276 return false;
277 }
278
279 bool HppMonitoringPlugin::projectCurrentConfigOn(ID idNode) {
280 hpp::floatSeq from = getCurrentConfig();
281 return projectConfigOn(from, idNode);
282 }
283
284 void HppMonitoringPlugin::setTargetState(ID idNode) {
285 if (manip_ == NULL) return;
286 manip_->problem()->setTargetState(idNode);
287 }
288
289 bool HppMonitoringPlugin::extendFromCurrentToCurrentConfigOn(hpp::ID idEdge) {
290 hpp::floatSeq from = getCurrentConfig();
291 return extendConfigOn(from, from, idEdge);
292 }
293
294 bool HppMonitoringPlugin::extendFromCurrentToRandomConfigOn(hpp::ID idEdge) {
295 hpp::floatSeq from = getCurrentConfig();
296 hpp::floatSeq_var qRand = basic_->robot()->shootRandomConfig();
297 return extendConfigOn(from, qRand.in(), idEdge);
298 }
299
300 bool HppMonitoringPlugin::projectConfigOn(hpp::floatSeq config,
301 hpp::ID idNode) {
302 if (manip_ == NULL) return false;
303 hpp::floatSeq_var res;
304 ::CORBA::Double error;
305 bool success =
306 manip_->graph()->applyNodeConstraints(idNode, config, res.out(), error);
307 if (success) {
308 setCurrentConfig(res.in());
309 } else {
310 MainWindow::instance()->logError(
311 QString("Unable to project configuration. Residual error is %1")
312 .arg(error));
313 }
314 return success;
315 }
316
317 bool HppMonitoringPlugin::extendConfigOn(hpp::floatSeq from,
318 hpp::floatSeq config, hpp::ID idEdge) {
319 if (manip_ == NULL) return false;
320 hpp::floatSeq_var res;
321 ::CORBA::Double error;
322 bool success = manip_->graph()->generateTargetConfig(idEdge, from, config,
323 res.out(), error);
324 if (success) {
325 setCurrentConfig(res.in());
326 } else {
327 MainWindow::instance()->logError(
328 QString("Unable to project configuration. Residual error is %1")
329 .arg(error));
330 }
331 return success;
332 }
333
334 void HppMonitoringPlugin::applyCurrentConfiguration() {
335 hpp::floatSeq q = getCurrentConfig();
336 cgWidget_->showNodeOfConfiguration(q);
337 }
338
339 void HppMonitoringPlugin::appliedConfigAtParam(int pid, double param) {
340 if (manip_ == NULL) return;
341 CORBA::String_var graphName;
342 hpp::ID id;
343 try {
344 id = manip_->problem()->edgeAtParam(pid, param, graphName.out());
345 } catch (const hpp::Error& e) {
346 return;
347 }
348 if (strcmp(graphName.in(), cgWidget_->graphName().c_str()) != 0)
349 cgWidget_->showEdge(id);
350 }
351
352 hpp::floatSeq HppMonitoringPlugin::getCurrentConfig() {
353 QObject* plugin(hppPlugin());
354 hpp::floatSeq const* config;
355 bool ok = QMetaObject::invokeMethod(
356 plugin, "getCurrentConfig", Qt::DirectConnection,
357 Q_RETURN_ARG(hpp::floatSeq const*, config));
358 if (!ok) {
359 qDebug() << "HppMonitoringPlugin::getCurrentConfig failed";
360 }
361 return *config;
362 }
363
364 void HppMonitoringPlugin::setCurrentConfig(const hpp::floatSeq& q) {
365 QObject* plugin(hppPlugin());
366 bool ok = QMetaObject::invokeMethod(plugin, "setCurrentConfig",
367 Qt::DirectConnection,
368 Q_ARG(const hpp::floatSeq&, q));
369
370 if (!ok) {
371 qDebug() << "HppMonitoringPlugin::setCurrentConfig failed";
372 }
373 }
374
375 QObject* HppMonitoringPlugin::hppPlugin() {
376 if (hppPlugin_ == NULL) {
377 MainWindow* main = MainWindow::instance();
378 hppPlugin_ = main->getFromSlot("getCurrentConfig");
379 }
380 if (hppPlugin_ == NULL) {
381 throw std::runtime_error(
382 "unable to retrieve slot getCurrentConfig from HPP plugin");
383 }
384 return hppPlugin_;
385 }
386 } // namespace plot
387 } // namespace hpp
388
389 #if (QT_VERSION < QT_VERSION_CHECK(5, 0, 0))
390 Q_EXPORT_PLUGIN2(hppmonitoringplugin, hpp::plot::HppMonitoringPlugin)
391 #endif
392