GCC Code Coverage Report

Directory: ./
File: src/roadmap.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 201 244 82.4%
Branches: 188 422 44.5%
Line Branch Exec Source
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #include <../src/nearest-neighbor/basic.hh>
31 #include <algorithm>
32 #include <hpp/core/connected-component.hh>
33 #include <hpp/core/edge.hh>
34 #include <hpp/core/node.hh>
35 #include <hpp/core/path-vector.hh>
36 #include <hpp/core/path.hh>
37 #include <hpp/core/roadmap.hh>
38 #include <hpp/pinocchio/configuration.hh>
39 #include <hpp/util/debug.hh>
40 #include <stdexcept>
41
42 namespace hpp {
43 namespace core {
44 using pinocchio::displayConfig;
45
46 20 RoadmapPtr_t Roadmap::create(const DistancePtr_t& distance,
47 const DevicePtr_t& robot) {
48
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 Roadmap* ptr = new Roadmap(distance, robot);
49 20 RoadmapPtr_t shPtr(ptr);
50
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 ptr->init(shPtr);
51 20 return shPtr;
52 }
53
54 20 Roadmap::Roadmap(const DistancePtr_t& distance, const DevicePtr_t&)
55 20 : distance_(distance),
56 20 connectedComponents_(),
57 20 nodes_(),
58 20 edges_(),
59 20 initNode_(),
60 20 goalNodes_(),
61
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 40 nearestNeighbor_(new nearestNeighbor::Basic(distance)) {}
62
63 84 Roadmap::~Roadmap() {
64 28 clear();
65
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 28 delete nearestNeighbor_;
66 56 }
67
68 213 const ConnectedComponents_t& Roadmap::connectedComponents() const {
69 213 return connectedComponents_;
70 }
71
72 8 NearestNeighborPtr_t Roadmap::nearestNeighbor() { return nearestNeighbor_; }
73
74 ✗ void Roadmap::nearestNeighbor(NearestNeighborPtr_t nearestNeighbor) {
75 ✗ if (nodes_.size() != 0) {
76 ✗ throw std::runtime_error(
77 "The roadmap must be empty before setting a new NearestNeighbor "
78 ✗ "object.");
79 }
80 ✗ if (nearestNeighbor != NULL) {
81 ✗ delete nearestNeighbor_;
82 ✗ nearestNeighbor_ = nearestNeighbor;
83 }
84 }
85
86 14 void Roadmap::clear() {
87 14 connectedComponents_.clear();
88
89
2/2
✓ Branch 4 taken 37 times.
✓ Branch 5 taken 14 times.
 51 for (Nodes_t::iterator it = nodes_.begin(); it != nodes_.end(); ++it) {
90
1/2
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
 37 delete *it;
91 }
92 14 nodes_.clear();
93
94
2/2
✓ Branch 4 taken 35 times.
✓ Branch 5 taken 14 times.
 49 for (Edges_t::iterator it = edges_.begin(); it != edges_.end(); ++it) {
95
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 delete *it;
96 }
97 14 edges_.clear();
98
99 14 goalNodes_.clear();
100 14 initNode_ = 0x0;
101 14 nearestNeighbor_->clear();
102 14 }
103
104 45 NodePtr_t Roadmap::addNode(ConfigurationIn_t configuration) {
105 value_type distance;
106
2/2
✓ Branch 1 taken 35 times.
✓ Branch 2 taken 10 times.
 45 if (nodes_.size() != 0) {
107
2/4
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35 times.
✗ Branch 5 not taken.
 35 NodePtr_t nearest = nearestNode(configuration, distance);
108
4/6
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 12 times.
✓ Branch 7 taken 23 times.
 35 if (nearest->configuration() == configuration) {
109 12 return nearest;
110 }
111 }
112
2/4
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33 times.
✗ Branch 5 not taken.
 33 NodePtr_t node = createNode(configuration);
113 hppDout(info, "Added node: " << displayConfig(configuration));
114
1/2
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
 33 push_node(node);
115 // Node constructor creates a new connected component. This new
116 // connected component needs to be added in the roadmap and the
117 // new node needs to be registered in the connected component.
118
1/2
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
 33 addConnectedComponent(node);
119 33 return node;
120 }
121
122 49 NodePtr_t Roadmap::addNode(ConfigurationIn_t configuration,
123 ConnectedComponentPtr_t connectedComponent) {
124
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 49 times.
 49 assert(connectedComponent);
125 value_type distance;
126
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 if (nodes_.size() != 0) {
127 NodePtr_t nearest =
128
2/4
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49 times.
✗ Branch 5 not taken.
 49 nearestNode(configuration, connectedComponent, distance);
129
3/6
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 49 times.
 49 if (nearest->configuration() == configuration) {
130 ✗ return nearest;
131 }
132 }
133
2/4
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49 times.
✗ Branch 5 not taken.
 49 NodePtr_t node = createNode(configuration);
134
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 node->connectedComponent(connectedComponent);
135 hppDout(info, "Added node: " << displayConfig(configuration));
136
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 push_node(node);
137 // The new node needs to be registered in the connected
138 // component.
139
1/2
✓ Branch 2 taken 49 times.
✗ Branch 3 not taken.
 49 connectedComponent->addNode(node);
140
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 nearestNeighbor_->addNode(node);
141 49 return node;
142 }
143
144 49 void Roadmap::addEdges(const NodePtr_t from, const NodePtr_t& to,
145 const PathPtr_t& path) {
146
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 EdgePtr_t edge = new Edge(from, to, path);
147
3/6
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 49 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 49 times.
✗ Branch 7 not taken.
 49 if (!from->isOutNeighbor(to)) from->addOutEdge(edge);
148
3/6
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 49 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 49 times.
✗ Branch 7 not taken.
 49 if (!to->isInNeighbor(from)) to->addInEdge(edge);
149
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 impl_addEdge(edge);
150
2/4
✓ Branch 2 taken 49 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 49 times.
✗ Branch 6 not taken.
 49 edge = new Edge(to, from, path->reverse());
151
3/6
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 49 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 49 times.
✗ Branch 7 not taken.
 49 if (!from->isInNeighbor(to)) from->addInEdge(edge);
152
3/6
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 49 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 49 times.
✗ Branch 7 not taken.
 49 if (!to->isOutNeighbor(from)) to->addOutEdge(edge);
153
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 impl_addEdge(edge);
154 49 }
155
156 49 NodePtr_t Roadmap::addNodeAndEdges(const NodePtr_t from, ConfigurationIn_t to,
157 const PathPtr_t path) {
158
3/6
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49 times.
✗ Branch 8 not taken.
 49 NodePtr_t nodeTo = addNode(to, from->connectedComponent());
159
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 addEdges(from, nodeTo, path);
160 49 return nodeTo;
161 }
162
163 ✗ NodePtr_t Roadmap::addNodeAndEdge(const NodePtr_t from, ConfigurationIn_t to,
164 const PathPtr_t path) {
165 ✗ NodePtr_t nodeTo = addNode(to, from->connectedComponent());
166 ✗ addEdge(from, nodeTo, path);
167 ✗ return nodeTo;
168 }
169
170 ✗ NodePtr_t Roadmap::addNodeAndEdge(ConfigurationIn_t from, const NodePtr_t to,
171 const PathPtr_t path) {
172 ✗ NodePtr_t nodeFrom = addNode(from, to->connectedComponent());
173 ✗ addEdge(nodeFrom, to, path);
174 ✗ return nodeFrom;
175 }
176
177 ✗ void Roadmap::merge(const RoadmapPtr_t& other) {
178 // Map nodes of other roadmap with nodes of this one
179 ✗ std::map<core::NodePtr_t, core::NodePtr_t> cNode;
180 ✗ for (const core::NodePtr_t& node : other->nodes()) {
181 ✗ cNode[node] = this->addNode(node->configuration());
182 }
183 ✗ for (const core::EdgePtr_t& edge : other->edges()) {
184 ✗ if (edge->path()->length() == 0)
185 ✗ assert(edge->from() == edge->to());
186 else
187 ✗ this->addEdges(cNode[edge->from()], cNode[edge->to()], edge->path());
188 }
189 }
190
191 ✗ void Roadmap::insertPathVector(const PathVectorPtr_t& path, bool backAndForth) {
192 ✗ if (path->constraints()) {
193 ✗ throw std::logic_error(
194 "Cannot insert a path vector with constraints"
195 ✗ " in a roadmap.");
196 }
197 ✗ Configuration_t q_init(path->initial());
198 ✗ NodePtr_t n(addNode(q_init));
199 ✗ for (std::size_t i = 0; i < path->numberPaths(); ++i) {
200 ✗ PathPtr_t p(path->pathAtRank(i));
201 ✗ if (backAndForth) {
202 ✗ n = addNodeAndEdges(n, p->end(), p);
203 } else {
204 ✗ n = addNodeAndEdge(n, p->end(), p);
205 }
206 }
207 }
208
209 35 NodePtr_t Roadmap::nearestNode(ConfigurationIn_t configuration,
210 value_type& minDistance, bool reverse) {
211 35 NodePtr_t closest = 0x0;
212 35 minDistance = std::numeric_limits<value_type>::infinity();
213 35 for (ConnectedComponents_t::const_iterator itcc =
214 35 connectedComponents_.begin();
215
2/2
✓ Branch 3 taken 97 times.
✓ Branch 4 taken 35 times.
 132 itcc != connectedComponents_.end(); ++itcc) {
216 value_type distance;
217 NodePtr_t node;
218
2/4
✓ Branch 2 taken 97 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 97 times.
✗ Branch 6 not taken.
 97 node = nearestNeighbor_->search(configuration, *itcc, distance, reverse);
219
2/2
✓ Branch 0 taken 63 times.
✓ Branch 1 taken 34 times.
 97 if (distance < minDistance) {
220 63 minDistance = distance;
221 63 closest = node;
222 }
223 }
224 35 return closest;
225 }
226
227 97 NodePtr_t Roadmap::nearestNode(
228 ConfigurationIn_t configuration,
229 const ConnectedComponentPtr_t& connectedComponent, value_type& minDistance,
230 bool reverse) {
231
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 97 times.
 97 assert(connectedComponent);
232
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 97 times.
 97 assert(connectedComponent->nodes().size() != 0);
233
1/2
✓ Branch 2 taken 97 times.
✗ Branch 3 not taken.
 97 NodePtr_t closest = nearestNeighbor_->search(
234 configuration, connectedComponent, minDistance, reverse);
235 97 return closest;
236 }
237
238 ✗ Nodes_t Roadmap::nearestNodes(ConfigurationIn_t configuration, size_type k) {
239 value_type d;
240 ✗ return nearestNeighbor_->KnearestSearch(configuration, weak_.lock(), k, d);
241 }
242
243 ✗ Nodes_t Roadmap::nearestNodes(ConfigurationIn_t configuration,
244 const ConnectedComponentPtr_t& connectedComponent,
245 size_type k) {
246 value_type d;
247 ✗ return nearestNeighbor_->KnearestSearch(configuration, connectedComponent, k,
248 ✗ d);
249 }
250
251 ✗ NodeVector_t Roadmap::nodesWithinBall(ConfigurationIn_t q,
252 const ConnectedComponentPtr_t& cc,
253 value_type maxD) {
254 ✗ return nearestNeighbor_->withinBall(q, cc, maxD);
255 }
256
257 12 NodePtr_t Roadmap::addGoalNode(ConfigurationIn_t config) {
258
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 NodePtr_t node = addNode(config);
259
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 goalNodes_.push_back(node);
260 12 return node;
261 }
262
263 ✗ const DistancePtr_t& Roadmap::distance() const { return distance_; }
264
265 34 EdgePtr_t Roadmap::addEdge(const NodePtr_t& n1, const NodePtr_t& n2,
266 const PathPtr_t& path) {
267
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 EdgePtr_t edge = new Edge(n1, n2, path);
268
3/6
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 34 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 34 times.
✗ Branch 7 not taken.
 34 if (!n1->isOutNeighbor(n2)) n1->addOutEdge(edge);
269
3/6
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 34 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 34 times.
✗ Branch 7 not taken.
 34 if (!n2->isInNeighbor(n1)) n2->addInEdge(edge);
270
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 impl_addEdge(edge);
271 34 return edge;
272 }
273
274 33 void Roadmap::addConnectedComponent(const NodePtr_t& node) {
275
1/2
✓ Branch 2 taken 33 times.
✗ Branch 3 not taken.
 33 connectedComponents_.insert(node->connectedComponent());
276
1/2
✓ Branch 3 taken 33 times.
✗ Branch 4 not taken.
 33 node->connectedComponent()->addNode(node);
277 33 nearestNeighbor_->addNode(node);
278 33 }
279
280 82 NodePtr_t Roadmap::createNode(ConfigurationIn_t configuration) const {
281
2/4
✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
 82 return NodePtr_t(new Node(configuration));
282 }
283
284 20 void Roadmap::init(RoadmapWkPtr_t weak) { weak_ = weak; }
285
286 132 void Roadmap::connect(const ConnectedComponentPtr_t& cc1,
287 const ConnectedComponentPtr_t& cc2) {
288
3/4
✓ Branch 2 taken 132 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 98 times.
✓ Branch 5 taken 34 times.
 132 if (cc1->canReach(cc2)) return;
289 34 ConnectedComponent::RawPtrs_t cc2Tocc1;
290
3/4
✓ Branch 2 taken 34 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 14 times.
✓ Branch 5 taken 20 times.
 34 if (cc2->canReach(cc1, cc2Tocc1)) {
291
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 merge(cc1, cc2Tocc1);
292 } else {
293
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 20 cc1->reachableTo_.insert(cc2.get());
294
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 20 cc2->reachableFrom_.insert(cc1.get());
295 }
296 34 }
297
298 14 void Roadmap::merge(const ConnectedComponentPtr_t& cc1,
299 ConnectedComponent::RawPtrs_t& ccs) {
300 14 for (ConnectedComponent::RawPtrs_t::iterator itcc = ccs.begin();
301
2/2
✓ Branch 3 taken 31 times.
✓ Branch 4 taken 14 times.
 45 itcc != ccs.end(); ++itcc) {
302
2/2
✓ Branch 2 taken 17 times.
✓ Branch 3 taken 14 times.
 31 if (*itcc != cc1.get()) {
303
1/2
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 cc1->merge((*itcc)->self());
304 #ifndef NDEBUG
305 std::size_t nb =
306 #endif
307
1/2
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
 17 connectedComponents_.erase((*itcc)->self());
308
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 17 times.
 17 assert(nb == 1);
309 }
310 }
311 14 }
312
313 132 void Roadmap::impl_addEdge(const EdgePtr_t& edge) {
314
1/2
✓ Branch 1 taken 132 times.
✗ Branch 2 not taken.
 132 edges_.push_back(edge);
315
316
1/2
✓ Branch 2 taken 132 times.
✗ Branch 3 not taken.
 132 ConnectedComponentPtr_t cc1 = edge->from()->connectedComponent();
317
1/2
✓ Branch 2 taken 132 times.
✗ Branch 3 not taken.
 132 ConnectedComponentPtr_t cc2 = edge->to()->connectedComponent();
318
319
1/2
✓ Branch 1 taken 132 times.
✗ Branch 2 not taken.
 132 connect(cc1, cc2);
320 132 }
321
322 10 bool Roadmap::pathExists() const {
323
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 const ConnectedComponentPtr_t ccInit = initNode()->connectedComponent();
324 10 for (NodeVector_t::const_iterator itGoal = goalNodes_.begin();
325
2/2
✓ Branch 3 taken 10 times.
✓ Branch 4 taken 7 times.
 17 itGoal != goalNodes_.end(); ++itGoal) {
326
4/6
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✓ Branch 10 taken 7 times.
 10 if (ccInit->canReach((*itGoal)->connectedComponent())) {
327 3 return true;
328 }
329 }
330 7 return false;
331 10 }
332
333 4 std::ostream& Roadmap::print(std::ostream& os) const {
334 // Enumerate nodes and connected components
335 4 std::map<NodePtr_t, size_type> nodeId;
336 4 std::map<ConnectedComponent::RawPtr_t, size_type> ccId;
337 4 std::map<ConnectedComponent::RawPtr_t, size_type> sccId;
338
339 4 size_type count = 0;
340
2/2
✓ Branch 5 taken 24 times.
✓ Branch 6 taken 4 times.
 28 for (Nodes_t::const_iterator it = nodes().begin(); it != nodes().end();
341 24 ++it) {
342
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 24 nodeId[*it] = count;
343 24 ++count;
344 }
345
346 4 count = 0;
347
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 for (ConnectedComponents_t::const_iterator it = connectedComponents().begin();
348
3/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 16 times.
✓ Branch 6 taken 4 times.
 20 it != connectedComponents().end(); ++it) {
349
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 16 ccId[it->get()] = count;
350 16 ++count;
351 }
352
353 // Display list of nodes
354
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 os << "-----------------------------------------------------------------"
355
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 << std::endl;
356
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 os << "Roadmap" << std::endl;
357
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 os << "-----------------------------------------------------------------"
358
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 << std::endl;
359
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 os << "-----------------------------------------------------------------"
360
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 << std::endl;
361
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 os << "Nodes" << std::endl;
362
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 os << "-----------------------------------------------------------------"
363
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 << std::endl;
364
2/2
✓ Branch 5 taken 24 times.
✓ Branch 6 taken 4 times.
 28 for (Nodes_t::const_iterator it = nodes().begin(); it != nodes().end();
365 24 ++it) {
366 24 const NodePtr_t node = *it;
367
6/12
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 24 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 24 times.
✗ Branch 17 not taken.
 24 os << "Node " << nodeId[node] << ": " << *node << std::endl;
368 }
369
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 os << "-----------------------------------------------------------------"
370
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 << std::endl;
371
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 os << "Edges" << std::endl;
372
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 os << "-----------------------------------------------------------------"
373
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 << std::endl;
374
2/2
✓ Branch 5 taken 20 times.
✓ Branch 6 taken 4 times.
 24 for (Edges_t::const_iterator it = edges().begin(); it != edges().end();
375 20 ++it) {
376 20 const EdgePtr_t edge = *it;
377
6/12
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 20 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 20 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 20 times.
✗ Branch 19 not taken.
 40 os << "Edge: " << nodeId[edge->from()] << " -> " << nodeId[edge->to()]
378
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 << std::endl;
379 }
380
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 os << "-----------------------------------------------------------------"
381
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 << std::endl;
382
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 os << "Connected components" << std::endl;
383
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 os << "-----------------------------------------------------------------"
384
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 << std::endl;
385
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 for (ConnectedComponents_t::const_iterator it = connectedComponents().begin();
386
3/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 16 times.
✓ Branch 7 taken 4 times.
 20 it != connectedComponents().end(); ++it) {
387 16 const ConnectedComponentPtr_t cc = *it;
388
4/8
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 16 times.
✗ Branch 12 not taken.
 16 os << "Connected component " << ccId[cc.get()] << std::endl;
389
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 os << "Nodes : ";
390 16 for (NodeVector_t::const_iterator itNode = cc->nodes().begin();
391
2/2
✓ Branch 5 taken 24 times.
✓ Branch 6 taken 16 times.
 40 itNode != cc->nodes().end(); ++itNode) {
392
3/6
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
 24 os << nodeId[*itNode] << ", ";
393 }
394
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 os << std::endl;
395
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 os << "Reachable to :";
396 16 for (ConnectedComponent::RawPtrs_t::const_iterator itTo =
397 16 cc->reachableTo().begin();
398
2/2
✓ Branch 5 taken 8 times.
✓ Branch 6 taken 16 times.
 24 itTo != cc->reachableTo().end(); ++itTo) {
399
3/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
 8 os << ccId[*itTo] << ", ";
400 }
401
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 os << std::endl;
402
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 os << "Reachable from :";
403 16 for (ConnectedComponent::RawPtrs_t::const_iterator itFrom =
404 16 cc->reachableFrom().begin();
405
2/2
✓ Branch 5 taken 8 times.
✓ Branch 6 taken 16 times.
 24 itFrom != cc->reachableFrom().end(); ++itFrom) {
406
3/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
 8 os << ccId[*itFrom] << ", ";
407 }
408
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 os << std::endl;
409 16 }
410
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 os << std::endl;
411
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 os << "----------------" << std::endl;
412
413 4 return os;
414 4 }
415
416 4 std::ostream& operator<<(std::ostream& os, const hpp::core::Roadmap& r) {
417 4 return r.print(os);
418 }
419 } // namespace core
420 } // namespace hpp
421