LightningRetrieveRepair.h
80 LightningRetrieveRepair(const base::SpaceInformationPtr &si, tools::LightningDBPtr experienceDB);
127 bool repairPath(const base::PlannerTerminationCondition &ptc, geometric::PathGeometric &primaryPath);
136 bool replan(const base::State *start, const base::State *goal, geometric::PathGeometric &newPathSegment,
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
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
std::size_t nearestPathsChosenID_
the ID within nearestPaths_ of the path that was chosen for repair
Definition: LightningRetrieveRepair.h:212
geometric::PathSimplifierPtr psk_
The instance of the path simplifier.
Definition: LightningRetrieveRepair.h:224
base::PlannerPtr repairPlanner_
A secondary planner for replanning.
Definition: LightningRetrieveRepair.h:215
base::PlannerDataPtr getChosenRecallPath() const
Get the chosen path used from database for repair.
Definition: LightningRetrieveRepair.cpp:572
std::vector< base::PlannerDataPtr > repairPlannerDatas_
Debug the repair planner by saving its planner data each time it is used.
Definition: LightningRetrieveRepair.h:221
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
std::vector< base::PlannerDataPtr > nearestPaths_
Recall the nearest paths and store this in planner data for introspection later.
Definition: LightningRetrieveRepair.h:209
int nearestK_
Number of 'k' close solutions to choose from database for further filtering.
Definition: LightningRetrieveRepair.h:227
void setNumNearestSolutions(int nearestK)
Setter for number of 'k' close solutions to choose from database for further filtering.
Definition: LightningRetrieveRepair.h:182
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:127
tools::LightningDBPtr experienceDB_
The database of motions to search through.
Definition: LightningRetrieveRepair.h:206
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:112
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
base::ProblemDefinitionPtr repairProblemDef_
A secondary problem definition for the repair planner to use.
Definition: LightningRetrieveRepair.h:218
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LightningRetrieveRepair.cpp:67
int getNumNearestSolutions() const
Getter for number of 'k' close solutions to choose from database for further filtering.
Definition: LightningRetrieveRepair.h:174