Loading...
Searching...
No Matches
LightningRetrieveRepair.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, University of Colorado, Boulder
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Univ of CO, Boulder nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Dave Coleman */
36
37#include "ompl/geometric/planners/experience/LightningRetrieveRepair.h"
38#include "ompl/geometric/planners/rrt/RRTConnect.h"
39#include "ompl/base/goals/GoalState.h"
40#include "ompl/base/goals/GoalSampleableRegion.h"
41#include "ompl/tools/config/SelfConfig.h"
42#include "ompl/tools/config/MagicConstants.h"
43#include "ompl/tools/lightning/LightningDB.h"
44
45#include <thread>
46
47#include <limits>
48#include <utility>
49
51 ompl::tools::LightningDBPtr experienceDB)
52 : base::Planner(si, "LightningRetrieveRepair")
53 , experienceDB_(std::move(experienceDB))
54 , nearestK_(ompl::magic::NEAREST_K_RECALL_SOLUTIONS) // default value
55{
57 specs_.directed = true;
58
59 // Repair Planner Specific:
60 repairProblemDef_ = std::make_shared<base::ProblemDefinition>(si_);
61
62 psk_ = std::make_shared<PathSimplifier>(si_);
63}
64
65ompl::geometric::LightningRetrieveRepair::~LightningRetrieveRepair() = default;
66
68{
69 Planner::clear();
70
71 // Clear the inner planner
72 if (repairPlanner_)
73 repairPlanner_->clear();
74}
75
76void ompl::geometric::LightningRetrieveRepair::setLightningDB(const ompl::tools::LightningDBPtr &experienceDB)
77{
78 experienceDB_ = experienceDB;
79}
80
82{
83 if (planner && planner->getSpaceInformation().get() != si_.get())
84 throw Exception("LightningRetrieveRepair: Repair planner instance does not match space information");
85 repairPlanner_ = planner;
86 setup_ = false;
87}
88
90{
91 Planner::setup();
92
93 // Setup repair planner (for use by the rrPlanner)
94 // Note: does not use the same pdef as the main planner in this class
95 if (!repairPlanner_)
96 {
97 // Set the repair planner
98 repairPlanner_ = std::make_shared<RRTConnect>(si_);
99 OMPL_DEBUG("LightningRetrieveRepair: No repairing planner specified. Using default: %s",
100 repairPlanner_->getName().c_str());
101 }
102
103 // Setup the problem definition for the repair planner
104 repairProblemDef_->setOptimizationObjective(pdef_->getOptimizationObjective()); // copy primary problem def
105
106 // Setup repair planner
107 repairPlanner_->setProblemDefinition(repairProblemDef_);
108 if (!repairPlanner_->isSetup())
109 repairPlanner_->setup();
110}
111
113{
114 bool solved = false;
115
116 // Check if the database is empty
117 if (experienceDB_->getExperiencesCount() == 0u)
118 {
119 OMPL_INFORM("LightningRetrieveRepair: Experience database is empty so unable to run LightningRetrieveRepair "
120 "algorithm.");
122 }
123
124 // Restart the Planner Input States so that the first start and goal state can be fetched
125 pis_.restart();
126
127 // Get a single start state TODO: more than one
128 const base::State *startState = pis_.nextStart();
129 const base::State *goalState = pis_.nextGoal(ptc);
130
131 // Error check start/goal states
132 if ((startState == nullptr) || (goalState == nullptr))
133 {
134 OMPL_ERROR("LightningRetrieveRepair: Start or goal states are null");
136 }
137
138 // Search for previous solution in database
139 nearestPaths_ = experienceDB_->findNearestStartGoal(nearestK_, startState, goalState);
140
141 // Check if there are any solutions
142 if (nearestPaths_.empty())
143 {
144 OMPL_INFORM("LightningRetrieveRepair: No similar path founds in nearest neighbor tree, unable to retrieve "
145 "repair");
146 return base::PlannerStatus::TIMEOUT; // The planner failed to find a solution
147 }
148
150
151 // Filter top n paths to 1
152 // TODO Rather than selecting 1 best path, you could also spawn n (n<=k) threads and repair the top n paths.
153 if (!findBestPath(startState, goalState, chosenPath))
154 {
156 }
157
158 // All saved trajectories should be at least 2 states long
159 assert(chosenPath->numVertices() >= 2);
160
161 // Convert chosen PlannerData experience to an actual path
162 auto primaryPath(std::make_shared<PathGeometric>(si_));
163 // Add start
164 primaryPath->append(startState);
165 // Add old states
166 for (std::size_t i = 0; i < chosenPath->numVertices(); ++i)
167 {
168 primaryPath->append(chosenPath->getVertex(i).getState());
169 }
170 // Add goal
171 primaryPath->append(goalState);
172
173 // All save trajectories should be at least 2 states long, and then we append the start and goal states
174 assert(primaryPath->getStateCount() >= 4);
175
176 // Repair chosen path
177 if (!repairPath(ptc, *primaryPath))
178 {
179 OMPL_INFORM("LightningRetrieveRepair: repairPath failed or aborted");
181 }
182
183 // Smooth the result
184 OMPL_INFORM("LightningRetrieveRepair solve: Simplifying solution (smoothing)...");
185 time::point simplifyStart = time::now();
186 std::size_t numStates = primaryPath->getStateCount();
187 psk_->simplify(*primaryPath, ptc);
188 double simplifyTime = time::seconds(time::now() - simplifyStart);
189 OMPL_INFORM("LightningRetrieveRepair: Path simplification took %f seconds and removed %d states", simplifyTime,
190 numStates - primaryPath->getStateCount());
191
192 // Finished
193 pdef_->addSolutionPath(primaryPath, false, 0., getName());
194 solved = true;
195 return {solved, false};
196}
197
199 ompl::base::PlannerDataPtr &chosenPath)
200{
201 OMPL_INFORM("LightningRetrieveRepair: Found %d similar paths. Filtering", nearestPaths_.size());
202
203 // Filter down to just 1 chosen path
204 ompl::base::PlannerDataPtr bestPath = nearestPaths_.front();
205 std::size_t bestPathScore = std::numeric_limits<std::size_t>::max();
206
207 // Track which path has the shortest distance
208 std::vector<double> distances(nearestPaths_.size(), 0);
209 std::vector<bool> isReversed(nearestPaths_.size());
210
211 assert(isReversed.size() == nearestPaths_.size());
212
213 for (std::size_t pathID = 0; pathID < nearestPaths_.size(); ++pathID)
214 {
215 const ompl::base::PlannerDataPtr &currentPath = nearestPaths_[pathID];
216
217 // Error check
218 if (currentPath->numVertices() < 2) // needs at least a start and a goal
219 {
220 OMPL_ERROR("A path was recalled that somehow has less than 2 vertices, which shouldn't happen");
221 return false;
222 }
223
224 const ompl::base::State *pathStartState = currentPath->getVertex(0).getState();
225 const ompl::base::State *pathGoalState = currentPath->getVertex(currentPath->numVertices() - 1).getState();
226
227 double regularDistance = si_->distance(startState, pathStartState) + si_->distance(goalState, pathGoalState);
228 double reversedDistance = si_->distance(startState, pathGoalState) + si_->distance(goalState, pathStartState);
229
230 // Check if path is reversed from normal [start->goal] direction and cache the distance
231 if (regularDistance > reversedDistance)
232 {
233 // The distance between starts and goals is less when in reverse
234 isReversed[pathID] = true;
235 distances[pathID] = reversedDistance;
236 // We won't actually flip it until later to save memory operations and not alter our NN tree in the
237 // LightningDB
238 }
239 else
240 {
241 isReversed[pathID] = false;
242 distances[pathID] = regularDistance;
243 }
244
245 std::size_t pathScore = 0; // the score
246
247 // Check the validity between our start location and the path's start
248 // TODO: this might bias the score to be worse for the little connecting segment
249 if (!isReversed[pathID])
250 pathScore += checkMotionScore(startState, pathStartState);
251 else
252 pathScore += checkMotionScore(startState, pathGoalState);
253
254 // Score current path for validity
255 std::size_t invalidStates = 0;
256 for (std::size_t vertex_id = 0; vertex_id < currentPath->numVertices(); ++vertex_id)
257 {
258 // Check if the sampled points are valid
259 if (!si_->isValid(currentPath->getVertex(vertex_id).getState()))
260 {
261 invalidStates++;
262 }
263 }
264 // Track separate for debugging
265 pathScore += invalidStates;
266
267 // Check the validity between our goal location and the path's goal
268 // TODO: this might bias the score to be worse for the little connecting segment
269 if (!isReversed[pathID])
270 pathScore += checkMotionScore(goalState, pathGoalState);
271 else
272 pathScore += checkMotionScore(goalState, pathStartState);
273
274 // Factor in the distance between start/goal and our new start/goal
275 OMPL_INFORM("LightningRetrieveRepair: Path %d | %d verticies | %d invalid | score %d | reversed: %s | "
276 "distance: %f",
277 int(pathID), currentPath->numVertices(), invalidStates, pathScore,
278 isReversed[pathID] ? "true" : "false", distances[pathID]);
279
280 // Check if we have a perfect score (0) and this is the shortest path (the first one)
281 if (pathID == 0 && pathScore == 0)
282 {
283 OMPL_DEBUG("LightningRetrieveRepair: --> The shortest path (path 0) has a perfect score (0), ending "
284 "filtering early.");
285 bestPathScore = pathScore;
286 bestPath = currentPath;
287 nearestPathsChosenID_ = pathID;
288 break; // end the for loop
289 }
290
291 // Check if this is the best score we've seen so far
292 if (pathScore < bestPathScore)
293 {
294 OMPL_DEBUG("LightningRetrieveRepair: --> This path is the best we've seen so far. Previous best: %d",
295 bestPathScore);
296 bestPathScore = pathScore;
297 bestPath = currentPath;
298 nearestPathsChosenID_ = pathID;
299 }
300 // if the best score is the same as a previous one we've seen,
301 // choose the one that has the shortest connecting component
302 else if (pathScore == bestPathScore && distances[nearestPathsChosenID_] > distances[pathID])
303 {
304 // This new path is a shorter distance
305 OMPL_DEBUG("LightningRetrieveRepair: --> This path is as good as the best we've seen so far, but its path "
306 "is shorter. Previous best score: %d from index %d",
307 bestPathScore, nearestPathsChosenID_);
308 bestPathScore = pathScore;
309 bestPath = currentPath;
310 nearestPathsChosenID_ = pathID;
311 }
312 else
313 OMPL_DEBUG("LightningRetrieveRepair: --> Not best. Best score: %d from index %d", bestPathScore,
314 nearestPathsChosenID_);
315 }
316
317 // Check if we have a solution
318 if (!bestPath)
319 {
320 OMPL_ERROR("LightningRetrieveRepair: No best path found from k filtered paths");
321 return false;
322 }
323 if ((bestPath->numVertices() == 0u) || bestPath->numVertices() == 1)
324 {
325 OMPL_ERROR("LightningRetrieveRepair: Only %d verticies found in PlannerData loaded from file. This is a bug.",
326 bestPath->numVertices());
327 return false;
328 }
329
330 // Reverse the path if necessary. We allocate memory for this so that we don't alter the database
331 if (isReversed[nearestPathsChosenID_])
332 {
333 OMPL_DEBUG("LightningRetrieveRepair: Reversing planner data verticies count %d", bestPath->numVertices());
334 auto newPath(std::make_shared<ompl::base::PlannerData>(si_));
335 for (std::size_t i = bestPath->numVertices(); i > 0; --i) // size_t can't go negative so subtract 1 instead
336 {
337 newPath->addVertex(bestPath->getVertex(i - 1));
338 }
339 // Set result
340 chosenPath = newPath;
341 }
342 else
343 {
344 // Set result
345 chosenPath = bestPath;
346 }
347 OMPL_DEBUG("LightningRetrieveRepair: Done Filtering\n");
348
349 return true;
350}
351
354{
355 // \todo: we should reuse our collision checking from the previous step to make this faster
356
357 OMPL_INFORM("LightningRetrieveRepair: Repairing path");
358
359 // Error check
360 if (primaryPath.getStateCount() < 2)
361 {
362 OMPL_ERROR("LightningRetrieveRepair: Cannot repair a path with less than 2 states");
363 return false;
364 }
365
366 // Loop through every pair of states and make sure path is valid.
367 // If not, replan between those states
368 for (std::size_t toID = 1; toID < primaryPath.getStateCount(); ++toID)
369 {
370 std::size_t fromID = toID - 1; // this is our last known valid state
371 ompl::base::State *fromState = primaryPath.getState(fromID);
372 ompl::base::State *toState = primaryPath.getState(toID);
373
374 // Check if our planner is out of time
375 if (ptc)
376 {
377 OMPL_DEBUG("LightningRetrieveRepair: Repair path function interrupted because termination condition is "
378 "true.");
379 return false;
380 }
381
382 // Check path between states
383 if (!si_->checkMotion(fromState, toState))
384 {
385 // Path between (from, to) states not valid, but perhaps to STATE is
386 // Search until next valid STATE is found in existing path
387 std::size_t subsearchID = toID;
388 ompl::base::State *new_to;
389 OMPL_DEBUG("LightningRetrieveRepair: Searching for next valid state, because state %d to %d was not valid "
390 "out %d total states",
391 fromID, toID, primaryPath.getStateCount());
392 while (subsearchID < primaryPath.getStateCount())
393 {
394 new_to = primaryPath.getState(subsearchID);
395 if (si_->isValid(new_to))
396 {
397 OMPL_DEBUG("LightningRetrieveRepair: State %d was found to valid, we can now repair between states",
398 subsearchID);
399 // This future state is valid, we can stop searching
400 toID = subsearchID;
401 toState = new_to;
402 break;
403 }
404 ++subsearchID; // keep searching for a new state to plan to
405 }
406 // Check if we ever found a next state that is valid
407 if (subsearchID >= primaryPath.getStateCount())
408 {
409 // We never found a valid state to plan to, instead we reached the goal state and it too wasn't valid.
410 // This is bad.
411 // I think this is a bug.
412 OMPL_ERROR("LightningRetrieveRepair: No state was found valid in the remainder of the path. Invalid "
413 "goal state. This should not happen.");
414 return false;
415 }
416
417 // Plan between our two valid states
418 PathGeometric newPathSegment(si_);
419
420 // Not valid motion, replan
421 OMPL_DEBUG("LightningRetrieveRepair: Planning from %d to %d", fromID, toID);
422
423 if (!replan(fromState, toState, newPathSegment, ptc))
424 {
425 OMPL_INFORM("LightningRetrieveRepair: Unable to repair path between state %d and %d", fromID, toID);
426 return false;
427 }
428
429 // TODO make sure not approximate solution
430
431 // Reference to the path
432 std::vector<base::State *> &primaryPathStates = primaryPath.getStates();
433
434 // Remove all invalid states between (fromID, toID) - not including those states themselves
435 while (fromID != toID - 1)
436 {
437 OMPL_INFORM("LightningRetrieveRepair: Deleting state %d", fromID + 1);
438 primaryPathStates.erase(primaryPathStates.begin() + fromID + 1);
439 --toID; // because vector has shrunk
440 OMPL_INFORM("LightningRetrieveRepair: toID is now %d", toID);
441 }
442
443 // Insert new path segment into current path
444 OMPL_DEBUG("LightningRetrieveRepair: Inserting new %d states into old path. Previous length: %d",
445 newPathSegment.getStateCount() - 2, primaryPathStates.size());
446
447 // Note: skip first and last states because they should be same as our start and goal state, same as
448 // `fromID` and `toID`
449 for (std::size_t i = 1; i < newPathSegment.getStateCount() - 1; ++i)
450 {
451 std::size_t insertLocation = toID + i - 1;
452 OMPL_DEBUG("LightningRetrieveRepair: Inserting newPathSegment state %d into old path at position %d", i,
453 insertLocation);
454 primaryPathStates.insert(primaryPathStates.begin() + insertLocation,
455 si_->cloneState(newPathSegment.getStates()[i]));
456 }
457 OMPL_DEBUG("LightningRetrieveRepair: Inserted new states into old path. New length: %d",
458 primaryPathStates.size());
459
460 // Set the toID to jump over the newly inserted states to the next unchecked state. Subtract 2 because we
461 // ignore start and goal
462 toID = toID + newPathSegment.getStateCount() - 2;
463 OMPL_DEBUG("LightningRetrieveRepair: Continuing searching at state %d", toID);
464 }
465 }
466
467 OMPL_INFORM("LightningRetrieveRepair: Done repairing");
468
469 return true;
470}
471
473 PathGeometric &newPathSegment,
475{
476 // Reset problem definition
477 repairProblemDef_->clearSolutionPaths();
478 repairProblemDef_->clearStartStates();
479 repairProblemDef_->clearGoal();
480
481 // Reset planner
482 repairPlanner_->clear();
483
484 // Configure problem definition
485 repairProblemDef_->setStartAndGoalStates(start, goal);
486
487 // Configure planner
488 repairPlanner_->setProblemDefinition(repairProblemDef_);
489
490 // Solve
491 OMPL_INFORM("LightningRetrieveRepair: Preparing to repair path");
493 time::point startTime = time::now();
494
495 lastStatus = repairPlanner_->solve(ptc);
496
497 // Results
498 double planTime = time::seconds(time::now() - startTime);
499 if (!lastStatus)
500 {
501 OMPL_INFORM("LightningRetrieveRepair: No replan solution between disconnected states found after %f seconds",
502 planTime);
503 return false;
504 }
505
506 // Check if approximate
507 if (repairProblemDef_->hasApproximateSolution() ||
508 repairProblemDef_->getSolutionDifference() > std::numeric_limits<double>::epsilon())
509 {
510 OMPL_INFORM("LightningRetrieveRepair: Solution is approximate, not using");
511 return false;
512 }
513
514 // Convert solution into a PathGeometric path
515 base::PathPtr p = repairProblemDef_->getSolutionPath();
516 if (!p)
517 {
518 OMPL_ERROR("LightningRetrieveRepair: Unable to get solution path from problem definition");
519 return false;
520 }
521
522 newPathSegment = static_cast<ompl::geometric::PathGeometric &>(*p);
523
524 // Smooth the result
525 OMPL_INFORM("LightningRetrieveRepair: Simplifying solution (smoothing)...");
526 time::point simplifyStart = time::now();
527 std::size_t numStates = newPathSegment.getStateCount();
528 psk_->simplify(newPathSegment, ptc);
529 double simplifyTime = time::seconds(time::now() - simplifyStart);
530 OMPL_INFORM("LightningRetrieveRepair: Path simplification took %f seconds and removed %d states", simplifyTime,
531 numStates - newPathSegment.getStateCount());
532
533 // Save the planner data for debugging purposes
534 repairPlannerDatas_.push_back(std::make_shared<ompl::base::PlannerData>(si_));
535 repairPlanner_->getPlannerData(*repairPlannerDatas_.back());
536 repairPlannerDatas_.back()->decoupleFromPlanner(); // copy states so that when planner unloads/clears we don't lose
537 // them
538
539 // Return success
540 OMPL_INFORM("LightningRetrieveRepair: solution found in %f seconds with %d states", planTime,
541 newPathSegment.getStateCount());
542
543 return true;
544}
545
547{
548 OMPL_INFORM("LightningRetrieveRepair: including %d similar paths", nearestPaths_.size());
549
550 // Visualize the n candidate paths that we recalled from the database
551 for (const auto &pd : nearestPaths_)
552 {
553 for (std::size_t j = 1; j < pd->numVertices(); ++j)
554 {
555 data.addEdge(base::PlannerDataVertex(pd->getVertex(j - 1).getState()),
556 base::PlannerDataVertex(pd->getVertex(j).getState()));
557 }
558 }
559}
560
561const std::vector<ompl::base::PlannerDataPtr> &
563{
564 return nearestPaths_; // list of candidate paths
565}
566
568{
569 return nearestPathsChosenID_; // of the candidate paths list, the one we chose
570}
571
573{
574 return nearestPaths_[nearestPathsChosenID_];
575}
576
577void ompl::geometric::LightningRetrieveRepair::getRepairPlannerDatas(std::vector<base::PlannerDataPtr> &data) const
578{
579 data = repairPlannerDatas_;
580}
581
583 const ompl::base::State *s2) const
584{
585 int segmentCount = si_->getStateSpace()->validSegmentCount(s1, s2);
586
587 std::size_t invalidStatesScore = 0; // count number of interpolated states in collision
588
589 // temporary storage for the checked state
590 ompl::base::State *test = si_->allocState();
591
592 // Linerarly step through motion between state 0 to state 1
593 for (double location = 0.0; location <= 1.0; location += 1.0 / double(segmentCount))
594 {
595 si_->getStateSpace()->interpolate(s1, s2, location, test);
596
597 if (!si_->isValid(test))
598 {
599 invalidStatesScore++;
600 }
601 }
602 si_->freeState(test);
603
604 return invalidStatesScore;
605}
The exception type for ompl.
Definition: Exception.h:47
A shared pointer wrapper for ompl::base::PlannerData.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:429
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
Definition of an abstract state.
Definition: State.h:50
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
base::PlannerDataPtr getChosenRecallPath() const
Get the chosen path used from database for repair.
bool replan(const base::State *start, const base::State *goal, geometric::PathGeometric &newPathSegment, const base::PlannerTerminationCondition &ptc)
Use our secondary planner to find a valid path between start and goal, and return that path.
base::ProblemDefinitionPtr repairProblemDef_
A secondary problem definition for the repair planner to use.
void getRepairPlannerDatas(std::vector< base::PlannerDataPtr > &data) const
Get information about the exploration data structure the repair motion planner used each call.
const std::vector< base::PlannerDataPtr > & getLastRecalledNearestPaths() const
Get debug information about the top recalled paths that were chosen for further filtering.
void setRepairPlanner(const base::PlannerPtr &planner)
Set the planner that will be used for repairing invalid paths recalled from experience.
void setLightningDB(const tools::LightningDBPtr &experienceDB)
Pass a pointer of the database from the lightning framework.
LightningRetrieveRepair(const base::SpaceInformationPtr &si, tools::LightningDBPtr experienceDB)
Constructor.
bool findBestPath(const base::State *startState, const base::State *goalState, base::PlannerDataPtr &chosenPath)
Filters the top n paths in nearestPaths_ to the top 1, based on state validity with current environme...
void getPlannerData(base::PlannerData &data) const override
Get information about the exploration data structure the planning from scratch motion planner used.
bool repairPath(const base::PlannerTerminationCondition &ptc, geometric::PathGeometric &primaryPath)
Repairs a path to be valid in the current planning environment.
geometric::PathSimplifierPtr psk_
The instance of the path simplifier.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
std::size_t checkMotionScore(const base::State *s1, const base::State *s2) const
Count the number of states along the discretized path that are in collision Note: This is kind of an ...
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
std::size_t getLastRecalledNearestPathChosen() const
Get debug information about the top recalled paths that were chosen for further filtering.
Definition of a geometric path.
Definition: PathGeometric.h:66
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
base::State * getState(unsigned int index)
Get the state located at index along the path.
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:52
point now()
Get the current time point.
Definition: Time.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:64
Main namespace. Contains everything in this library.
STL namespace.
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:212
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:202
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ ABORT
The planner did not find a solution for some other reason.
Definition: PlannerStatus.h:70
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62
@ UNKNOWN
Uninitialized status.
Definition: PlannerStatus.h:54