GCC Code Coverage Report

Directory: plugins/
File: plugins/hppwidgetsplugin/roadmap.cc
Date: 2024-12-13 15:51:58
Exec Total Coverage
Lines: 0 97 0.0%
Branches: 0 136 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) CNRS
3 // Authors: Joseph Mirabel
4 //
5
6 #include <QDebug>
7 #include <gepetto/gui/mainwindow.hh>
8 #include <gepetto/gui/windows-manager.hh>
9 #include <gepetto/viewer/corba/graphical-interface.hh>
10 #include <hppwidgetsplugin/conversions.hh>
11 #include <hppwidgetsplugin/roadmap.hh>
12 #include <sstream>
13 #include <string>
14
15 namespace hpp {
16 namespace gui {
17 ✗ Roadmap::Roadmap(HppWidgetsPlugin* plugin)
18 ✗ : radius(0.01f),
19 ✗ axisSize(0.05f),
20 ✗ currentNodeId_(0),
21 ✗ currentEdgeId_(0),
22 ✗ nodeColorMap_(0),
23 ✗ edgeColorMap_(0),
24 ✗ plugin_(plugin),
25 ✗ link_(false) {}
26
27 ✗ void Roadmap::initRoadmap() {
28 ✗ HppWidgetsPlugin::HppClient* hpp = plugin_->client();
29 ✗ int nbCC = hpp->problem()->numberConnectedComponents();
30 ✗ nodeColorMap_ = gepetto::gui::ColorMap(nbCC + 10);
31 ✗ edgeColorMap_ = gepetto::gui::ColorMap(nbCC + 10);
32
33 try {
34 gepetto::gui::WindowsManagerPtr_t wsm =
35 ✗ gepetto::gui::MainWindow::instance()->osg();
36 ✗ wsm->createScene(roadmapName().c_str());
37 ✗ } catch (const gepetto::Error&) {
38 ✗ qDebug() << "Roadmap" << QString::fromStdString(roadmapName())
39 ✗ << "already exists.";
40 }
41 }
42
43 ✗ void Roadmap::initRoadmapFromJoint(const std::string& jointName) {
44 ✗ name_ = jointName;
45 ✗ link_ = false;
46 ✗ initRoadmap();
47 }
48
49 ✗ void Roadmap::initRoadmapFromBody(const std::string& bodyName) {
50 ✗ name_ = bodyName;
51 ✗ link_ = true;
52 ✗ initRoadmap();
53 }
54
55 ✗ void Roadmap::displayRoadmap() {
56 ✗ std::size_t nbNodes = numberNodes();
57 ✗ std::size_t nbEdges = numberEdges();
58 ✗ if (currentNodeId_ >= nbNodes && currentEdgeId_ >= nbEdges) return;
59
60 ✗ std::string rn = roadmapName();
61 ✗ Color color;
62 gepetto::gui::WindowsManagerPtr_t wsm =
63 ✗ gepetto::gui::MainWindow::instance()->osg();
64 ✗ if (nbNodes == 0) {
65 ✗ gepetto::gui::MainWindow::instance()->logError(
66 "There is no node in the roadmap.");
67 ✗ return;
68 }
69 ✗ beforeDisplay();
70 ✗ for (; currentNodeId_ < nbNodes; ++currentNodeId_) {
71 ✗ Frame pos;
72 ✗ nodePosition(currentNodeId_, pos);
73 ✗ nodeColor(currentNodeId_, color);
74 ✗ std::string name = nodeName(currentNodeId_);
75 ✗ wsm->addXYZaxis(name, color, radius, axisSize);
76 ✗ wsm->applyConfiguration(name, pos);
77 }
78 ✗ for (; currentEdgeId_ < nbEdges; ++currentEdgeId_) {
79 ✗ Position pos1, pos2;
80 ✗ edgePositions(currentEdgeId_, pos1, pos2);
81 ✗ edgeColor(currentEdgeId_, color);
82 ✗ std::string name = edgeName(currentEdgeId_);
83 ✗ wsm->addLine(name, pos1, pos2, color);
84 }
85 ✗ wsm->addToGroup(rn, "hpp-gui");
86 ✗ afterDisplay();
87 ✗ wsm->refresh();
88 }
89
90 ✗ void Roadmap::beforeDisplay() {}
91
92 ✗ void Roadmap::afterDisplay() {}
93
94 ✗ std::size_t Roadmap::numberNodes() {
95 ✗ return plugin_->client()->problem()->numberNodes();
96 }
97
98 ✗ std::size_t Roadmap::numberEdges() {
99 ✗ return plugin_->client()->problem()->numberEdges();
100 }
101
102 ✗ std::string Roadmap::roadmapName() { return "roadmap_" + name_; }
103
104 ✗ std::string Roadmap::nodeName(NodeID nodeId) {
105 ✗ std::stringstream ss;
106 ✗ ss << roadmapName() << "/node" << nodeId;
107 ✗ return ss.str();
108 }
109
110 ✗ std::string Roadmap::edgeName(EdgeID edgeId) {
111 ✗ std::stringstream ss;
112 ✗ ss << roadmapName() << "/edge" << edgeId;
113 ✗ return ss.str();
114 }
115
116 ✗ void Roadmap::nodePosition(NodeID nodeId, Frame& frame) {
117 ✗ HppWidgetsPlugin::HppClient* hpp = plugin_->client();
118 ✗ hpp::floatSeq_var n = hpp->problem()->node(nodeId);
119 ✗ getPosition(n.in(), frame);
120 }
121
122 ✗ void Roadmap::edgePositions(EdgeID edgeId, Position& start, Position& end) {
123 ✗ HppWidgetsPlugin::HppClient* hpp = plugin_->client();
124 ✗ hpp::floatSeq_var n1, n2;
125 ✗ Frame t;
126 ✗ hpp->problem()->edge(edgeId, n1.out(), n2.out());
127 ✗ getPosition(n1.in(), t);
128 ✗ start = t.position;
129 ✗ getPosition(n2.in(), t);
130 ✗ end = t.position;
131 }
132
133 ✗ void Roadmap::nodeColor(NodeID nodeId, Color& color) {
134 CORBA::Long iCC =
135 ✗ plugin_->client()->problem()->connectedComponentOfNode(nodeId);
136 ✗ nodeColorMap_.getColor(iCC, color);
137 }
138
139 ✗ void Roadmap::edgeColor(EdgeID edgeId, Color& color) {
140 CORBA::Long iCC =
141 ✗ plugin_->client()->problem()->connectedComponentOfEdge(edgeId);
142 ✗ edgeColorMap_.getColor(iCC, color);
143 }
144
145 ✗ inline void Roadmap::getPosition(const hpp::floatSeq& q, Frame& t) const {
146 ✗ HppWidgetsPlugin::HppClient* hpp = plugin_->client();
147 ✗ hpp::TransformSeq_var Ts;
148 // Temporary object to avoid dynamic allocation.
149 // Arguments are max, length, storage, take ownership.
150 char* tmps[1];
151 ✗ hpp::Names_t names(1, 1, tmps, false);
152 ✗ names[0] = name_.c_str();
153 ✗ if (link_)
154 ✗ Ts = hpp->robot()->getLinksPosition(q, names);
155 else
156 ✗ Ts = hpp->robot()->getJointsPosition(q, names);
157 ✗ fromHPP(Ts[0], t);
158 }
159 } // namespace gui
160 } // namespace hpp
161