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 base::PlannerStatus(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,
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 {
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 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:203
void getRepairPlannerDatas(std::vector< base::PlannerDataPtr > &data) const
Get information about the exploration data structure the repair motion planner used each call...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
std::vector< base::PlannerDataPtr > nearestPaths_
Recall the nearest paths and store this in planner data for introspection later.
void setRepairPlanner(const base::PlannerPtr &planner)
Set the planner that will be used for repairing invalid paths recalled from experience.
The planner failed to find a solution.
Definition: PlannerStatus.h:62
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...
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.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
const std::vector< base::PlannerDataPtr > & getLastRecalledNearestPaths() const
Get debug information about the top recalled paths that were chosen for further filtering.
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
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.
void getPlannerData(base::PlannerData &data) const override
Get information about the exploration data structure the planning from scratch motion planner used...
tools::LightningDBPtr experienceDB_
The database of motions to search through.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:429
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
std::size_t getLastRecalledNearestPathChosen() const
Get debug information about the top recalled paths that were chosen for further filtering.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
A shared pointer wrapper for ompl::base::Planner.
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
The planner did not find a solution for some other reason.
Definition: PlannerStatus.h:70
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
geometric::PathSimplifierPtr psk_
The instance of the path simplifier.
base::PlannerPtr repairPlanner_
A secondary planner for replanning.
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...
A shared pointer wrapper for ompl::base::SpaceInformation.
std::size_t nearestPathsChosenID_
the ID within nearestPaths_ of the path that was chosen for repair
Definition of an abstract state.
Definition: State.h:49
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
std::vector< base::PlannerDataPtr > repairPlannerDatas_
Debug the repair planner by saving its planner data each time it is used.
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
int nearestK_
Number of &#39;k&#39; close solutions to choose from database for further filtering.
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...
The exception type for ompl.
Definition: Exception.h:46
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
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 ...
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
point now()
Get the current time point.
Definition: Time.h:70
base::PlannerDataPtr getChosenRecallPath() const
Get the chosen path used from database for repair.
LightningRetrieveRepair(const base::SpaceInformationPtr &si, tools::LightningDBPtr experienceDB)
Constructor.
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:170
Definition of a geometric path.
Definition: PathGeometric.h:60
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:64
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
void setLightningDB(const tools::LightningDBPtr &experienceDB)
Pass a pointer of the database from the lightning framework.
base::ProblemDefinitionPtr repairProblemDef_
A secondary problem definition for the repair planner to use.
bool repairPath(const base::PlannerTerminationCondition &ptc, geometric::PathGeometric &primaryPath)
Repairs a path to be valid in the current planning environment.
A shared pointer wrapper for ompl::base::PlannerData.
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...
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
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...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68