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 
65 ompl::geometric::LightningRetrieveRepair::~LightningRetrieveRepair() = default;
66 
68 {
69  Planner::clear();
70 
71  // Clear the inner planner
72  if (repairPlanner_)
73  repairPlanner_->clear();
74 }
75 
76 void 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 
149  ompl::base::PlannerDataPtr chosenPath;
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 
353  ompl::geometric::PathGeometric &primaryPath)
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 
561 const 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 
577 void 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 }
void setLightningDB(const tools::LightningDBPtr &experienceDB)
Pass a pointer of the database from the lightning framework.
std::size_t getLastRecalledNearestPathChosen() const
Get debug information about the top recalled paths that were chosen for further filtering.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
void setRepairPlanner(const base::PlannerPtr &planner)
Set the planner that will be used for repairing invalid paths recalled from experience.
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...
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:52
void getPlannerData(base::PlannerData &data) const override
Get information about the exploration data structure the planning from scratch motion planner used.
geometric::PathSimplifierPtr psk_
The instance of the path simplifier.
Definition of an abstract state.
Definition: State.h:50
base::PlannerDataPtr getChosenRecallPath() const
Get the chosen path used from database for repair.
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 ...
point now()
Get the current time point.
Definition: Time.h:58
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62
Definition of a geometric path.
Definition: PathGeometric.h:66
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...
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
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
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
const std::vector< base::PlannerDataPtr > & getLastRecalledNearestPaths() const
Get debug information about the top recalled paths that were chosen for further filtering.
@ ABORT
The planner did not find a solution for some other reason.
Definition: PlannerStatus.h:70
bool repairPath(const base::PlannerTerminationCondition &ptc, geometric::PathGeometric &primaryPath)
Repairs a path to be valid in the current planning environment.
@ UNKNOWN
Uninitialized status.
Definition: PlannerStatus.h:54
void getRepairPlannerDatas(std::vector< base::PlannerDataPtr > &data) const
Get information about the exploration data structure the repair motion planner used each call.
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
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.
LightningRetrieveRepair(const base::SpaceInformationPtr &si, tools::LightningDBPtr experienceDB)
Constructor.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
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...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:202
base::ProblemDefinitionPtr repairProblemDef_
A secondary problem definition for the repair planner to use.
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...
The exception type for ompl.
Definition: Exception.h:47
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
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.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22