Directory: | ./ |
---|---|
File: | src/roadmap.cc |
Date: | 2024-08-10 11:29:48 |
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 | 43 | NodePtr_t Roadmap::addNode(ConfigurationIn_t configuration) { | |
105 | value_type distance; | ||
106 |
2/2✓ Branch 1 taken 33 times.
✓ Branch 2 taken 10 times.
|
43 | if (nodes_.size() != 0) { |
107 |
2/4✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33 times.
✗ Branch 5 not taken.
|
33 | NodePtr_t nearest = nearestNode(configuration, distance); |
108 |
4/6✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 10 times.
✓ Branch 7 taken 23 times.
|
33 | if (nearest->configuration() == configuration) { |
109 | 10 | 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 | 33 | NodePtr_t Roadmap::nearestNode(ConfigurationIn_t configuration, | |
210 | value_type& minDistance, bool reverse) { | ||
211 | 33 | NodePtr_t closest = 0x0; | |
212 | 33 | minDistance = std::numeric_limits<value_type>::infinity(); | |
213 | 33 | for (ConnectedComponents_t::const_iterator itcc = | |
214 | 33 | connectedComponents_.begin(); | |
215 |
2/2✓ Branch 3 taken 95 times.
✓ Branch 4 taken 33 times.
|
128 | itcc != connectedComponents_.end(); ++itcc) { |
216 | value_type distance; | ||
217 | NodePtr_t node; | ||
218 |
2/4✓ Branch 2 taken 95 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 95 times.
✗ Branch 6 not taken.
|
95 | node = nearestNeighbor_->search(configuration, *itcc, distance, reverse); |
219 |
2/2✓ Branch 0 taken 61 times.
✓ Branch 1 taken 34 times.
|
95 | if (distance < minDistance) { |
220 | 61 | minDistance = distance; | |
221 | 61 | closest = node; | |
222 | } | ||
223 | } | ||
224 | 33 | 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 | 11 | NodePtr_t Roadmap::addGoalNode(ConfigurationIn_t config) { | |
258 |
2/4✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
|
11 | NodePtr_t node = addNode(config); |
259 |
1/2✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
|
11 | goalNodes_.push_back(node); |
260 | 11 | 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 |