AITstar.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, University of Oxford
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 University of Toronto 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 // Authors: Marlin Strub
36 
37 #include <algorithm>
38 #include <cmath>
39 #include <string>
40 
41 #include <boost/range/adaptor/reversed.hpp>
42 
43 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
44 #include "ompl/geometric/planners/informedtrees/AITstar.h"
45 #include "ompl/util/Console.h"
46 
47 using namespace std::string_literals;
48 
49 namespace ompl
50 {
51  namespace geometric
52  {
53  AITstar::AITstar(const ompl::base::SpaceInformationPtr &spaceInformation)
54  : ompl::base::Planner(spaceInformation, "AITstar")
55  , solutionCost_()
56  , graph_(solutionCost_)
57  , forwardQueue_([this](const aitstar::Edge &lhs, const aitstar::Edge &rhs) {
58  return std::lexicographical_compare(lhs.getSortKey().cbegin(), lhs.getSortKey().cend(),
59  rhs.getSortKey().cbegin(), rhs.getSortKey().cend(),
60  [this](const ompl::base::Cost &a, const ompl::base::Cost &b) {
61  return objective_->isCostBetterThan(a, b);
62  });
63  })
64  , reverseQueue_(
65  [this](const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> &lhs,
66  const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> &rhs) {
67  return std::lexicographical_compare(lhs.first.cbegin(), lhs.first.cend(), rhs.first.cbegin(),
68  rhs.first.cend(),
69  [this](const ompl::base::Cost &a, const ompl::base::Cost &b) {
70  return objective_->isCostBetterThan(a, b);
71  });
72  })
73  {
74  // Specify AIT*'s planner specs.
75  specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
76  specs_.multithreaded = false;
77  specs_.approximateSolutions = true;
78  specs_.optimizingPaths = true;
79  specs_.directed = true;
80  specs_.provingSolutionNonExistence = false;
81  specs_.canReportIntermediateSolutions = true;
82 
83  // Register the setting callbacks.
84  declareParam<bool>("use_k_nearest", this, &AITstar::setUseKNearest, &AITstar::getUseKNearest, "0,1");
85  declareParam<double>("rewire_factor", this, &AITstar::setRewireFactor, &AITstar::getRewireFactor,
86  "1.0:0.01:3.0");
87  declareParam<std::size_t>("samples_per_batch", this, &AITstar::setBatchSize, &AITstar::getBatchSize,
88  "1:1:1000");
89  declareParam<bool>("use_graph_pruning", this, &AITstar::enablePruning, &AITstar::isPruningEnabled, "0,1");
90  declareParam<bool>("find_approximate_solutions", this, &AITstar::trackApproximateSolutions,
92 
93  // Register the progress properties.
94  addPlannerProgressProperty("iterations INTEGER", [this]() { return std::to_string(numIterations_); });
95  addPlannerProgressProperty("best cost DOUBLE", [this]() { return std::to_string(solutionCost_.value()); });
96  addPlannerProgressProperty("state collision checks INTEGER",
97  [this]() { return std::to_string(graph_.getNumberOfStateCollisionChecks()); });
98  addPlannerProgressProperty("edge collision checks INTEGER",
99  [this]() { return std::to_string(numEdgeCollisionChecks_); });
100  addPlannerProgressProperty("nearest neighbour calls INTEGER",
101  [this]() { return std::to_string(graph_.getNumberOfNearestNeighborCalls()); });
102  }
103 
105  {
106  // Call the base-class setup.
107  Planner::setup();
108 
109  // Check that a problem definition has been set.
110  if (static_cast<bool>(Planner::pdef_))
111  {
112  // Default to path length optimization objective if none has been specified.
113  if (!pdef_->hasOptimizationObjective())
114  {
115  OMPL_WARN("%s: No optimization objective has been specified. Defaulting to path length.",
116  Planner::getName().c_str());
117  Planner::pdef_->setOptimizationObjective(
118  std::make_shared<ompl::base::PathLengthOptimizationObjective>(Planner::si_));
119  }
120 
121  if (static_cast<bool>(pdef_->getGoal()))
122  {
123  // If we were given a goal, make sure its of appropriate type.
124  if (!(pdef_->getGoal()->hasType(ompl::base::GOAL_SAMPLEABLE_REGION)))
125  {
126  OMPL_ERROR("AIT* is currently only implemented for goals that can be cast to "
127  "ompl::base::GOAL_SAMPLEABLE_GOAL_REGION.");
128  setup_ = false;
129  return;
130  }
131  }
132 
133  // Pull the optimization objective through the problem definition.
134  objective_ = pdef_->getOptimizationObjective();
135 
136  // Initialize the solution cost to be infinite.
137  solutionCost_ = objective_->infiniteCost();
138  approximateSolutionCost_ = objective_->infiniteCost();
139  approximateSolutionCostToGoal_ = objective_->infiniteCost();
140 
141  // Pull the motion validator through the space information.
142  motionValidator_ = si_->getMotionValidator();
143 
144  // Setup a graph.
145  graph_.setup(si_, pdef_, &pis_);
146  }
147  else
148  {
149  // AIT* can't be setup without a problem definition.
150  setup_ = false;
151  OMPL_WARN("AIT*: Unable to setup without a problem definition.");
152  }
153  }
154 
156  {
157  graph_.clear();
158  forwardQueue_.clear();
159  reverseQueue_.clear();
160  solutionCost_ = objective_->infiniteCost();
161  approximateSolutionCost_ = objective_->infiniteCost();
162  approximateSolutionCostToGoal_ = objective_->infiniteCost();
163  edgesToBeInserted_.clear();
164  numIterations_ = 0u;
165  performReverseSearchIteration_ = true;
166  isForwardSearchStartedOnBatch_ = false;
167  forwardQueueMustBeRebuilt_ = false;
168  Planner::clear();
169  }
170 
172  {
173  // The planner status to return.
174  auto status = ompl::base::PlannerStatus::StatusType::UNKNOWN;
175 
176  // Ensure the planner is setup.
177  Planner::checkValidity();
178  if (!Planner::setup_)
179  {
180  OMPL_WARN("%s: Failed to setup and thus solve can not do anything meaningful.", name_.c_str());
181  status = ompl::base::PlannerStatus::StatusType::ABORT;
182  informAboutPlannerStatus(status);
183  return status;
184  }
185 
186  // If the graph currently does not have a goal state, we wait until we get one.
187  if (!graph_.hasAGoalState())
188  {
189  graph_.updateStartAndGoalStates(terminationCondition, &pis_);
190  }
191 
192  if (!graph_.hasAStartState())
193  {
194  OMPL_WARN("%s: No solution can be found as no start states are available", name_.c_str());
195  status = ompl::base::PlannerStatus::StatusType::INVALID_START;
196  informAboutPlannerStatus(status);
197  return status;
198  }
199 
200  // If the graph still doesn't have a goal after waiting there's nothing to solve.
201  if (!graph_.hasAGoalState())
202  {
203  OMPL_WARN("%s: No solution can be found as no goal states are available", name_.c_str());
204  status = ompl::base::PlannerStatus::StatusType::INVALID_GOAL;
205  informAboutPlannerStatus(status);
206  return status;
207  }
208 
209  OMPL_INFORM("%s: Searching for a solution to the given planning problem. The current best solution cost is "
210  "%.4f",
211  name_.c_str(), solutionCost_.value());
212 
213  // Iterate to solve the problem.
214  while (!terminationCondition && !objective_->isSatisfied(solutionCost_))
215  {
216  iterate(terminationCondition);
217  }
218 
219  // Someone might call ProblemDefinition::clearSolutionPaths() between invokations of Planner::solve(), in
220  // which case previously found solutions are not registered with the problem definition anymore.
221  updateExactSolution();
222 
223  // If there are no exact solutions registered in the problem definition and we're tracking approximate
224  // solutions, find the best vertex in the graph.
225  if (!pdef_->hasExactSolution() && trackApproximateSolutions_)
226  {
227  for (const auto &vertex : graph_.getVertices())
228  {
229  updateApproximateSolution(vertex);
230  }
231  }
232 
233  // Return the right planner status.
234  if (objective_->isFinite(solutionCost_))
235  {
236  status = ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION;
237  }
238  else if (trackApproximateSolutions_ && objective_->isFinite(approximateSolutionCost_))
239  {
240  status = ompl::base::PlannerStatus::StatusType::APPROXIMATE_SOLUTION;
241  }
242  else
243  {
244  status = ompl::base::PlannerStatus::StatusType::TIMEOUT;
245  }
246 
247  informAboutPlannerStatus(status);
248  return status;
249  }
250 
252  {
253  return solutionCost_;
254  }
255 
257  {
258  // base::PlannerDataVertex takes a raw pointer to a state. I want to guarantee, that the state lives as long
259  // as the program lives.
260  static std::set<
261  std::shared_ptr<aitstar::Vertex>,
262  std::function<bool(const std::shared_ptr<aitstar::Vertex> &, const std::shared_ptr<aitstar::Vertex> &)>>
263  liveStates([](const auto &lhs, const auto &rhs) { return lhs->getId() < rhs->getId(); });
264 
265  // Fill the planner progress properties.
266  Planner::getPlannerData(data);
267 
268  // Get the vertices.
269  auto vertices = graph_.getVertices();
270 
271  // Add the vertices and edges.
272  for (const auto &vertex : vertices)
273  {
274  // Add the vertex to the live states.
275  liveStates.insert(vertex);
276 
277  // Add the vertex as the right kind of vertex.
278  if (graph_.isStart(vertex))
279  {
280  data.addStartVertex(ompl::base::PlannerDataVertex(vertex->getState(), vertex->getId()));
281  }
282  else if (graph_.isGoal(vertex))
283  {
284  data.addGoalVertex(ompl::base::PlannerDataVertex(vertex->getState(), vertex->getId()));
285  }
286  else
287  {
288  data.addVertex(ompl::base::PlannerDataVertex(vertex->getState(), vertex->getId()));
289  }
290 
291  // If it has a parent, add the corresponding edge.
292  if (vertex->hasForwardParent())
293  {
294  data.addEdge(ompl::base::PlannerDataVertex(vertex->getState(), vertex->getId()),
295  ompl::base::PlannerDataVertex(vertex->getForwardParent()->getState(),
296  vertex->getForwardParent()->getId()));
297  }
298  }
299  }
300 
301  void AITstar::setBatchSize(std::size_t batchSize)
302  {
303  batchSize_ = batchSize;
304  }
305 
306  std::size_t AITstar::getBatchSize() const
307  {
308  return batchSize_;
309  }
310 
311  void AITstar::setRewireFactor(double rewireFactor)
312  {
313  graph_.setRewireFactor(rewireFactor);
314  }
315 
317  {
318  return graph_.getRewireFactor();
319  }
320 
322  {
323  trackApproximateSolutions_ = track;
324  if (!trackApproximateSolutions_)
325  {
326  if (static_cast<bool>(objective_))
327  {
328  approximateSolutionCost_ = objective_->infiniteCost();
329  approximateSolutionCostToGoal_ = objective_->infiniteCost();
330  }
331  }
332  }
333 
335  {
336  return trackApproximateSolutions_;
337  }
338 
339  void AITstar::enablePruning(bool prune)
340  {
341  isPruningEnabled_ = prune;
342  }
343 
345  {
346  return isPruningEnabled_;
347  }
348 
349  void AITstar::setUseKNearest(bool useKNearest)
350  {
351  graph_.setUseKNearest(useKNearest);
352  }
353 
355  {
356  return graph_.getUseKNearest();
357  }
358 
359  void AITstar::setRepairReverseSearch(bool repairReverseSearch)
360  {
361  repairReverseSearch_ = repairReverseSearch;
362  }
363 
364  void AITstar::rebuildForwardQueue()
365  {
366  // Get all edges from the queue.
367  std::vector<aitstar::Edge> edges;
368  forwardQueue_.getContent(edges);
369 
370  // Rebuilding the queue invalidates the incoming and outgoing lookup.
371  for (const auto &edge : edges)
372  {
373  edge.getChild()->resetForwardQueueIncomingLookup();
374  edge.getParent()->resetForwardQueueOutgoingLookup();
375  }
376 
377  // Clear the queue.
378  forwardQueue_.clear();
379 
380  // Insert all edges into the queue if they connect vertices that have been processed, otherwise store them
381  // in the cache of edges that are to be inserted.
382  if (haveAllVerticesBeenProcessed(edges))
383  {
384  for (auto &edge : edges)
385  {
386  insertOrUpdateInForwardQueue(aitstar::Edge(edge.getParent(), edge.getChild(),
387  computeSortKey(edge.getParent(), edge.getChild())));
388  }
389  }
390  else
391  {
392  edgesToBeInserted_ = edges;
393  performReverseSearchIteration_ = true;
394  }
395  }
396 
397  void AITstar::rebuildReverseQueue()
398  {
399  // Rebuilding the reverse queue invalidates the reverse queue pointers.
400  std::vector<KeyVertexPair> content;
401  reverseQueue_.getContent(content);
402  for (auto &element : content)
403  {
404  element.second->resetReverseQueuePointer();
405  }
406  reverseQueue_.clear();
407 
408  for (auto &vertex : content)
409  {
410  // Compute the sort key for the vertex queue.
411  std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
412  computeSortKey(vertex.second), vertex.second);
413  auto reverseQueuePointer = reverseQueue_.insert(element);
414  element.second->setReverseQueuePointer(reverseQueuePointer);
415  }
416  }
417 
418  void AITstar::informAboutNewSolution() const
419  {
420  OMPL_INFORM("%s (%u iterations): Found a new exact solution of cost %.4f. Sampled a total of %u states, %u "
421  "of which were valid samples (%.1f \%). Processed %u edges, %u of which were collision checked "
422  "(%.1f \%). The forward search tree has %u vertices. The reverse search tree has %u vertices.",
423  name_.c_str(), numIterations_, solutionCost_.value(), graph_.getNumberOfSampledStates(),
424  graph_.getNumberOfValidSamples(),
425  graph_.getNumberOfSampledStates() == 0u ?
426  0.0 :
427  100.0 * (static_cast<double>(graph_.getNumberOfValidSamples()) /
428  static_cast<double>(graph_.getNumberOfSampledStates())),
429  numProcessedEdges_, numEdgeCollisionChecks_,
430  numProcessedEdges_ == 0u ? 0.0 :
431  100.0 * (static_cast<float>(numEdgeCollisionChecks_) /
432  static_cast<float>(numProcessedEdges_)),
433  countNumVerticesInForwardTree(), countNumVerticesInReverseTree());
434  }
435 
436  void AITstar::informAboutPlannerStatus(ompl::base::PlannerStatus::StatusType status) const
437  {
438  switch (status)
439  {
440  case ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION:
441  {
442  OMPL_INFORM("%s (%u iterations): Found an exact solution of cost %.4f.", name_.c_str(),
443  numIterations_, solutionCost_.value());
444  break;
445  }
446  case ompl::base::PlannerStatus::StatusType::APPROXIMATE_SOLUTION:
447  {
448  OMPL_INFORM("%s (%u iterations): Did not find an exact solution, but found an approximate solution "
449  "of cost %.4f which is %.4f away from a goal (in cost space).",
450  name_.c_str(), numIterations_, approximateSolutionCost_.value(),
451  approximateSolutionCostToGoal_.value());
452  break;
453  }
454  case ompl::base::PlannerStatus::StatusType::TIMEOUT:
455  {
456  if (trackApproximateSolutions_)
457  {
458  OMPL_INFORM("%s (%u iterations): Did not find any solution.", name_.c_str(), numIterations_);
459  }
460  else
461  {
462  OMPL_INFORM("%s (%u iterations): Did not find an exact solution, and tracking approximate "
463  "solutions is disabled.",
464  name_.c_str(), numIterations_);
465  }
466  break;
467  }
468  case ompl::base::PlannerStatus::StatusType::UNKNOWN:
469  case ompl::base::PlannerStatus::StatusType::INVALID_START:
470  case ompl::base::PlannerStatus::StatusType::INVALID_GOAL:
471  case ompl::base::PlannerStatus::StatusType::UNRECOGNIZED_GOAL_TYPE:
472  case ompl::base::PlannerStatus::StatusType::CRASH:
473  case ompl::base::PlannerStatus::StatusType::ABORT:
474  case ompl::base::PlannerStatus::StatusType::TYPE_COUNT:
475  {
476  OMPL_INFORM("%s (%u iterations): Unable to solve the given planning problem.", name_.c_str(),
477  numIterations_);
478  }
479  }
480 
481  OMPL_INFORM(
482  "%s (%u iterations): Sampled a total of %u states, %u of which were valid samples (%.1f \%). "
483  "Processed %u edges, %u of which were collision checked (%.1f \%). The forward search tree "
484  "has %u vertices. The reverse search tree has %u vertices.",
485  name_.c_str(), numIterations_, graph_.getNumberOfSampledStates(), graph_.getNumberOfValidSamples(),
486  graph_.getNumberOfSampledStates() == 0u ?
487  0.0 :
488  100.0 * (static_cast<double>(graph_.getNumberOfValidSamples()) /
489  static_cast<double>(graph_.getNumberOfSampledStates())),
490  numProcessedEdges_, numEdgeCollisionChecks_,
491  numProcessedEdges_ == 0u ?
492  0.0 :
493  100.0 * (static_cast<float>(numEdgeCollisionChecks_) / static_cast<float>(numProcessedEdges_)),
494  countNumVerticesInForwardTree(), countNumVerticesInReverseTree());
495  }
496 
497  std::vector<aitstar::Edge> AITstar::getEdgesInQueue() const
498  {
499  std::vector<aitstar::Edge> edges;
500  forwardQueue_.getContent(edges);
501  return edges;
502  }
503 
504  std::vector<std::shared_ptr<aitstar::Vertex>> AITstar::getVerticesInQueue() const
505  {
506  // Get the content from the queue.
507  std::vector<std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>>> content;
508  reverseQueue_.getContent(content);
509 
510  // Return the vertices.
511  std::vector<std::shared_ptr<aitstar::Vertex>> vertices;
512  for (const auto &pair : content)
513  {
514  vertices.emplace_back(pair.second);
515  }
516  return vertices;
517  }
518 
520  {
521  if (!forwardQueue_.empty())
522  {
523  return forwardQueue_.top()->data;
524  }
525 
526  return {};
527  }
528 
529  std::shared_ptr<aitstar::Vertex> AITstar::getNextVertexInQueue() const
530  {
531  if (!reverseQueue_.empty())
532  {
533  return reverseQueue_.top()->data.second;
534  }
535 
536  return {};
537  }
538 
539  std::vector<std::shared_ptr<aitstar::Vertex>> AITstar::getVerticesInReverseSearchTree() const
540  {
541  // Get all vertices from the graph.
542  auto vertices = graph_.getVertices();
543 
544  // Erase the vertices that are not in the reverse search tree.
545  vertices.erase(std::remove_if(vertices.begin(), vertices.end(),
546  [this](const std::shared_ptr<aitstar::Vertex> &vertex) {
547  return !graph_.isGoal(vertex) && !vertex->hasReverseParent();
548  }),
549  vertices.end());
550  return vertices;
551  }
552 
553  void AITstar::iterate(const ompl::base::PlannerTerminationCondition &terminationCondition)
554  {
555  // If this is the first time solve is called, populate the reverse queue.
556  if (numIterations_ == 0u)
557  {
558  for (const auto &goal : graph_.getGoalVertices())
559  {
560  // Set the cost to come from the goal to identity cost.
561  goal->setCostToComeFromGoal(objective_->identityCost());
562 
563  // Create an element for the queue.
564  std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
565  std::array<ompl::base::Cost, 2u>(
566  {computeCostToGoToStartHeuristic(goal), ompl::base::Cost(0.0)}),
567  goal);
568 
569  // Insert the element into the queue and set the corresponding pointer.
570  auto reverseQueuePointer = reverseQueue_.insert(element);
571  goal->setReverseQueuePointer(reverseQueuePointer);
572  }
573  }
574 
575  // Keep track of the number of iterations.
576  ++numIterations_;
577 
578  // If the algorithm is in a state that requires performing a reverse search iteration, try to perform one.
579  if (performReverseSearchIteration_)
580  {
581  // If the reverse queue is not empty, perform a reverse search iteration.
582  if (!reverseQueue_.empty())
583  {
584  performReverseSearchIteration();
585  }
586  else
587  {
588  // If the reverse queue is empty, check if there are forward edges to be inserted.
589  // Only insert forward edges that connect vertices that have been processed with the reverse search.
590  // If the reverse queue is empty and a vertex has not been processed with the reverse queue, it
591  // means that it's not in the same connected component of the RGG as the goal. We can not reach the
592  // goal from this vertex and therefore this edge can be disregarded.
593  for (const auto &edge : edgesToBeInserted_)
594  {
595  if (haveAllVerticesBeenProcessed(edge))
596  {
597  insertOrUpdateInForwardQueue(aitstar::Edge(
598  edge.getParent(), edge.getChild(), computeSortKey(edge.getParent(), edge.getChild())));
599  }
600  }
601  edgesToBeInserted_.clear();
602  performReverseSearchIteration_ = false;
603  forwardQueueMustBeRebuilt_ = true;
604  }
605  }
606  else
607  {
608  if (!isForwardSearchStartedOnBatch_)
609  {
610  // Remember that we've started the forward search on this batch.
611  isForwardSearchStartedOnBatch_ = true;
612 
613  // If no start vertex has finite cost to come from the goal, there is no need to start the
614  // forward search.
615  std::vector<aitstar::Edge> outgoingStartEdges;
616  for (const auto &start : graph_.getStartVertices())
617  {
618  if (objective_->isFinite(start->getCostToComeFromGoal()))
619  {
620  // Add the outgoing edges of all start vertices to the queue.
621  for (const auto &start : graph_.getStartVertices())
622  {
623  const auto outgoingEdges = getOutgoingEdges(start);
624  outgoingStartEdges.insert(outgoingStartEdges.end(), outgoingEdges.begin(),
625  outgoingEdges.end());
626  }
627  }
628  }
629  // If all vertices of the outgoing start edges have been processed, insert the edges into the
630  // forward queue. If not, remember that they are to be inserted.
631  if (haveAllVerticesBeenProcessed(outgoingStartEdges))
632  {
633  for (const auto &edge : outgoingStartEdges)
634  {
635  insertOrUpdateInForwardQueue(edge);
636  }
637  }
638  else
639  {
640  assert(edgesToBeInserted_.empty());
641  edgesToBeInserted_ = outgoingStartEdges;
642  performReverseSearchIteration_ = true;
643  }
644  }
645  else if (forwardQueueMustBeRebuilt_)
646  {
647  // Rebuild the forwared queue if necessary.
648  rebuildForwardQueue();
649  forwardQueueMustBeRebuilt_ = false;
650  }
651  else if (!forwardQueue_.empty())
652  {
653  // If the forward queue is not empty, perform a forward search iteration.
654  performForwardSearchIteration();
655  }
656  else // We should not perform a reverse search iteration and the forward queue is empty. Add more
657  // samples.
658  {
659  // Add new samples to the graph.
660  if (graph_.addSamples(batchSize_, terminationCondition))
661  {
662  // Clear the reverse queue.
663  std::vector<std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>>>
664  reverseQueue;
665  reverseQueue_.getContent(reverseQueue);
666  for (const auto &element : reverseQueue)
667  {
668  element.second->resetReverseQueuePointer();
669  }
670  reverseQueue_.clear();
671 
672  // Clear the forward queue.
673  std::vector<aitstar::Edge> forwardQueue;
674  forwardQueue_.getContent(forwardQueue);
675  for (const auto &element : forwardQueue)
676  {
677  element.getChild()->resetForwardQueueIncomingLookup();
678  element.getParent()->resetForwardQueueOutgoingLookup();
679  }
680  forwardQueue_.clear();
681 
682  // Clear the cache of edges to be inserted.
683  edgesToBeInserted_.clear();
684 
685  // Remove useless samples from the graph.
686  if (isPruningEnabled_)
687  {
688  graph_.prune();
689  }
690 
691  // Add new start and goal states if necessary.
693  {
695  }
696 
697  // Add the goals to the reverse queue.
698  for (const auto &goal : graph_.getGoalVertices())
699  {
700  goal->setCostToComeFromGoal(objective_->identityCost());
701  auto reverseQueuePointer = reverseQueue_.insert(std::make_pair(computeSortKey(goal), goal));
702  goal->setReverseQueuePointer(reverseQueuePointer);
703  }
704 
705  // This is a new batch, so the search hasn't been started.
706  isForwardSearchStartedOnBatch_ = false;
707 
708  // We have to update the heuristic. Start with a reverse iteration.
709  performReverseSearchIteration_ = true;
710  }
711  }
712  }
713  }
714 
715  void AITstar::performForwardSearchIteration()
716  {
717  // We should never perform a forward search iteration while there are still edges to be inserted.
718  assert(edgesToBeInserted_.empty());
719 
720  // Get the most promising edge.
721  auto &edge = forwardQueue_.top()->data;
722  auto parent = edge.getParent();
723  auto child = edge.getChild();
724 
725  // Make sure the edge is sane
726  assert(child->hasReverseParent() || graph_.isGoal(child));
727  assert(parent->hasReverseParent() || graph_.isGoal(parent));
728 
729  // Remove the edge from the incoming and outgoing lookups.
730  child->removeFromForwardQueueIncomingLookup(forwardQueue_.top());
731  parent->removeFromForwardQueueOutgoingLookup(forwardQueue_.top());
732 
733  // Remove the edge from the queue.
734  forwardQueue_.pop();
735 
736  // This counts as processing an edge.
737  ++numProcessedEdges_;
738 
739  // Register that an outgoing edge of the parent has been popped from the queue. This means that the parent
740  // has optimal cost-to-come for the current approximation.
741  parent->registerPoppedOutgoingEdgeDuringForwardSearch();
742 
743  // If this is edge can not possibly improve our solution, the search is done.
744  auto edgeCost = objective_->motionCostHeuristic(parent->getState(), child->getState());
745  auto parentCostToGoToGoal = objective_->combineCosts(edgeCost, child->getCostToGoToGoal());
746  auto pathThroughEdgeCost = objective_->combineCosts(parent->getCostToComeFromStart(), parentCostToGoToGoal);
747  if (!objective_->isCostBetterThan(pathThroughEdgeCost, solutionCost_))
748  {
749  if (objective_->isFinite(pathThroughEdgeCost) ||
750  !objective_->isFinite(computeBestCostToComeFromGoalOfAnyStart()))
751  {
752  std::vector<aitstar::Edge> edges;
753  forwardQueue_.getContent(edges);
754  for (const auto &edge : edges)
755  {
756  edge.getChild()->resetForwardQueueIncomingLookup();
757  edge.getParent()->resetForwardQueueOutgoingLookup();
758  }
759  forwardQueue_.clear();
760  }
761  else
762  {
763  performReverseSearchIteration_ = true;
764  }
765  } // This edge can improve the solution. Check if it's already in the reverse search tree.
766  else if (child->hasForwardParent() && child->getForwardParent()->getId() == parent->getId())
767  {
768  // This is a freebie, just insert the outgoing edges of the child.
769  auto edges = getOutgoingEdges(child);
770  if (haveAllVerticesBeenProcessed(edges))
771  {
772  for (const auto &edge : edges)
773  {
774  insertOrUpdateInForwardQueue(edge);
775  }
776  }
777  else
778  {
779  edgesToBeInserted_ = edges;
780  performReverseSearchIteration_ = true;
781  return;
782  }
783  } // This edge can improve the solution and is not already in the reverse search tree.
784  else if (objective_->isCostBetterThan(child->getCostToComeFromStart(),
785  objective_->combineCosts(parent->getCostToComeFromStart(),
786  objective_->motionCostHeuristic(
787  parent->getState(), child->getState()))))
788  {
789  // If the edge cannot improve the cost to come to the child, we're done processing it.
790  return;
791  } // The edge can possibly improve the solution and the path to the child. Let's check it for collision.
792  else if (parent->isWhitelistedAsChild(child) ||
793  motionValidator_->checkMotion(parent->getState(), child->getState()))
794  {
795  // Remember that this is a good edge.
796  if (!parent->isWhitelistedAsChild(child))
797  {
798  parent->whitelistAsChild(child);
799  numEdgeCollisionChecks_++;
800  }
801 
802  // Compute the edge cost.
803  auto edgeCost = objective_->motionCost(parent->getState(), child->getState());
804 
805  // Check if the edge can improve the cost to come to the child.
806  if (objective_->isCostBetterThan(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCost),
807  child->getCostToComeFromStart()))
808  {
809  // If the child has already been expanded during the current forward search, something's fishy.
810  assert(!child->hasHadOutgoingEdgePoppedDuringCurrentForwardSearch());
811 
812  // Rewire the child.
813  child->setForwardParent(parent, edgeCost);
814 
815  // Add it to the children of the parent.
816  parent->addToForwardChildren(child);
817 
818  // Share the good news with the whole branch.
819  child->updateCostOfForwardBranch();
820 
821  // Check if the solution can benefit from this.
822  updateExactSolution();
823 
824  // If we don't have an exact solution but are tracking approximate solutions, see if the child is
825  // the best approximate solution so far.
826  if (!pdef_->hasExactSolution() && trackApproximateSolutions_)
827  {
828  updateApproximateSolution(child);
829  }
830 
831  // Insert the child's outgoing edges into the queue.
832  auto edges = getOutgoingEdges(child);
833  if (haveAllVerticesBeenProcessed(edges))
834  {
835  for (const auto &edge : edges)
836  {
837  insertOrUpdateInForwardQueue(edge);
838  }
839  }
840  else
841  {
842  edgesToBeInserted_ = edges;
843  performReverseSearchIteration_ = true;
844  return;
845  }
846  }
847  }
848  else
849  {
850  // This child should be blacklisted.
851  parent->blacklistAsChild(child);
852 
853  // If desired, now is the time to repair the reverse search.
854  if (repairReverseSearch_)
855  {
856  if (parent->hasReverseParent() && parent->getReverseParent()->getId() == child->getId())
857  {
858  // The parent was connected to the child through an invalid edge.
859  parent->setCostToComeFromGoal(objective_->infiniteCost());
860  parent->setExpandedCostToComeFromGoal(objective_->infiniteCost());
861  parent->resetReverseParent();
862  child->removeFromReverseChildren(parent->getId());
863 
864  // This also affects all children of this vertex.
865  invalidateCostToComeFromGoalOfReverseBranch(parent);
866 
867  // If any of these children are in the reverse queue, their sort key is outdated.
868  rebuildReverseQueue();
869 
870  // The parent's cost-to-come needs to be updated. This places children in open.
871  reverseSearchUpdateVertex(parent);
872 
873  // If the reverse queue is empty, this means we have to add new samples.
874  if (reverseQueue_.empty())
875  {
876  std::vector<aitstar::Edge> edges;
877  forwardQueue_.getContent(edges);
878  for (const auto &edge : edges)
879  {
880  edge.getChild()->resetForwardQueueIncomingLookup();
881  edge.getParent()->resetForwardQueueOutgoingLookup();
882  }
883  forwardQueue_.clear();
884  }
885  else
886  {
887  performReverseSearchIteration_ = true;
888  }
889  }
890  }
891  }
892  }
893 
894  void AITstar::performReverseSearchIteration()
895  {
896  assert(!reverseQueue_.empty());
897 
898  // Get the most promising vertex.
899  auto vertex = reverseQueue_.top()->data.second;
900 
901  // Remove it from the queue.
902  reverseQueue_.pop();
903  vertex->resetReverseQueuePointer();
904 
905  // The open queue should not contain consistent vertices.
906  assert((!objective_->isFinite(vertex->getCostToComeFromGoal()) &&
907  !objective_->isFinite(vertex->getExpandedCostToComeFromGoal())) ||
908  (!objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(),
909  vertex->getExpandedCostToComeFromGoal())));
910 
911  // If any goal is underconsistent, we need to continue.
912  bool underconsistentStart{false};
913  for (const auto &start : graph_.getStartVertices())
914  {
915  if (objective_->isCostBetterThan(start->getExpandedCostToComeFromGoal(),
916  start->getCostToComeFromGoal()))
917  {
918  underconsistentStart = true;
919  break;
920  }
921  }
922 
923  // If there is currently no reason to think this vertex can be on an optimal path, clear the queue.
924  if (edgesToBeInserted_.empty() &&
925  ((!underconsistentStart &&
926  !objective_->isCostBetterThan(objective_->combineCosts(vertex->getCostToComeFromGoal(),
927  computeCostToGoToStartHeuristic(vertex)),
928  solutionCost_)) ||
929  objective_->isCostBetterThan(
930  ompl::base::Cost(computeBestCostToComeFromGoalOfAnyStart().value() + 1e-6), solutionCost_)))
931  {
932  // This invalidates the cost-to-go estimate of the forward search.
933  performReverseSearchIteration_ = false;
934  forwardQueueMustBeRebuilt_ = true;
935  vertex->registerExpansionDuringReverseSearch();
936  return;
937  }
938 
939  // Check if the vertex is overconsistent. g(s) < v(s).
940  if (objective_->isCostBetterThan(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal()))
941  {
942  // Register the expansion of this vertex.
943  vertex->registerExpansionDuringReverseSearch();
944  }
945  else
946  {
947  // Register the expansion of this vertex.
948  vertex->registerExpansionDuringReverseSearch();
949  vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
950  reverseSearchUpdateVertex(vertex);
951  }
952 
953  // Update all successors. Start with the reverse search children, because if this vertex
954  // becomes the parent of a neighbor, that neighbor would be updated again as part of the
955  // reverse children.
956  for (const auto &child : vertex->getReverseChildren())
957  {
958  reverseSearchUpdateVertex(child);
959  }
960 
961  // We can now process the neighbors.
962  for (const auto &neighbor : graph_.getNeighbors(vertex))
963  {
964  if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
965  !vertex->isBlacklistedAsChild(neighbor))
966  {
967  reverseSearchUpdateVertex(neighbor);
968  }
969  }
970 
971  // We also need to update the forward search children.
972  for (const auto &child : vertex->getForwardChildren())
973  {
974  reverseSearchUpdateVertex(child);
975  }
976 
977  // We also need to update the forward search parent if it exists.
978  if (vertex->hasForwardParent())
979  {
980  reverseSearchUpdateVertex(vertex->getForwardParent());
981  }
982 
983  if (!edgesToBeInserted_.empty())
984  {
985  if (haveAllVerticesBeenProcessed(edgesToBeInserted_))
986  {
987  for (std::size_t i = 0u; i < edgesToBeInserted_.size(); ++i)
988  {
989  auto &edge = edgesToBeInserted_.at(i);
990  edge.setSortKey(computeSortKey(edge.getParent(), edge.getChild()));
991  insertOrUpdateInForwardQueue(edge);
992  }
993  edgesToBeInserted_.clear();
994  }
995  }
996  }
997 
998  void AITstar::reverseSearchUpdateVertex(const std::shared_ptr<aitstar::Vertex> &vertex)
999  {
1000  if (!graph_.isGoal(vertex))
1001  {
1002  // Get the best parent for this vertex.
1003  auto bestParent = vertex->getReverseParent();
1004  auto bestCost =
1005  vertex->hasReverseParent() ? vertex->getCostToComeFromGoal() : objective_->infiniteCost();
1006 
1007  // Check all neighbors as defined by the graph.
1008  for (const auto &neighbor : graph_.getNeighbors(vertex))
1009  {
1010  if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
1011  !vertex->isBlacklistedAsChild(neighbor))
1012  {
1013  auto edgeCost = objective_->motionCostHeuristic(neighbor->getState(), vertex->getState());
1014  auto parentCost = objective_->combineCosts(neighbor->getExpandedCostToComeFromGoal(), edgeCost);
1015  if (objective_->isCostBetterThan(parentCost, bestCost))
1016  {
1017  bestParent = neighbor;
1018  bestCost = parentCost;
1019  }
1020  }
1021  }
1022 
1023  // Check all children this vertex holds in the forward search.
1024  for (const auto &forwardChild : vertex->getForwardChildren())
1025  {
1026  auto edgeCost = objective_->motionCostHeuristic(forwardChild->getState(), vertex->getState());
1027  auto parentCost = objective_->combineCosts(forwardChild->getExpandedCostToComeFromGoal(), edgeCost);
1028 
1029  if (objective_->isCostBetterThan(parentCost, bestCost))
1030  {
1031  bestParent = forwardChild;
1032  bestCost = parentCost;
1033  }
1034  }
1035 
1036  // Check the parent of this vertex in the forward search.
1037  if (vertex->hasForwardParent())
1038  {
1039  auto forwardParent = vertex->getForwardParent();
1040  auto edgeCost = objective_->motionCostHeuristic(forwardParent->getState(), vertex->getState());
1041  auto parentCost =
1042  objective_->combineCosts(forwardParent->getExpandedCostToComeFromGoal(), edgeCost);
1043 
1044  if (objective_->isCostBetterThan(parentCost, bestCost))
1045  {
1046  bestParent = forwardParent;
1047  bestCost = parentCost;
1048  }
1049  }
1050 
1051  // Check the parent of this vertex in the reverse search.
1052  if (vertex->hasReverseParent())
1053  {
1054  auto reverseParent = vertex->getReverseParent();
1055  auto edgeCost = objective_->motionCostHeuristic(reverseParent->getState(), vertex->getState());
1056  auto parentCost =
1057  objective_->combineCosts(reverseParent->getExpandedCostToComeFromGoal(), edgeCost);
1058 
1059  if (objective_->isCostBetterThan(parentCost, bestCost))
1060  {
1061  bestParent = reverseParent;
1062  bestCost = parentCost;
1063  }
1064  }
1065 
1066  // If this vertex is now disconnected, take special care.
1067  if (!objective_->isFinite(bestCost))
1068  {
1069  // Reset the reverse parent if the vertex has one.
1070  if (vertex->hasReverseParent())
1071  {
1072  vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
1073  vertex->resetReverseParent();
1074  }
1075 
1076  // Invalidate the branch in the reverse search tree that is rooted at this vertex.
1077  vertex->setCostToComeFromGoal(objective_->infiniteCost());
1078  vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1079  auto affectedVertices = vertex->invalidateReverseBranch();
1080 
1081  // Remove the affected edges from the forward queue, placing them in the edge cache.
1082  for (const auto &affectedVertex : affectedVertices)
1083  {
1084  auto forwardQueueIncomingLookup = affectedVertex.lock()->getForwardQueueIncomingLookup();
1085  for (const auto &element : forwardQueueIncomingLookup)
1086  {
1087  edgesToBeInserted_.emplace_back(element->data);
1088  element->data.getParent()->removeFromForwardQueueOutgoingLookup(element);
1089  forwardQueue_.remove(element);
1090  }
1091  affectedVertex.lock()->resetForwardQueueIncomingLookup();
1092 
1093  auto forwardQueueOutgoingLookup = affectedVertex.lock()->getForwardQueueOutgoingLookup();
1094  for (const auto &element : forwardQueueOutgoingLookup)
1095  {
1096  edgesToBeInserted_.emplace_back(element->data);
1097  element->data.getChild()->removeFromForwardQueueIncomingLookup(element);
1098  forwardQueue_.remove(element);
1099  }
1100  affectedVertex.lock()->resetForwardQueueOutgoingLookup();
1101  }
1102 
1103  // Remove appropriate edges from the forward queue that target the root of the branch.
1104  auto vertexForwardQueueIncomingLookup = vertex->getForwardQueueIncomingLookup();
1105  for (const auto &element : vertexForwardQueueIncomingLookup)
1106  {
1107  auto &edge = element->data;
1108  auto it = std::find_if(affectedVertices.begin(), affectedVertices.end(),
1109  [edge](const auto &affectedVertex) {
1110  return affectedVertex.lock()->getId() == edge.getParent()->getId();
1111  });
1112  if (it != affectedVertices.end())
1113  {
1114  edgesToBeInserted_.emplace_back(element->data);
1115  vertex->removeFromForwardQueueIncomingLookup(element);
1116  element->data.getParent()->removeFromForwardQueueOutgoingLookup(element);
1117  forwardQueue_.remove(element);
1118  }
1119  }
1120 
1121  // Remove appropriate edges from the forward queue that target the root of the branch.
1122  auto vertexForwardQueueOutgoingLookup = vertex->getForwardQueueOutgoingLookup();
1123  for (const auto &element : vertexForwardQueueOutgoingLookup)
1124  {
1125  edgesToBeInserted_.emplace_back(element->data);
1126  vertex->removeFromForwardQueueOutgoingLookup(element);
1127  element->data.getChild()->removeFromForwardQueueIncomingLookup(element);
1128  forwardQueue_.remove(element);
1129  }
1130 
1131  // Check update the invalidated vertices and insert them in open if they become connected to the
1132  // tree.
1133  for (const auto &affectedVertex : affectedVertices)
1134  {
1135  auto affectedVertexPtr = affectedVertex.lock();
1136 
1137  reverseSearchUpdateVertex(affectedVertexPtr);
1138  if (affectedVertex.lock()->hasReverseParent())
1139  {
1140  insertOrUpdateInReverseQueue(affectedVertexPtr);
1141  affectedVertexPtr->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1142  }
1143  }
1144 
1145  return;
1146  }
1147 
1148  // Update the reverse parent.
1149  vertex->setReverseParent(bestParent);
1150 
1151  // Update the children of the parent.
1152  bestParent->addToReverseChildren(vertex);
1153 
1154  // Set the cost to come from the goal.
1155  vertex->setCostToComeFromGoal(bestCost);
1156 
1157  // If this has made the vertex inconsistent, insert or update it in the open queue.
1158  if (!objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(),
1159  vertex->getExpandedCostToComeFromGoal()))
1160  {
1161  insertOrUpdateInReverseQueue(vertex);
1162  }
1163  else
1164  {
1165  // Remove this vertex from the queue if it is in the queue.
1166  auto reverseQueuePointer = vertex->getReverseQueuePointer();
1167  if (reverseQueuePointer)
1168  {
1169  reverseQueue_.remove(reverseQueuePointer);
1170  vertex->resetReverseQueuePointer();
1171  }
1172  }
1173  }
1174  }
1175 
1176  void AITstar::insertOrUpdateInReverseQueue(const std::shared_ptr<aitstar::Vertex> &vertex)
1177  {
1178  // Get the pointer to the element in the queue.
1179  auto element = vertex->getReverseQueuePointer();
1180 
1181  // Update it if it is in the queue.
1182  if (element)
1183  {
1184  element->data.first = computeSortKey(vertex);
1185  reverseQueue_.update(element);
1186  }
1187  else // Insert it into the queue otherwise.
1188  {
1189  // Compute the sort key for the vertex queue.
1190  std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
1191  computeSortKey(vertex), vertex);
1192 
1193  // Insert the vertex into the queue, storing the corresponding pointer.
1194  auto reverseQueuePointer = reverseQueue_.insert(element);
1195  vertex->setReverseQueuePointer(reverseQueuePointer);
1196  }
1197  }
1198 
1199  void AITstar::insertOrUpdateInForwardQueue(const aitstar::Edge &edge)
1200  {
1201  // Check if the edge is already in the queue and can be updated.
1202  auto lookup = edge.getChild()->getForwardQueueIncomingLookup();
1203  auto it = std::find_if(lookup.begin(), lookup.end(), [&edge](const auto element) {
1204  return element->data.getParent()->getId() == edge.getParent()->getId();
1205  });
1206 
1207  if (it != lookup.end())
1208  {
1209  // We used the incoming lookup of the child. Assert that it is already in the outgoing lookup of the
1210  // parent.
1211  assert(std::find_if(edge.getParent()->getForwardQueueOutgoingLookup().begin(),
1212  edge.getParent()->getForwardQueueOutgoingLookup().end(),
1213  [&edge](const auto element) {
1214  return element->data.getChild()->getId() == edge.getChild()->getId();
1215  }) != edge.getParent()->getForwardQueueOutgoingLookup().end());
1216  (*it)->data.setSortKey(edge.getSortKey());
1217  forwardQueue_.update(*it);
1218  }
1219  else
1220  {
1221  auto element = forwardQueue_.insert(edge);
1222  edge.getParent()->addToForwardQueueOutgoingLookup(element);
1223  edge.getChild()->addToForwardQueueIncomingLookup(element);
1224  }
1225  }
1226 
1227  std::shared_ptr<ompl::geometric::PathGeometric>
1228  AITstar::getPathToVertex(const std::shared_ptr<aitstar::Vertex> &vertex) const
1229  {
1230  // Create the reverse path by following the parents to the start.
1231  std::vector<std::shared_ptr<aitstar::Vertex>> reversePath;
1232  auto current = vertex;
1233  while (!graph_.isStart(current))
1234  {
1235  reversePath.emplace_back(current);
1236  current = current->getForwardParent();
1237  }
1238  reversePath.emplace_back(current);
1239 
1240  // Reverse the reverse path to get the forward path.
1241  auto path = std::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
1242  for (const auto &vertex : boost::adaptors::reverse(reversePath))
1243  {
1244  path->append(vertex->getState());
1245  }
1246 
1247  return path;
1248  }
1249 
1250  std::array<ompl::base::Cost, 3u> AITstar::computeSortKey(const std::shared_ptr<aitstar::Vertex> &parent,
1251  const std::shared_ptr<aitstar::Vertex> &child) const
1252  {
1253  // Compute the sort key [g_T(start) + c_hat(start, neighbor) + h_hat(neighbor), g_T(start) +
1254  // c_hat(start, neighbor), g_T(start)].
1255  ompl::base::Cost edgeCostHeuristic = objective_->motionCostHeuristic(parent->getState(), child->getState());
1256  return {
1257  objective_->combineCosts(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCostHeuristic),
1258  child->getCostToGoToGoal()),
1259  objective_->combineCosts(edgeCostHeuristic, child->getCostToGoToGoal()),
1260  parent->getCostToComeFromStart()};
1261  }
1262 
1263  std::array<ompl::base::Cost, 2u> AITstar::computeSortKey(const std::shared_ptr<aitstar::Vertex> &vertex) const
1264  {
1265  // LPA* sort key is [min(g(x), v(x)) + h(x); min(g(x), v(x))].
1266  return {objective_->combineCosts(objective_->betterCost(vertex->getCostToComeFromGoal(),
1267  vertex->getExpandedCostToComeFromGoal()),
1268  computeCostToGoToStartHeuristic(vertex)),
1269  objective_->betterCost(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal())};
1270  }
1271 
1272  std::vector<aitstar::Edge> AITstar::getOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex) const
1273  {
1274  // Prepare the return variable.
1275  std::vector<aitstar::Edge> outgoingEdges;
1276 
1277  // Insert the edges to the current children.
1278  for (const auto &child : vertex->getForwardChildren())
1279  {
1280  outgoingEdges.emplace_back(vertex, child, computeSortKey(vertex, child));
1281  }
1282 
1283  // Insert the edges to the current neighbors.
1284  for (const auto &neighbor : graph_.getNeighbors(vertex))
1285  {
1286  // We do not want self loops.
1287  if (vertex->getId() == neighbor->getId())
1288  {
1289  continue;
1290  }
1291 
1292  // If the neighbor is the reverse parent, it will explicitly be added later.
1293  if (vertex->hasReverseParent() && neighbor->getId() == vertex->getReverseParent()->getId())
1294  {
1295  continue;
1296  }
1297 
1298  // We do not want blacklisted edges.
1299  if (neighbor->isBlacklistedAsChild(vertex) || vertex->isBlacklistedAsChild(neighbor))
1300  {
1301  continue;
1302  }
1303 
1304  // If none of the above tests caught on, we can insert the edge.
1305  outgoingEdges.emplace_back(vertex, neighbor, computeSortKey(vertex, neighbor));
1306  }
1307 
1308  // Insert the edge to the reverse search parent.
1309  if (vertex->hasReverseParent())
1310  {
1311  const auto &reverseParent = vertex->getReverseParent();
1312  outgoingEdges.emplace_back(vertex, reverseParent, computeSortKey(vertex, reverseParent));
1313  }
1314 
1315  return outgoingEdges;
1316  }
1317 
1318  bool AITstar::haveAllVerticesBeenProcessed(const std::vector<aitstar::Edge> &edges) const
1319  {
1320  for (const auto &edge : edges)
1321  {
1322  if (!haveAllVerticesBeenProcessed(edge))
1323  {
1324  return false;
1325  }
1326  }
1327 
1328  return true;
1329  }
1330 
1331  bool AITstar::haveAllVerticesBeenProcessed(const aitstar::Edge &edge) const
1332  {
1333  return edge.getParent()->hasBeenExpandedDuringCurrentReverseSearch() &&
1334  edge.getChild()->hasBeenExpandedDuringCurrentReverseSearch();
1335  }
1336 
1337  void AITstar::updateExactSolution()
1338  {
1339  // Check if any of the goals have a cost to come less than the current solution cost.
1340  for (const auto &goal : graph_.getGoalVertices())
1341  {
1342  // We need to check whether the cost is better, or whether someone has removed the exact solution from
1343  // the problem definition.
1344  if (objective_->isCostBetterThan(goal->getCostToComeFromStart(), solutionCost_) ||
1345  (!pdef_->hasExactSolution() && objective_->isFinite(goal->getCostToComeFromStart())))
1346  {
1347  // Remember the incumbent cost.
1348  solutionCost_ = goal->getCostToComeFromStart();
1349 
1350  // Create a solution.
1351  ompl::base::PlannerSolution solution(getPathToVertex(goal));
1352  solution.setPlannerName(name_);
1353 
1354  // Set the optimized flag.
1355  solution.setOptimized(objective_, solutionCost_, objective_->isSatisfied(solutionCost_));
1356 
1357  // Let the problem definition know that a new solution exists.
1358  pdef_->addSolutionPath(solution);
1359 
1360  // Let the user know about the new solution.
1361  informAboutNewSolution();
1362  }
1363  }
1364  }
1365 
1366  void AITstar::updateApproximateSolution(const std::shared_ptr<aitstar::Vertex> &vertex)
1367  {
1368  assert(trackApproximateSolutions_);
1369  if (vertex->hasForwardParent() || graph_.isStart(vertex))
1370  {
1371  auto costToGoal = computeCostToGoToGoal(vertex);
1372 
1373  // We need to check whether this is better than the current approximate solution or whether someone has
1374  // removed all approximate solutions from the problem definition.
1375  if (objective_->isCostBetterThan(costToGoal, approximateSolutionCostToGoal_) ||
1376  !pdef_->hasApproximateSolution())
1377  {
1378  // Remember the incumbent approximate cost.
1379  approximateSolutionCost_ = vertex->getCostToComeFromStart();
1380  approximateSolutionCostToGoal_ = costToGoal;
1381  ompl::base::PlannerSolution solution(getPathToVertex(vertex));
1382  solution.setPlannerName(name_);
1383 
1384  // Set the approximate flag.
1385  solution.setApproximate(costToGoal.value());
1386 
1387  // This solution is approximate and can not satisfy the objective.
1388  solution.setOptimized(objective_, approximateSolutionCost_, false);
1389 
1390  // Let the problem definition know that a new solution exists.
1391  pdef_->addSolutionPath(solution);
1392  }
1393  }
1394  };
1395 
1396  ompl::base::Cost AITstar::computeCostToGoToStartHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const
1397  {
1398  // We need to loop over all start vertices and see which is the closest one.
1399  ompl::base::Cost bestCost = objective_->infiniteCost();
1400  for (const auto &start : graph_.getStartVertices())
1401  {
1402  bestCost = objective_->betterCost(
1403  bestCost, objective_->motionCostHeuristic(vertex->getState(), start->getState()));
1404  }
1405  return bestCost;
1406  }
1407 
1408  ompl::base::Cost AITstar::computeCostToGoToGoalHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const
1409  {
1410  // We need to loop over all goal vertices and see which is the closest one.
1411  ompl::base::Cost bestCost = objective_->infiniteCost();
1412  for (const auto &goal : graph_.getGoalVertices())
1413  {
1414  bestCost = objective_->betterCost(
1415  bestCost, objective_->motionCostHeuristic(vertex->getState(), goal->getState()));
1416  }
1417  return bestCost;
1418  }
1419 
1420  ompl::base::Cost AITstar::computeCostToGoToGoal(const std::shared_ptr<aitstar::Vertex> &vertex) const
1421  {
1422  // We need to loop over all goal vertices and see which is the closest one.
1423  ompl::base::Cost bestCost = objective_->infiniteCost();
1424  for (const auto &goal : graph_.getGoalVertices())
1425  {
1426  bestCost =
1427  objective_->betterCost(bestCost, objective_->motionCost(vertex->getState(), goal->getState()));
1428  }
1429  return bestCost;
1430  }
1431 
1432  ompl::base::Cost AITstar::computeBestCostToComeFromGoalOfAnyStart() const
1433  {
1434  // We need to loop over all start vertices and see which is the closest one.
1435  ompl::base::Cost bestCost = objective_->infiniteCost();
1436  for (const auto &start : graph_.getStartVertices())
1437  {
1438  bestCost = objective_->betterCost(bestCost, start->getCostToComeFromGoal());
1439  }
1440  return bestCost;
1441  }
1442 
1443  std::size_t AITstar::countNumVerticesInForwardTree() const
1444  {
1445  std::size_t numVerticesInForwardTree = 0u;
1446  auto vertices = graph_.getVertices();
1447  for (const auto &vertex : vertices)
1448  {
1449  if (graph_.isStart(vertex) || vertex->hasForwardParent())
1450  {
1451  ++numVerticesInForwardTree;
1452  }
1453  }
1454  return numVerticesInForwardTree;
1455  }
1456 
1457  std::size_t AITstar::countNumVerticesInReverseTree() const
1458  {
1459  std::size_t numVerticesInReverseTree = 0u;
1460  auto vertices = graph_.getVertices();
1461  for (const auto &vertex : vertices)
1462  {
1463  if (graph_.isGoal(vertex) || vertex->hasReverseParent())
1464  {
1465  ++numVerticesInReverseTree;
1466  }
1467  }
1468  return numVerticesInReverseTree;
1469  }
1470 
1471  void AITstar::invalidateCostToComeFromGoalOfReverseBranch(const std::shared_ptr<aitstar::Vertex> &vertex)
1472  {
1473  vertex->unregisterExpansionDuringReverseSearch();
1474  // Update the cost of all reverse children and remove from open.
1475  for (const auto &child : vertex->getReverseChildren())
1476  {
1477  invalidateCostToComeFromGoalOfReverseBranch(child);
1478  child->setCostToComeFromGoal(objective_->infiniteCost());
1479  child->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1480  auto reverseQueuePointer = child->getReverseQueuePointer();
1481  if (reverseQueuePointer)
1482  {
1483  reverseQueue_.remove(reverseQueuePointer);
1484  child->resetReverseQueuePointer();
1485  }
1486  }
1487  }
1488 
1489  } // namespace geometric
1490 } // namespace ompl
void pop()
Remove the top element.
Definition: BinaryHeap.h:126
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:195
void remove(Element *element)
Remove a specific element.
Definition: BinaryHeap.h:132
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:120
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:140
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:186
void getContent(std::vector< _T > &content) const
Get the data stored in this heap.
Definition: BinaryHeap.h:207
void clear()
Clear the heap.
Definition: BinaryHeap.h:112
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
double value() const
The value of the cost.
Definition: Cost.h:56
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
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...
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:348
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:355
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerInputStates pis_
Utility class to extract valid input states
Definition: Planner.h:423
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:420
std::string name_
The name of this planner.
Definition: Planner.h:426
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:440
A shared pointer wrapper for ompl::base::SpaceInformation.
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInQueue() const
Get the vertex queue.
Definition: AITstar.cpp:504
std::vector< aitstar::Edge > getEdgesInQueue() const
Get the edge queue.
Definition: AITstar.cpp:497
void enablePruning(bool prune)
Set whether pruning is enabled or not.
Definition: AITstar.cpp:339
aitstar::Edge getNextEdgeInQueue() const
Get the next edge in the queue.
Definition: AITstar.cpp:519
double getRewireFactor() const
Get the rewire factor of the RGG graph.
Definition: AITstar.cpp:316
bool isPruningEnabled() const
Get whether pruning is enabled or not.
Definition: AITstar.cpp:344
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG graph.
Definition: AITstar.cpp:311
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInReverseSearchTree() const
Get the vertices in the reverse search tree.
Definition: AITstar.cpp:539
void setBatchSize(std::size_t batchSize)
Set the batch size.
Definition: AITstar.cpp:301
ompl::base::Cost bestCost() const
Get the cost of the incumbent solution.
Definition: AITstar.cpp:251
bool getUseKNearest() const
Get whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition: AITstar.cpp:354
void getPlannerData(base::PlannerData &data) const override
Get the planner data.
Definition: AITstar.cpp:256
void setRepairReverseSearch(bool repairReverseSearch)
Enable LPA* repair of reverse search.
Definition: AITstar.cpp:359
std::shared_ptr< aitstar::Vertex > getNextVertexInQueue() const
Get the next vertex in the queue.
Definition: AITstar.cpp:529
void setup() override
Additional setup that can only be done once a problem definition is set.
Definition: AITstar.cpp:104
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override
Solves a motion planning problem.
Definition: AITstar.cpp:171
void trackApproximateSolutions(bool track)
Set whether to track approximate solutions.
Definition: AITstar.cpp:321
void setUseKNearest(bool useKNearest)
Set whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition: AITstar.cpp:349
void clear() override
Clears the algorithm's internal state.
Definition: AITstar.cpp:155
std::size_t getBatchSize() const
Get the batch size.
Definition: AITstar.cpp:306
bool areApproximateSolutionsTracked() const
Get whether approximate solutions are tracked.
Definition: AITstar.cpp:334
bool isStart(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a start vertex.
bool hasAStartState() const
Returns whether the graph has a goal state.
const std::vector< std::shared_ptr< Vertex > > & getStartVertices() const
Get the start vertices.
std::vector< std::shared_ptr< Vertex > > getVertices() const
Get all vertices.
bool isGoal(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a goal vertex.
double getRewireFactor() const
Get the reqire factor of the RGG.
std::size_t getNumberOfValidSamples() const
Returns the total number of valid samples found.
void prune()
Prune all samples that can not contribute to a solution better than the current one.
void updateStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition, ompl::base::PlannerInputStates *inputStates)
Adds new start and goals to the graph if avavilable and creates a new informed sampler if necessary.
void clear()
Resets the graph to its construction state, without resetting options.
const std::vector< std::shared_ptr< Vertex > > & getGoalVertices() const
Get the goal vertices.
bool addSamples(std::size_t numNewSamples, const ompl::base::PlannerTerminationCondition &terminationCondition)
Adds a batch of samples and returns the samples it has added.
void setup(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, ompl::base::PlannerInputStates *inputStates)
The setup method for the graph. Needed to have it on the stack.
std::vector< std::shared_ptr< Vertex > > getNeighbors(const std::shared_ptr< Vertex > &vertex) const
Get neighbors of a vertex.
bool hasAGoalState() const
Returns whether the graph has a goal state.
bool getUseKNearest() const
Whether the graph uses a k-nearest connection model. If false, it uses an r-disc model.
std::size_t getNumberOfSampledStates() const
Returns the total number of sampled states.
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG.
void setUseKNearest(bool useKNearest)
Whether to use a k-nearest connection model. If false, it uses an r-disc model.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:56
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
Representation of a solution to a planning problem.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
StatusType
The possible values of the status returned by a planner.
Definition: PlannerStatus.h:52