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