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
std::vector< base::PlannerDataPtr > repairPlannerDatas_
Debug the repair planner by saving its planner data each time it is used.
Definition: ThunderRetrieveRepair.h:187
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:203
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
tools::ThunderDBPtr experienceDB_
The database of motions to search through.
Definition: ThunderRetrieveRepair.h:172
void freeMemory()
Free the memory allocated by this planner.
Definition: ThunderRetrieveRepair.cpp:122
The planner failed to find a solution.
Definition: PlannerStatus.h:62
base::ProblemDefinitionPtr repairProblemDef_
A secondary problem definition for the repair planner to use.
Definition: ThunderRetrieveRepair.h:184
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:264
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
Definition: PathGeometric.h:249
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
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:63
STL namespace.
Struct for passing around partially solved solutions.
Definition: SPARSdb.h:233
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:213
base::State * getState(unsigned int index)
Get the state located at index along the path.
Definition: PathGeometric.h:237
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
const PathGeometric & getChosenRecallPath() const
Get the chosen path used from database for repair.
Definition: ThunderRetrieveRepair.cpp:407
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
void setExperienceDB(const tools::ThunderDBPtr &experienceDB)
Pass a pointer of the database from the thunder framework.
Definition: ThunderRetrieveRepair.cpp:86
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
std::size_t nearestPathsChosenID_
the ID within nearestPaths_ of the path that was chosen for repair
Definition: ThunderRetrieveRepair.h:178
A shared pointer wrapper for ompl::base::Planner.
std::size_t getLastRecalledNearestPathChosen() const
Get debug information about the top recalled paths that were chosen for further filtering.
Definition: ThunderRetrieveRepair.cpp:402
The planner did not find a solution for some other reason.
Definition: PlannerStatus.h:70
base::PlannerPtr repairPlanner_
A secondary planner for replanning.
Definition: ThunderRetrieveRepair.h:181
bool repairPath(const base::PlannerTerminationCondition &ptc, PathGeometric &primaryPath)
Repairs a path to be valid in the current planning environment.
Definition: ThunderRetrieveRepair.cpp:189
bool smoothingEnabled_
Optionally smooth retrieved and repaired paths from database.
Definition: ThunderRetrieveRepair.h:196
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: ThunderRetrieveRepair.cpp:99
int nearestK_
Number of 'k' close solutions to choose from database for further filtering.
Definition: ThunderRetrieveRepair.h:193
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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
A shared pointer wrapper for ompl::base::SpaceInformation.
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: ThunderRetrieveRepair.cpp:76
const std::vector< PathGeometric > & getLastRecalledNearestPaths() const
Get debug information about the top recalled paths that were chosen for further filtering.
Definition: ThunderRetrieveRepair.cpp:397
std::vector< PathGeometric > nearestPaths_
Recall the nearest paths and store this in planner data for introspection later.
Definition: ThunderRetrieveRepair.h:175
void setRepairPlanner(const base::PlannerPtr &planner)
Set the planner that will be used for repairing invalid paths recalled from experience.
Definition: ThunderRetrieveRepair.cpp:91
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
PathSimplifierPtr path_simplifier_
The instance of the path simplifier.
Definition: ThunderRetrieveRepair.h:190
ThunderRetrieveRepair(const base::SpaceInformationPtr &si, tools::ThunderDBPtr experienceDB)
Constructor.
Definition: ThunderRetrieveRepair.cpp:55
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:170
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
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
A shared pointer wrapper for ompl::base::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...
Definition: PathGeometric.h:231