LightningRetrieveRepair.cpp
50 ompl::geometric::LightningRetrieveRepair::LightningRetrieveRepair(const base::SpaceInformationPtr &si,
76 void ompl::geometric::LightningRetrieveRepair::setLightningDB(const ompl::tools::LightningDBPtr &experienceDB)
84 throw Exception("LightningRetrieveRepair: Repair planner instance does not match space information");
104 repairProblemDef_->setOptimizationObjective(pdef_->getOptimizationObjective()); // copy primary problem def
112 ompl::base::PlannerStatus ompl::geometric::LightningRetrieveRepair::solve(const base::PlannerTerminationCondition &ptc)
119 OMPL_INFORM("LightningRetrieveRepair: Experience database is empty so unable to run LightningRetrieveRepair "
144 OMPL_INFORM("LightningRetrieveRepair: No similar path founds in nearest neighbor tree, unable to retrieve "
152 // TODO Rather than selecting 1 best path, you could also spawn n (n<=k) threads and repair the top n paths.
173 // All save trajectories should be at least 2 states long, and then we append the start and goal states
189 OMPL_INFORM("LightningRetrieveRepair: Path simplification took %f seconds and removed %d states", simplifyTime,
198 bool ompl::geometric::LightningRetrieveRepair::findBestPath(const base::State *startState, const base::State *goalState,
201 OMPL_INFORM("LightningRetrieveRepair: Found %d similar paths. Filtering", nearestPaths_.size());
220 OMPL_ERROR("A path was recalled that somehow has less than 2 vertices, which shouldn't happen");
225 const ompl::base::State *pathGoalState = currentPath->getVertex(currentPath->numVertices() - 1).getState();
227 double regularDistance = si_->distance(startState, pathStartState) + si_->distance(goalState, pathGoalState);
228 double reversedDistance = si_->distance(startState, pathGoalState) + si_->distance(goalState, pathStartState);
236 // We won't actually flip it until later to save memory operations and not alter our NN tree in the
275 OMPL_INFORM("LightningRetrieveRepair: Path %d | %d vertices | %d invalid | score %d | reversed: %s | "
283 OMPL_DEBUG("LightningRetrieveRepair: --> The shortest path (path 0) has a perfect score (0), ending "
294 OMPL_DEBUG("LightningRetrieveRepair: --> This path is the best we've seen so far. Previous best: %d",
305 OMPL_DEBUG("LightningRetrieveRepair: --> This path is as good as the best we've seen so far, but its path "
313 OMPL_DEBUG("LightningRetrieveRepair: --> Not best. Best score: %d from index %d", bestPathScore,
325 OMPL_ERROR("LightningRetrieveRepair: Only %d vertices found in PlannerData loaded from file. This is a bug.",
330 // Reverse the path if necessary. We allocate memory for this so that we don't alter the database
333 OMPL_DEBUG("LightningRetrieveRepair: Reversing planner data vertices count %d", bestPath->numVertices());
335 for (std::size_t i = bestPath->numVertices(); i > 0; --i) // size_t can't go negative so subtract 1 instead
352 bool ompl::geometric::LightningRetrieveRepair::repairPath(const base::PlannerTerminationCondition &ptc,
377 OMPL_DEBUG("LightningRetrieveRepair: Repair path function interrupted because termination condition is "
389 OMPL_DEBUG("LightningRetrieveRepair: Searching for next valid state, because state %d to %d was not valid "
397 OMPL_DEBUG("LightningRetrieveRepair: State %d was found to valid, we can now repair between states",
409 // We never found a valid state to plan to, instead we reached the goal state and it too wasn't valid.
412 OMPL_ERROR("LightningRetrieveRepair: No state was found valid in the remainder of the path. Invalid "
425 OMPL_INFORM("LightningRetrieveRepair: Unable to repair path between state %d and %d", fromID, toID);
444 OMPL_DEBUG("LightningRetrieveRepair: Inserting new %d states into old path. Previous length: %d",
447 // Note: skip first and last states because they should be same as our start and goal state, same as
452 OMPL_DEBUG("LightningRetrieveRepair: Inserting newPathSegment state %d into old path at position %d", i,
460 // Set the toID to jump over the newly inserted states to the next unchecked state. Subtract 2 because we
472 bool ompl::geometric::LightningRetrieveRepair::replan(const ompl::base::State *start, const ompl::base::State *goal,
501 OMPL_INFORM("LightningRetrieveRepair: No replan solution between disconnected states found after %f seconds",
530 OMPL_INFORM("LightningRetrieveRepair: Path simplification took %f seconds and removed %d states", simplifyTime,
536 repairPlannerDatas_.back()->decoupleFromPlanner(); // copy states so that when planner unloads/clears we don't lose
572 ompl::base::PlannerDataPtr ompl::geometric::LightningRetrieveRepair::getChosenRecallPath() const
577 void ompl::geometric::LightningRetrieveRepair::getRepairPlannerDatas(std::vector<base::PlannerDataPtr> &data) const
582 std::size_t ompl::geometric::LightningRetrieveRepair::checkMotionScore(const ompl::base::State *s1,
void setLightningDB(const tools::LightningDBPtr &experienceDB)
Pass a pointer of the database from the lightning framework.
Definition: LightningRetrieveRepair.cpp:76
std::size_t getLastRecalledNearestPathChosen() const
Get debug information about the top recalled paths that were chosen for further filtering.
Definition: LightningRetrieveRepair.cpp:567
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:188
void setRepairPlanner(const base::PlannerPtr &planner)
Set the planner that will be used for repairing invalid paths recalled from experience.
Definition: LightningRetrieveRepair.cpp:81
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...
Definition: LightningRetrieveRepair.cpp:198
void getPlannerData(base::PlannerData &data) const override
Get information about the exploration data structure the planning from scratch motion planner used.
Definition: LightningRetrieveRepair.cpp:546
geometric::PathSimplifierPtr psk_
The instance of the path simplifier.
Definition: LightningRetrieveRepair.h:224
base::PlannerDataPtr getChosenRecallPath() const
Get the chosen path used from database for repair.
Definition: LightningRetrieveRepair.cpp:572
A shared pointer wrapper for ompl::base::PlannerData.
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 ...
Definition: LightningRetrieveRepair.cpp:582
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
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...
Definition: PathGeometric.h:274
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:127
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:269
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:112
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
Definition: PathGeometric.h:292
const std::vector< base::PlannerDataPtr > & getLastRecalledNearestPaths() const
Get debug information about the top recalled paths that were chosen for further filtering.
Definition: LightningRetrieveRepair.cpp:562
bool repairPath(const base::PlannerTerminationCondition &ptc, geometric::PathGeometric &primaryPath)
Repairs a path to be valid in the current planning environment.
Definition: LightningRetrieveRepair.cpp:352
void getRepairPlannerDatas(std::vector< base::PlannerDataPtr > &data) const
Get information about the exploration data structure the repair motion planner used each call.
Definition: LightningRetrieveRepair.cpp:577
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.
Definition: LightningRetrieveRepair.cpp:472
LightningRetrieveRepair(const base::SpaceInformationPtr &si, tools::LightningDBPtr experienceDB)
Constructor.
Definition: LightningRetrieveRepair.cpp:50
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LightningRetrieveRepair.cpp:89
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...
Definition: LightningRetrieveRepair.cpp:112
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
base::ProblemDefinitionPtr repairProblemDef_
A secondary problem definition for the repair planner to use.
Definition: LightningRetrieveRepair.h:218
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...
Definition: PlannerData.cpp:432
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LightningRetrieveRepair.cpp:67
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:128
base::State * getState(unsigned int index)
Get the state located at index along the path.
Definition: PathGeometric.h:280