GCC Code Coverage Report

Directory: ./
File: src/path-planner.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 118 152 77.6%
Branches: 122 322 37.9%
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 <hpp/core/edge.hh>
31 #include <hpp/core/nearest-neighbor.hh>
32 #include <hpp/core/node.hh>
33 #include <hpp/core/path-planner.hh>
34 #include <hpp/core/path-planning-failed.hh>
35 #include <hpp/core/path-projector.hh>
36 #include <hpp/core/path-validation.hh>
37 #include <hpp/core/path.hh>
38 #include <hpp/core/problem-target/goal-configurations.hh>
39 #include <hpp/core/problem.hh>
40 #include <hpp/core/roadmap.hh>
41 #include <hpp/core/steering-method.hh>
42 #include <hpp/util/debug.hh>
43 #include <hpp/util/timer.hh>
44 #include <tuple>
45
46 #include "astar.hh"
47
48 namespace hpp {
49 namespace core {
50 unsigned long int uint_infty =
51 std::numeric_limits<unsigned long int>::infinity();
52 value_type float_infty = std::numeric_limits<value_type>::infinity();
53
54 ✗ PathPlanner::PathPlanner(const ProblemConstPtr_t& problem)
55 ✗ : problem_(problem),
56 ✗ roadmap_(Roadmap::create(problem->distance(), problem->robot())),
57 ✗ interrupt_(false),
58 ✗ maxIterations_(uint_infty),
59 ✗ timeOut_(float_infty),
60 ✗ stopWhenProblemIsSolved_(true) {
61 ✗ assert(problem_.lock());
62 }
63
64 8 PathPlanner::PathPlanner(const ProblemConstPtr_t& problem,
65 8 const RoadmapPtr_t& roadmap)
66 8 : problem_(problem),
67 8 roadmap_(roadmap),
68 8 interrupt_(false),
69 8 maxIterations_(uint_infty),
70 8 timeOut_(float_infty),
71 16 stopWhenProblemIsSolved_(true) {
72
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
 8 assert(problem_.lock());
73 8 }
74
75 2 PathPlanner::~PathPlanner() {}
76
77 ✗ void PathPlanner::init(const PathPlannerWkPtr_t& weak) { weakPtr_ = weak; }
78
79 383 const RoadmapPtr_t& PathPlanner::roadmap() const { return roadmap_; }
80
81 366 ProblemConstPtr_t PathPlanner::problem() const { return problem_.lock(); }
82
83 8 void PathPlanner::startSolve() {
84
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 8 problem()->checkProblem();
85 // Tag init and goal configurations in the roadmap
86
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 roadmap()->resetGoalNodes();
87
4/8
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
 8 roadmap()->initNode(problem()->initConfig());
88
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 problemTarget::GoalConfigurationsPtr_t gc(HPP_DYNAMIC_PTR_CAST(
89 8 problemTarget::GoalConfigurations, problem()->target()));
90
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (gc) {
91
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 8 const Configurations_t goals(gc->configurations());
92 8 for (Configurations_t::const_iterator itGoal = goals.begin();
93
2/2
✓ Branch 2 taken 8 times.
✓ Branch 3 taken 8 times.
 16 itGoal != goals.end(); ++itGoal) {
94
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8 times.
✗ Branch 10 not taken.
 8 roadmap()->addGoalNode(*itGoal);
95 }
96 8 }
97
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
 8 problem()->target()->check(roadmap());
98 8 }
99
100 8 PathVectorPtr_t PathPlanner::solve() {
101 namespace bpt = boost::posix_time;
102
103 8 int status = 0;
104 // 0 = execute one more step
105 // 1 = pb solved
106 // 2 = throw exception
107
108 8 interrupt_ = false;
109 8 bool solved = false;
110 8 unsigned long int nIter(0);
111
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 bpt::ptime timeStart(bpt::microsec_clock::universal_time());
112
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 startSolve();
113
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 tryConnectInitAndGoals();
114 // We choose to stop if a direct path solves the problem.
115 // We could also respect the stopWhenLimitReached_ attribute.
116 // It is ambiguous what should be done as it is case dependent.
117 // If the intent is to build a roadmap, then we should not stop.
118 // If the intent is to solve a optimal planning problem, then we should stop.
119
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
 8 solved = problem()->target()->reached(roadmap());
120 if (solved) {
121 hppDout(info, "tryConnectInitAndGoals succeeded");
122 }
123
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 8 if (interrupt_) throw path_planning_failed("Interruption");
124
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 8 times.
 32 while (!solved) {
125 // Check limits
126
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 std::ostringstream oss;
127
2/4
✓ Branch 0 taken 24 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 24 times.
 24 if (maxIterations_ != uint_infty && nIter >= maxIterations_)
128 // If the maximal nb of iterations is defined and reached
129 {
130 ✗ if (problem()->target()->reached(roadmap()))
131 // but a solution has been found
132 ✗ status = 1;
133 else {
134 // and no solution has been found
135 ✗ oss << "Maximal number of iterations reached: " << maxIterations_;
136 ✗ status = 2;
137 }
138 }
139
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 bpt::ptime timeStop(bpt::microsec_clock::universal_time());
140 value_type elapsed_ms =
141
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 24 static_cast<value_type>((timeStop - timeStart).total_milliseconds());
142
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 24 times.
 24 if (elapsed_ms > timeOut_ * 1000)
143 // If the time limit has been reached
144 {
145 ✗ if (problem()->target()->reached(roadmap()))
146 // but a solution has been found
147 ✗ status = 1;
148 else {
149 // and no solution has been found
150 ✗ oss << "time out (" << timeOut_ << "s) reached after "
151 ✗ << elapsed_ms * 1e-3 << "s";
152 ✗ status = 2;
153 }
154 }
155
156 // Check what to do
157
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 24 times.
 24 if (status == 1) break;
158
1/8
✗ Branch 0 not taken.
✓ Branch 1 taken 24 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 24 if (status == 2) throw path_planning_failed(oss.str().c_str());
159
1/2
✓ Branch 0 taken 24 times.
✗ Branch 1 not taken.
 24 if (status == 0) { // Execute one step
160 hppStartBenchmark(ONE_STEP);
161
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 oneStep();
162 hppStopBenchmark(ONE_STEP);
163 hppDisplayBenchmark(ONE_STEP);
164
165 // Check if problem is solved.
166 24 ++nIter;
167 24 solved =
168
7/14
✓ Branch 0 taken 24 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 24 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 24 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 7 times.
✓ Branch 15 taken 17 times.
✓ Branch 16 taken 24 times.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 24 stopWhenProblemIsSolved_ && problem()->target()->reached(roadmap());
169 }
170
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 24 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 24 if (interrupt_) throw path_planning_failed("Interruption");
171
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 }
172
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 PathVectorPtr_t planned = computePath();
173
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 return finishSolve(planned);
174 8 }
175
176 ✗ void PathPlanner::interrupt() { interrupt_ = true; }
177
178 7 void PathPlanner::maxIterations(const unsigned long int& n) {
179
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 7 if (!stopWhenProblemIsSolved_ && n == uint_infty && timeOut_ == float_infty)
180 ✗ throw std::invalid_argument(
181 "stopWhenProblemIsSolved is disabled so "
182 ✗ "maxIterations or timeOut must be finite.");
183 7 maxIterations_ = n;
184 7 }
185
186 7 void PathPlanner::timeOut(const double& time) {
187
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 7 if (!stopWhenProblemIsSolved_ && maxIterations_ == uint_infty &&
188 ✗ time == float_infty)
189 ✗ throw std::invalid_argument(
190 "stopWhenProblemIsSolved is disabled so "
191 ✗ "maxIterations or timeOut must be finite.");
192 7 timeOut_ = time;
193 7 }
194
195 ✗ void PathPlanner::stopWhenProblemIsSolved(bool enable) {
196 ✗ if (!enable && maxIterations_ == uint_infty && timeOut_ == float_infty)
197 ✗ throw std::invalid_argument(
198 "Disabling stopWhenProblemIsSolved is only "
199 ✗ "possible when maxIterations or timeOut are set to a finite value.");
200 ✗ stopWhenProblemIsSolved_ = enable;
201 }
202
203 8 PathVectorPtr_t PathPlanner::computePath() const {
204
2/4
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
 16 return problem()->target()->computePath(roadmap());
205 }
206
207 8 PathVectorPtr_t PathPlanner::finishSolve(const PathVectorPtr_t& path) {
208 8 return path;
209 }
210
211 7 void PathPlanner::tryConnectInitAndGoals() {
212 // call steering method here to build a direct conexion
213
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 const SteeringMethodPtr_t& sm(problem()->steeringMethod());
214
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 PathValidationPtr_t pathValidation(problem()->pathValidation());
215
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 PathProjectorPtr_t pathProjector(problem()->pathProjector());
216 7 PathPtr_t validPath, projPath, path;
217
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 NodePtr_t initNode = roadmap()->initNode();
218
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 NearestNeighborPtr_t nn(roadmap()->nearestNeighbor());
219 // Register edges to add to roadmap and add them after iterating
220 // among the connected components.
221 typedef std::tuple<NodePtr_t, NodePtr_t, PathPtr_t> FutureEdge_t;
222 typedef std::vector<FutureEdge_t> FutureEdges_t;
223 7 FutureEdges_t futureEdges;
224
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ConnectedComponentPtr_t initCC(initNode->connectedComponent());
225 7 for (ConnectedComponents_t::iterator itCC(
226
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 roadmap()->connectedComponents().begin());
227
4/6
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 21 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 14 times.
✓ Branch 11 taken 7 times.
 21 itCC != roadmap()->connectedComponents().end(); ++itCC) {
228
2/2
✓ Branch 2 taken 7 times.
✓ Branch 3 taken 7 times.
 14 if (*itCC != initCC) {
229 value_type d;
230
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 NodePtr_t near(nn->search(initNode->configuration(), *itCC, d, true));
231
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
 7 assert(near);
232
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 Configuration_t q1(initNode->configuration());
233
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 Configuration_t q2(near->configuration());
234
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 path = (*sm)(q1, q2);
235
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
 7 if (!path) continue;
236
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
 7 if (pathProjector) {
237 ✗ if (!pathProjector->apply(path, projPath)) continue;
238 } else {
239 7 projPath = path;
240 }
241
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 if (projPath) {
242 7 PathValidationReportPtr_t report;
243 bool pathValid =
244
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 pathValidation->validate(projPath, false, validPath, report);
245
2/8
✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 7 times.
 7 if (pathValid && validPath->length() > 0) {
246 ✗ futureEdges.push_back(FutureEdge_t(initNode, near, projPath));
247 }
248 7 }
249
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 }
250 }
251
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 for (NodeVector_t::const_iterator itn = roadmap()->goalNodes().begin();
252
3/4
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 7 times.
✓ Branch 9 taken 7 times.
 14 itn != roadmap()->goalNodes().end(); ++itn) {
253
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 ConnectedComponentPtr_t goalCC((*itn)->connectedComponent());
254 7 for (ConnectedComponents_t::iterator itCC(
255
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 roadmap()->connectedComponents().begin());
256
4/6
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 21 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 14 times.
✓ Branch 11 taken 7 times.
 21 itCC != roadmap()->connectedComponents().end(); ++itCC) {
257
2/2
✓ Branch 2 taken 7 times.
✓ Branch 3 taken 7 times.
 14 if (*itCC != goalCC) {
258 value_type d;
259
3/6
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
 7 NodePtr_t near(nn->search((*itn)->configuration(), *itCC, d, false));
260
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
 7 assert(near);
261
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 Configuration_t q1(near->configuration());
262
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Configuration_t q2((*itn)->configuration());
263
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 path = (*sm)(q1, q2);
264
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
 7 if (!path) continue;
265
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
 7 if (pathProjector) {
266 ✗ if (!pathProjector->apply(path, projPath)) continue;
267 } else {
268 7 projPath = path;
269 }
270
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 if (projPath) {
271 7 PathValidationReportPtr_t report;
272 bool pathValid =
273
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 pathValidation->validate(projPath, false, validPath, report);
274
2/8
✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 7 times.
 7 if (pathValid && validPath->length() > 0) {
275 ✗ futureEdges.push_back(FutureEdge_t(near, (*itn), projPath));
276 }
277 7 }
278
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 }
279 }
280 7 }
281 // Add edges
282
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
 7 for (const auto& e : futureEdges)
283 ✗ roadmap()->addEdge(std::get<0>(e), std::get<1>(e), std::get<2>(e));
284 7 }
285
286 } // namespace core
287 } // namespace hpp
288