Directory: | ./ |
---|---|
File: | src/path-planner.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 117 | 152 | 77.0% |
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 | 7 | PathPlanner::PathPlanner(const ProblemConstPtr_t& problem, | |
65 | 7 | const RoadmapPtr_t& roadmap) | |
66 | 7 | : problem_(problem), | |
67 | 7 | roadmap_(roadmap), | |
68 | 7 | interrupt_(false), | |
69 | 7 | maxIterations_(uint_infty), | |
70 | 7 | timeOut_(float_infty), | |
71 | 14 | stopWhenProblemIsSolved_(true) { | |
72 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
|
7 | assert(problem_.lock()); |
73 | 7 | } | |
74 | |||
75 | ✗ | PathPlanner::~PathPlanner() {} | |
76 | |||
77 | ✗ | void PathPlanner::init(const PathPlannerWkPtr_t& weak) { weakPtr_ = weak; } | |
78 | |||
79 | 377 | const RoadmapPtr_t& PathPlanner::roadmap() const { return roadmap_; } | |
80 | |||
81 | 360 | ProblemConstPtr_t PathPlanner::problem() const { return problem_.lock(); } | |
82 | |||
83 | 7 | void PathPlanner::startSolve() { | |
84 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
|
7 | problem()->checkProblem(); |
85 | // Tag init and goal configurations in the roadmap | ||
86 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | roadmap()->resetGoalNodes(); |
87 |
4/8✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
|
7 | roadmap()->initNode(problem()->initConfig()); |
88 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | problemTarget::GoalConfigurationsPtr_t gc(HPP_DYNAMIC_PTR_CAST( |
89 | 7 | problemTarget::GoalConfigurations, problem()->target())); | |
90 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (gc) { |
91 |
2/4✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
|
7 | const Configurations_t goals(gc->configurations()); |
92 | 7 | for (Configurations_t::const_iterator itGoal = goals.begin(); | |
93 |
2/2✓ Branch 2 taken 7 times.
✓ Branch 3 taken 7 times.
|
14 | itGoal != goals.end(); ++itGoal) { |
94 |
3/6✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
|
7 | roadmap()->addGoalNode(*itGoal); |
95 | } | ||
96 | 7 | } | |
97 |
3/6✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
|
7 | problem()->target()->check(roadmap()); |
98 | 7 | } | |
99 | |||
100 | 7 | PathVectorPtr_t PathPlanner::solve() { | |
101 | namespace bpt = boost::posix_time; | ||
102 | |||
103 | 7 | int status = 0; | |
104 | // 0 = execute one more step | ||
105 | // 1 = pb solved | ||
106 | // 2 = throw exception | ||
107 | |||
108 | 7 | interrupt_ = false; | |
109 | 7 | bool solved = false; | |
110 | 7 | unsigned long int nIter(0); | |
111 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | bpt::ptime timeStart(bpt::microsec_clock::universal_time()); |
112 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | startSolve(); |
113 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | 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 7 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
|
7 | solved = problem()->target()->reached(roadmap()); |
120 | if (solved) { | ||
121 | hppDout(info, "tryConnectInitAndGoals succeeded"); | ||
122 | } | ||
123 |
1/6✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
7 | if (interrupt_) throw path_planning_failed("Interruption"); |
124 |
2/2✓ Branch 0 taken 24 times.
✓ Branch 1 taken 7 times.
|
31 | 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 7 times.
✗ Branch 2 not taken.
|
7 | PathVectorPtr_t planned = computePath(); |
173 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | return finishSolve(planned); |
174 | 7 | } | |
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 | 7 | PathVectorPtr_t PathPlanner::computePath() const { | |
204 |
2/4✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
|
14 | return problem()->target()->computePath(roadmap()); |
205 | } | ||
206 | |||
207 | 7 | PathVectorPtr_t PathPlanner::finishSolve(const PathVectorPtr_t& path) { | |
208 | 7 | 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 |