ThunderRetrieveRepair.cpp
110 OMPL_DEBUG("No repairing planner specified. Using default: %s", repair_planner->getName().c_str());
114 repairProblemDef_->setOptimizationObjective(pdef_->getOptimizationObjective()); // copy primary problem def
152 if (!experienceDB_->findNearestStartGoal(nearestK_, startState, goalState, candidateSolution, ptc))
162 // All save trajectories should be at least 2 states long, then we append the start and goal states, for min
172 // ompl::geometric::PathGeometric pg = candidateSolution.getGeometricPath(); // TODO do not copy to new
189 bool ThunderRetrieveRepair::repairPath(const base::PlannerTerminationCondition &ptc, PathGeometric &primaryPath)
282 // Note: skip first and last states because they should be same as our start and goal state, same as
287 OMPL_DEBUG("Inserting newPathSegment state %d into old path at position %d", i, insertLocation);
291 // primaryPathStates.insert( primaryPathStates.begin() + toID, newPathSegment.getStates().begin(),
329 // TODO: if we use replanner like RRT* the ptc will allow it to run too long and no time will be left for
337 OMPL_WARN("Replan Solve: No replan solution between disconnected states found after %f seconds",
372 repairPlannerDatas_.back()->decoupleFromPlanner(); // copy states so that when planner unloads/clears we
384 OMPL_INFORM("ThunderRetrieveRepair getPlannerData: including %d similar paths", nearestPaths_.size());
412 void ThunderRetrieveRepair::getRepairPlannerDatas(std::vector<base::PlannerDataPtr> &data) const
417 std::size_t ThunderRetrieveRepair::checkMotionScore(const base::State *s1, const base::State *s2) const
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again.
Definition: Planner.cpp:180
void freeMemory()
Free the memory allocated by this planner.
Definition: ThunderRetrieveRepair.cpp:122
ThunderRetrieveRepair(const base::SpaceInformationPtr &si, tools::ThunderDBPtr experienceDB)
Constructor.
Definition: ThunderRetrieveRepair.cpp:55
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: ThunderRetrieveRepair.cpp:76
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: ThunderRetrieveRepair.cpp:417
void setRepairPlanner(const base::PlannerPtr &planner)
Set the planner that will be used for repairing invalid paths recalled from experience.
Definition: ThunderRetrieveRepair.cpp:91
void setExperienceDB(const tools::ThunderDBPtr &experienceDB)
Pass a pointer of the database from the thunder framework.
Definition: ThunderRetrieveRepair.cpp:86
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: ThunderRetrieveRepair.cpp:126
const PathGeometric & getChosenRecallPath() const
Get the chosen path used from database for repair.
Definition: ThunderRetrieveRepair.cpp:407
base::ProblemDefinitionPtr repairProblemDef_
A secondary problem definition for the repair planner to use.
Definition: ThunderRetrieveRepair.h:184
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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:237
tools::ThunderDBPtr experienceDB_
The database of motions to search through.
Definition: ThunderRetrieveRepair.h:172
std::size_t nearestPathsChosenID_
the ID within nearestPaths_ of the path that was chosen for repair
Definition: ThunderRetrieveRepair.h:178
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:237
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 replan(const base::State *start, const base::State *goal, PathGeometric &newPathSegment, const base::PlannerTerminationCondition &ptc)
Use our secondary planner to find a valid path between start and goal, and return that path.
Definition: ThunderRetrieveRepair.cpp:307
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
bool repairPath(const base::PlannerTerminationCondition &ptc, PathGeometric &primaryPath)
Repairs a path to be valid in the current planning environment.
Definition: ThunderRetrieveRepair.cpp:189
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available....
Definition: Planner.cpp:274
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
Definition: PathGeometric.h:255
std::size_t getLastRecalledNearestPathChosen() const
Get debug information about the top recalled paths that were chosen for further filtering.
Definition: ThunderRetrieveRepair.cpp:402
PathSimplifierPtr path_simplifier_
The instance of the path simplifier.
Definition: ThunderRetrieveRepair.h:190
int nearestK_
Number of 'k' close solutions to choose from database for further filtering.
Definition: ThunderRetrieveRepair.h:193
base::PlannerPtr repairPlanner_
A secondary planner for replanning.
Definition: ThunderRetrieveRepair.h:181
std::vector< PathGeometric > nearestPaths_
Recall the nearest paths and store this in planner data for introspection later.
Definition: ThunderRetrieveRepair.h:175
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: ThunderRetrieveRepair.cpp:99
void getPlannerData(base::PlannerData &data) const override
Get information about the exploration data structure the planning from scratch motion planner used.
Definition: ThunderRetrieveRepair.cpp:382
std::vector< base::PlannerDataPtr > repairPlannerDatas_
Debug the repair planner by saving its planner data each time it is used.
Definition: ThunderRetrieveRepair.h:187
const std::vector< PathGeometric > & getLastRecalledNearestPaths() const
Get debug information about the top recalled paths that were chosen for further filtering.
Definition: ThunderRetrieveRepair.cpp:397
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:202
bool smoothingEnabled_
Optionally smooth retrieved and repaired paths from database.
Definition: ThunderRetrieveRepair.h:196
void getRepairPlannerDatas(std::vector< base::PlannerDataPtr > &data) const
Get information about the exploration data structure the repair motion planner used each call.
Definition: ThunderRetrieveRepair.cpp:412
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
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:64
base::State * getState(unsigned int index)
Get the state located at index along the path.
Definition: PathGeometric.h:243
Struct for passing around partially solved solutions.
Definition: SPARSdb.h:234