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();
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()
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  // Clear the reverse queue.
660  std::vector<std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>>>
661  reverseQueue;
662  reverseQueue_.getContent(reverseQueue);
663  for (const auto &element : reverseQueue)
664  {
665  element.second->resetReverseQueuePointer();
666  }
667  reverseQueue_.clear();
668 
669  // Clear the forward queue.
670  std::vector<aitstar::Edge> forwardQueue;
671  forwardQueue_.getContent(forwardQueue);
672  for (const auto &element : forwardQueue)
673  {
674  element.getChild()->resetForwardQueueIncomingLookup();
675  element.getParent()->resetForwardQueueOutgoingLookup();
676  }
677  forwardQueue_.clear();
678 
679  // Clear the cache of edges to be inserted.
680  edgesToBeInserted_.clear();
681 
682  // Add new start and goal states if necessary.
684  {
686  }
687 
688  // Remove useless samples from the graph.
689  if (isPruningEnabled_)
690  {
691  graph_.prune();
692  }
693 
694  // Add new samples to the graph.
695  graph_.addSamples(batchSize_);
696 
697  // Add the goals to the reverse queue.
698  for (const auto &goal : graph_.getGoalVertices())
699  {
700  auto reverseQueuePointer = reverseQueue_.insert(std::make_pair(computeSortKey(goal), goal));
701  goal->setReverseQueuePointer(reverseQueuePointer);
702  goal->setCostToComeFromGoal(objective_->identityCost());
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  void AITstar::performForwardSearchIteration()
715  {
716  // We should never perform a forward search iteration while there are still edges to be inserted.
717  assert(edgesToBeInserted_.empty());
718 
719  // Get the most promising edge.
720  auto &edge = forwardQueue_.top()->data;
721  auto parent = edge.getParent();
722  auto child = edge.getChild();
723 
724  // Make sure the edge is sane
725  assert(child->hasReverseParent() || graph_.isGoal(child));
726  assert(parent->hasReverseParent() || graph_.isGoal(parent));
727 
728  // Remove the edge from the incoming and outgoing lookups.
729  child->removeFromForwardQueueIncomingLookup(forwardQueue_.top());
730  parent->removeFromForwardQueueOutgoingLookup(forwardQueue_.top());
731 
732  // Remove the edge from the queue.
733  forwardQueue_.pop();
734 
735  // This counts as processing an edge.
736  ++numProcessedEdges_;
737 
738  // Register that an outgoing edge of the parent has been popped from the queue. This means that the parent
739  // has optimal cost-to-come for the current approximation.
740  parent->registerPoppedOutgoingEdgeDuringForwardSearch();
741 
742  // If this is edge can not possibly improve our solution, the search is done.
743  auto edgeCost = objective_->motionCostHeuristic(parent->getState(), child->getState());
744  auto parentCostToGoToGoal = objective_->combineCosts(edgeCost, child->getCostToGoToGoal());
745  auto pathThroughEdgeCost = objective_->combineCosts(parent->getCostToComeFromStart(), parentCostToGoToGoal);
746  if (!objective_->isCostBetterThan(pathThroughEdgeCost, solutionCost_))
747  {
748  if (objective_->isFinite(pathThroughEdgeCost) ||
749  !objective_->isFinite(computeBestCostToComeFromGoalOfAnyStart()))
750  {
751  std::vector<aitstar::Edge> edges;
752  forwardQueue_.getContent(edges);
753  for (const auto &edge : edges)
754  {
755  edge.getChild()->resetForwardQueueIncomingLookup();
756  edge.getParent()->resetForwardQueueOutgoingLookup();
757  }
758  forwardQueue_.clear();
759  }
760  else
761  {
762  performReverseSearchIteration_ = true;
763  }
764  } // This edge can improve the solution. Check if it's already in the reverse search tree.
765  else if (child->hasForwardParent() && child->getForwardParent()->getId() == parent->getId())
766  {
767  // This is a freebie, just insert the outgoing edges of the child.
768  auto edges = getOutgoingEdges(child);
769  if (haveAllVerticesBeenProcessed(edges))
770  {
771  for (const auto &edge : edges)
772  {
773  insertOrUpdateInForwardQueue(edge);
774  }
775  }
776  else
777  {
778  edgesToBeInserted_ = edges;
779  performReverseSearchIteration_ = true;
780  return;
781  }
782  } // This edge can improve the solution and is not already in the reverse search tree.
783  else if (objective_->isCostBetterThan(child->getCostToComeFromStart(),
784  objective_->combineCosts(parent->getCostToComeFromStart(),
785  objective_->motionCostHeuristic(
786  parent->getState(), child->getState()))))
787  {
788  // If the edge cannot improve the cost to come to the child, we're done processing it.
789  return;
790  } // The edge can possibly improve the solution and the path to the child. Let's check it for collision.
791  else if (parent->isWhitelistedAsChild(child) ||
792  motionValidator_->checkMotion(parent->getState(), child->getState()))
793  {
794  // Remember that this is a good edge.
795  if (!parent->isWhitelistedAsChild(child))
796  {
797  parent->whitelistAsChild(child);
798  numEdgeCollisionChecks_++;
799  }
800 
801  // Compute the edge cost.
802  auto edgeCost = objective_->motionCost(parent->getState(), child->getState());
803 
804  // Check if the edge can improve the cost to come to the child.
805  if (objective_->isCostBetterThan(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCost),
806  child->getCostToComeFromStart()))
807  {
808  // If the child has already been expanded during the current forward search, something's fishy.
809  assert(!child->hasHadOutgoingEdgePoppedDuringCurrentForwardSearch());
810 
811  // Rewire the child.
812  child->setForwardParent(parent, edgeCost);
813 
814  // Add it to the children of the parent.
815  parent->addToForwardChildren(child);
816 
817  // Share the good news with the whole branch.
818  child->updateCostOfForwardBranch();
819 
820  // Check if the solution can benefit from this.
821  updateExactSolution();
822 
823  // If we don't have an exact solution but are tracking approximate solutions, see if the child is
824  // the best approximate solution so far.
825  if (!pdef_->hasExactSolution() && trackApproximateSolutions_)
826  {
827  updateApproximateSolution(child);
828  }
829 
830  // Insert the child's outgoing edges into the queue.
831  auto edges = getOutgoingEdges(child);
832  if (haveAllVerticesBeenProcessed(edges))
833  {
834  for (const auto &edge : edges)
835  {
836  insertOrUpdateInForwardQueue(edge);
837  }
838  }
839  else
840  {
841  edgesToBeInserted_ = edges;
842  performReverseSearchIteration_ = true;
843  return;
844  }
845  }
846  }
847  else
848  {
849  // This child should be blacklisted.
850  parent->blacklistAsChild(child);
851 
852  // If desired, now is the time to repair the reverse search.
853  if (repairReverseSearch_)
854  {
855  if (parent->hasReverseParent() && parent->getReverseParent()->getId() == child->getId())
856  {
857  // The parent was connected to the child through an invalid edge.
858  parent->setCostToComeFromGoal(objective_->infiniteCost());
859  parent->setExpandedCostToComeFromGoal(objective_->infiniteCost());
860  parent->resetReverseParent();
861  child->removeFromReverseChildren(parent->getId());
862 
863  // This also affects all children of this vertex.
864  invalidateCostToComeFromGoalOfReverseBranch(parent);
865 
866  // If any of these children are in the reverse queue, their sort key is outdated.
867  rebuildReverseQueue();
868 
869  // The parent's cost-to-come needs to be updated. This places children in open.
870  reverseSearchUpdateVertex(parent);
871 
872  // If the reverse queue is empty, this means we have to add new samples.
873  if (reverseQueue_.empty())
874  {
875  std::vector<aitstar::Edge> edges;
876  forwardQueue_.getContent(edges);
877  for (const auto &edge : edges)
878  {
879  edge.getChild()->resetForwardQueueIncomingLookup();
880  edge.getParent()->resetForwardQueueOutgoingLookup();
881  }
882  forwardQueue_.clear();
883  }
884  else
885  {
886  performReverseSearchIteration_ = true;
887  }
888  }
889  }
890  }
891  }
892 
893  void AITstar::performReverseSearchIteration()
894  {
895  assert(!reverseQueue_.empty());
896 
897  // Get the most promising vertex.
898  auto vertex = reverseQueue_.top()->data.second;
899 
900  // Remove it from the queue.
901  reverseQueue_.pop();
902  vertex->resetReverseQueuePointer();
903 
904  // The open queue should not contain consistent vertices.
905  assert((!objective_->isFinite(vertex->getCostToComeFromGoal()) &&
906  !objective_->isFinite(vertex->getExpandedCostToComeFromGoal())) ||
907  (!objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(),
908  vertex->getExpandedCostToComeFromGoal())));
909 
910  // If any goal is underconsistent, we need to continue.
911  bool underconsistentStart{false};
912  for (const auto &start : graph_.getStartVertices())
913  {
914  if (objective_->isCostBetterThan(start->getExpandedCostToComeFromGoal(),
915  start->getCostToComeFromGoal()))
916  {
917  underconsistentStart = true;
918  break;
919  }
920  }
921 
922  // If there is currently no reason to think this vertex can be on an optimal path, clear the queue.
923  if (edgesToBeInserted_.empty() &&
924  ((!underconsistentStart &&
925  !objective_->isCostBetterThan(objective_->combineCosts(vertex->getCostToComeFromGoal(),
926  computeCostToGoToStartHeuristic(vertex)),
927  solutionCost_)) ||
928  objective_->isCostBetterThan(
929  ompl::base::Cost(computeBestCostToComeFromGoalOfAnyStart().value() + 1e-6), solutionCost_)))
930  {
931  // This invalidates the cost-to-go estimate of the forward search.
932  performReverseSearchIteration_ = false;
933  forwardQueueMustBeRebuilt_ = true;
934  vertex->registerExpansionDuringReverseSearch();
935  return;
936  }
937 
938  // Check if the vertex is overconsistent. g(s) < v(s).
939  if (objective_->isCostBetterThan(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal()))
940  {
941  // Register the expansion of this vertex.
942  vertex->registerExpansionDuringReverseSearch();
943  }
944  else
945  {
946  // Register the expansion of this vertex.
947  vertex->registerExpansionDuringReverseSearch();
948  vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
949  reverseSearchUpdateVertex(vertex);
950  }
951 
952  // Update all successors. Start with the reverse search children, because if this vertex
953  // becomes the parent of a neighbor, that neighbor would be updated again as part of the
954  // reverse children.
955  for (const auto &child : vertex->getReverseChildren())
956  {
957  reverseSearchUpdateVertex(child);
958  }
959 
960  // We can now process the neighbors.
961  for (const auto &neighbor : graph_.getNeighbors(vertex))
962  {
963  if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
964  !vertex->isBlacklistedAsChild(neighbor))
965  {
966  reverseSearchUpdateVertex(neighbor);
967  }
968  }
969 
970  // We also need to update the forward search children.
971  for (const auto &child : vertex->getForwardChildren())
972  {
973  reverseSearchUpdateVertex(child);
974  }
975 
976  // We also need to update the forward search parent if it exists.
977  if (vertex->hasForwardParent())
978  {
979  reverseSearchUpdateVertex(vertex->getForwardParent());
980  }
981 
982  if (!edgesToBeInserted_.empty())
983  {
984  if (haveAllVerticesBeenProcessed(edgesToBeInserted_))
985  {
986  for (std::size_t i = 0u; i < edgesToBeInserted_.size(); ++i)
987  {
988  auto &edge = edgesToBeInserted_.at(i);
989  edge.setSortKey(computeSortKey(edge.getParent(), edge.getChild()));
990  insertOrUpdateInForwardQueue(edge);
991  }
992  edgesToBeInserted_.clear();
993  }
994  }
995  }
996 
997  void AITstar::reverseSearchUpdateVertex(const std::shared_ptr<aitstar::Vertex> &vertex)
998  {
999  if (!graph_.isGoal(vertex))
1000  {
1001  // Get the best parent for this vertex.
1002  auto bestParent = vertex->getReverseParent();
1003  auto bestCost =
1004  vertex->hasReverseParent() ? vertex->getCostToComeFromGoal() : objective_->infiniteCost();
1005 
1006  // Check all neighbors as defined by the graph.
1007  for (const auto &neighbor : graph_.getNeighbors(vertex))
1008  {
1009  if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
1010  !vertex->isBlacklistedAsChild(neighbor))
1011  {
1012  auto edgeCost = objective_->motionCostHeuristic(neighbor->getState(), vertex->getState());
1013  auto parentCost = objective_->combineCosts(neighbor->getExpandedCostToComeFromGoal(), edgeCost);
1014  if (objective_->isCostBetterThan(parentCost, bestCost))
1015  {
1016  bestParent = neighbor;
1017  bestCost = parentCost;
1018  }
1019  }
1020  }
1021 
1022  // Check all children this vertex holds in the forward search.
1023  for (const auto &forwardChild : vertex->getForwardChildren())
1024  {
1025  auto edgeCost = objective_->motionCostHeuristic(forwardChild->getState(), vertex->getState());
1026  auto parentCost = objective_->combineCosts(forwardChild->getExpandedCostToComeFromGoal(), edgeCost);
1027 
1028  if (objective_->isCostBetterThan(parentCost, bestCost))
1029  {
1030  bestParent = forwardChild;
1031  bestCost = parentCost;
1032  }
1033  }
1034 
1035  // Check the parent of this vertex in the forward search.
1036  if (vertex->hasForwardParent())
1037  {
1038  auto forwardParent = vertex->getForwardParent();
1039  auto edgeCost = objective_->motionCostHeuristic(forwardParent->getState(), vertex->getState());
1040  auto parentCost =
1041  objective_->combineCosts(forwardParent->getExpandedCostToComeFromGoal(), edgeCost);
1042 
1043  if (objective_->isCostBetterThan(parentCost, bestCost))
1044  {
1045  bestParent = forwardParent;
1046  bestCost = parentCost;
1047  }
1048  }
1049 
1050  // Check the parent of this vertex in the reverse search.
1051  if (vertex->hasReverseParent())
1052  {
1053  auto reverseParent = vertex->getReverseParent();
1054  auto edgeCost = objective_->motionCostHeuristic(reverseParent->getState(), vertex->getState());
1055  auto parentCost =
1056  objective_->combineCosts(reverseParent->getExpandedCostToComeFromGoal(), edgeCost);
1057 
1058  if (objective_->isCostBetterThan(parentCost, bestCost))
1059  {
1060  bestParent = reverseParent;
1061  bestCost = parentCost;
1062  }
1063  }
1064 
1065  // If this vertex is now disconnected, take special care.
1066  if (!objective_->isFinite(bestCost))
1067  {
1068  // Reset the reverse parent if the vertex has one.
1069  if (vertex->hasReverseParent())
1070  {
1071  vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
1072  vertex->resetReverseParent();
1073  }
1074 
1075  // Invalidate the branch in the reverse search tree that is rooted at this vertex.
1076  vertex->setCostToComeFromGoal(objective_->infiniteCost());
1077  vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1078  auto affectedVertices = vertex->invalidateReverseBranch();
1079 
1080  // Remove the affected edges from the forward queue, placing them in the edge cache.
1081  for (const auto &affectedVertex : affectedVertices)
1082  {
1083  auto forwardQueueIncomingLookup = affectedVertex.lock()->getForwardQueueIncomingLookup();
1084  for (const auto &element : forwardQueueIncomingLookup)
1085  {
1086  edgesToBeInserted_.emplace_back(element->data);
1087  element->data.getParent()->removeFromForwardQueueOutgoingLookup(element);
1088  forwardQueue_.remove(element);
1089  }
1090  affectedVertex.lock()->resetForwardQueueIncomingLookup();
1091 
1092  auto forwardQueueOutgoingLookup = affectedVertex.lock()->getForwardQueueOutgoingLookup();
1093  for (const auto &element : forwardQueueOutgoingLookup)
1094  {
1095  edgesToBeInserted_.emplace_back(element->data);
1096  element->data.getChild()->removeFromForwardQueueIncomingLookup(element);
1097  forwardQueue_.remove(element);
1098  }
1099  affectedVertex.lock()->resetForwardQueueOutgoingLookup();
1100  }
1101 
1102  // Remove appropriate edges from the forward queue that target the root of the branch.
1103  auto vertexForwardQueueIncomingLookup = vertex->getForwardQueueIncomingLookup();
1104  for (const auto &element : vertexForwardQueueIncomingLookup)
1105  {
1106  auto &edge = element->data;
1107  auto it = std::find_if(affectedVertices.begin(), affectedVertices.end(),
1108  [edge](const auto &affectedVertex) {
1109  return affectedVertex.lock()->getId() == edge.getParent()->getId();
1110  });
1111  if (it != affectedVertices.end())
1112  {
1113  edgesToBeInserted_.emplace_back(element->data);
1114  vertex->removeFromForwardQueueIncomingLookup(element);
1115  element->data.getParent()->removeFromForwardQueueOutgoingLookup(element);
1116  forwardQueue_.remove(element);
1117  }
1118  }
1119 
1120  // Remove appropriate edges from the forward queue that target the root of the branch.
1121  auto vertexForwardQueueOutgoingLookup = vertex->getForwardQueueOutgoingLookup();
1122  for (const auto &element : vertexForwardQueueOutgoingLookup)
1123  {
1124  edgesToBeInserted_.emplace_back(element->data);
1125  vertex->removeFromForwardQueueOutgoingLookup(element);
1126  element->data.getChild()->removeFromForwardQueueIncomingLookup(element);
1127  forwardQueue_.remove(element);
1128  }
1129 
1130  // Check update the invalidated vertices and insert them in open if they become connected to the
1131  // tree.
1132  for (const auto &affectedVertex : affectedVertices)
1133  {
1134  auto affectedVertexPtr = affectedVertex.lock();
1135 
1136  reverseSearchUpdateVertex(affectedVertexPtr);
1137  if (affectedVertex.lock()->hasReverseParent())
1138  {
1139  insertOrUpdateInReverseQueue(affectedVertexPtr);
1140  affectedVertexPtr->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1141  }
1142  }
1143 
1144  return;
1145  }
1146 
1147  // Update the reverse parent.
1148  vertex->setReverseParent(bestParent);
1149 
1150  // Update the children of the parent.
1151  bestParent->addToReverseChildren(vertex);
1152 
1153  // Set the cost to come from the goal.
1154  vertex->setCostToComeFromGoal(bestCost);
1155 
1156  // If this has made the vertex inconsistent, insert or update it in the open queue.
1157  if (!objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(),
1158  vertex->getExpandedCostToComeFromGoal()))
1159  {
1160  insertOrUpdateInReverseQueue(vertex);
1161  }
1162  else
1163  {
1164  // Remove this vertex from the queue if it is in the queue.
1165  auto reverseQueuePointer = vertex->getReverseQueuePointer();
1166  if (reverseQueuePointer)
1167  {
1168  reverseQueue_.remove(reverseQueuePointer);
1169  vertex->resetReverseQueuePointer();
1170  }
1171  }
1172  }
1173  }
1174 
1175  void AITstar::insertOrUpdateInReverseQueue(const std::shared_ptr<aitstar::Vertex> &vertex)
1176  {
1177  // Get the pointer to the element in the queue.
1178  auto element = vertex->getReverseQueuePointer();
1179 
1180  // Update it if it is in the queue.
1181  if (element)
1182  {
1183  element->data.first = computeSortKey(vertex);
1184  reverseQueue_.update(element);
1185  }
1186  else // Insert it into the queue otherwise.
1187  {
1188  // Compute the sort key for the vertex queue.
1189  std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
1190  computeSortKey(vertex), vertex);
1191 
1192  // Insert the vertex into the queue, storing the corresponding pointer.
1193  auto reverseQueuePointer = reverseQueue_.insert(element);
1194  vertex->setReverseQueuePointer(reverseQueuePointer);
1195  }
1196  }
1197 
1198  void AITstar::insertOrUpdateInForwardQueue(const aitstar::Edge &edge)
1199  {
1200  // Check if the edge is already in the queue and can be updated.
1201  auto lookup = edge.getChild()->getForwardQueueIncomingLookup();
1202  auto it = std::find_if(lookup.begin(), lookup.end(), [&edge](const auto element) {
1203  return element->data.getParent()->getId() == edge.getParent()->getId();
1204  });
1205 
1206  if (it != lookup.end())
1207  {
1208  // We used the incoming lookup of the child. Assert that it is already in the outgoing lookup of the
1209  // parent.
1210  assert(std::find_if(edge.getParent()->getForwardQueueOutgoingLookup().begin(),
1211  edge.getParent()->getForwardQueueOutgoingLookup().end(),
1212  [&edge](const auto element) {
1213  return element->data.getChild()->getId() == edge.getChild()->getId();
1214  }) != edge.getParent()->getForwardQueueOutgoingLookup().end());
1215  (*it)->data.setSortKey(edge.getSortKey());
1216  forwardQueue_.update(*it);
1217  }
1218  else
1219  {
1220  auto element = forwardQueue_.insert(edge);
1221  edge.getParent()->addToForwardQueueOutgoingLookup(element);
1222  edge.getChild()->addToForwardQueueIncomingLookup(element);
1223  }
1224  }
1225 
1226  std::shared_ptr<ompl::geometric::PathGeometric>
1227  AITstar::getPathToVertex(const std::shared_ptr<aitstar::Vertex> &vertex) const
1228  {
1229  // Create the reverse path by following the parents to the start.
1230  std::vector<std::shared_ptr<aitstar::Vertex>> reversePath;
1231  auto current = vertex;
1232  while (!graph_.isStart(current))
1233  {
1234  reversePath.emplace_back(current);
1235  current = current->getForwardParent();
1236  }
1237  reversePath.emplace_back(current);
1238 
1239  // Reverse the reverse path to get the forward path.
1240  auto path = std::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
1241  for (const auto &vertex : boost::adaptors::reverse(reversePath))
1242  {
1243  path->append(vertex->getState());
1244  }
1245 
1246  return path;
1247  }
1248 
1249  std::array<ompl::base::Cost, 3u> AITstar::computeSortKey(const std::shared_ptr<aitstar::Vertex> &parent,
1250  const std::shared_ptr<aitstar::Vertex> &child) const
1251  {
1252  // Compute the sort key [g_T(start) + c_hat(start, neighbor) + h_hat(neighbor), g_T(start) +
1253  // c_hat(start, neighbor), g_T(start)].
1254  ompl::base::Cost edgeCostHeuristic = objective_->motionCostHeuristic(parent->getState(), child->getState());
1255  return {
1256  objective_->combineCosts(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCostHeuristic),
1257  child->getCostToGoToGoal()),
1258  objective_->combineCosts(edgeCostHeuristic, child->getCostToGoToGoal()),
1259  parent->getCostToComeFromStart()};
1260  }
1261 
1262  std::array<ompl::base::Cost, 2u> AITstar::computeSortKey(const std::shared_ptr<aitstar::Vertex> &vertex) const
1263  {
1264  // LPA* sort key is [min(g(x), v(x)) + h(x); min(g(x), v(x))].
1265  return {objective_->combineCosts(objective_->betterCost(vertex->getCostToComeFromGoal(),
1266  vertex->getExpandedCostToComeFromGoal()),
1267  computeCostToGoToStartHeuristic(vertex)),
1268  objective_->betterCost(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal())};
1269  }
1270 
1271  std::vector<aitstar::Edge> AITstar::getOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex) const
1272  {
1273  // Prepare the return variable.
1274  std::vector<aitstar::Edge> outgoingEdges;
1275 
1276  // Insert the edges to the current children.
1277  for (const auto &child : vertex->getForwardChildren())
1278  {
1279  outgoingEdges.emplace_back(vertex, child, computeSortKey(vertex, child));
1280  }
1281 
1282  // Insert the edges to the current neighbors.
1283  for (const auto &neighbor : graph_.getNeighbors(vertex))
1284  {
1285  // We do not want self loops.
1286  if (vertex->getId() == neighbor->getId())
1287  {
1288  continue;
1289  }
1290 
1291  // If the neighbor is the reverse parent, it will explicitly be added later.
1292  if (vertex->hasReverseParent() && neighbor->getId() == vertex->getReverseParent()->getId())
1293  {
1294  continue;
1295  }
1296 
1297  // We do not want blacklisted edges.
1298  if (neighbor->isBlacklistedAsChild(vertex) || vertex->isBlacklistedAsChild(neighbor))
1299  {
1300  continue;
1301  }
1302 
1303  // If none of the above tests caught on, we can insert the edge.
1304  outgoingEdges.emplace_back(vertex, neighbor, computeSortKey(vertex, neighbor));
1305  }
1306 
1307  // Insert the edge to the reverse search parent.
1308  if (vertex->hasReverseParent())
1309  {
1310  const auto &reverseParent = vertex->getReverseParent();
1311  outgoingEdges.emplace_back(vertex, reverseParent, computeSortKey(vertex, reverseParent));
1312  }
1313 
1314  return outgoingEdges;
1315  }
1316 
1317  bool AITstar::haveAllVerticesBeenProcessed(const std::vector<aitstar::Edge> &edges) const
1318  {
1319  for (const auto &edge : edges)
1320  {
1321  if (!haveAllVerticesBeenProcessed(edge))
1322  {
1323  return false;
1324  }
1325  }
1326 
1327  return true;
1328  }
1329 
1330  bool AITstar::haveAllVerticesBeenProcessed(const aitstar::Edge &edge) const
1331  {
1332  return edge.getParent()->hasBeenExpandedDuringCurrentReverseSearch() &&
1333  edge.getChild()->hasBeenExpandedDuringCurrentReverseSearch();
1334  }
1335 
1336  void AITstar::updateExactSolution()
1337  {
1338  // Check if any of the goals have a cost to come less than the current solution cost.
1339  for (const auto &goal : graph_.getGoalVertices())
1340  {
1341  // We need to check whether the cost is better, or whether someone has removed the exact solution from
1342  // the problem definition.
1343  if (objective_->isCostBetterThan(goal->getCostToComeFromStart(), solutionCost_) ||
1344  (!pdef_->hasExactSolution() && objective_->isFinite(goal->getCostToComeFromStart())))
1345  {
1346  // Remember the incumbent cost.
1347  solutionCost_ = goal->getCostToComeFromStart();
1348 
1349  // Create a solution.
1350  ompl::base::PlannerSolution solution(getPathToVertex(goal));
1351  solution.setPlannerName(name_);
1352 
1353  // Set the optimized flag.
1354  solution.setOptimized(objective_, solutionCost_, objective_->isSatisfied(solutionCost_));
1355 
1356  // Let the problem definition know that a new solution exists.
1357  pdef_->addSolutionPath(solution);
1358 
1359  // Let the user know about the new solution.
1360  informAboutNewSolution();
1361  }
1362  }
1363  }
1364 
1365  void AITstar::updateApproximateSolution(const std::shared_ptr<aitstar::Vertex> &vertex)
1366  {
1367  assert(trackApproximateSolutions_);
1368  if (vertex->hasForwardParent() || graph_.isStart(vertex))
1369  {
1370  auto costToGoal = computeCostToGoToGoal(vertex);
1371 
1372  // We need to check whether this is better than the current approximate solution or whether someone has
1373  // removed all approximate solutions from the problem definition.
1374  if (objective_->isCostBetterThan(costToGoal, approximateSolutionCostToGoal_) ||
1375  !pdef_->hasApproximateSolution())
1376  {
1377  // Remember the incumbent approximate cost.
1378  approximateSolutionCost_ = vertex->getCostToComeFromStart();
1379  approximateSolutionCostToGoal_ = costToGoal;
1380  ompl::base::PlannerSolution solution(getPathToVertex(vertex));
1381  solution.setPlannerName(name_);
1382 
1383  // Set the approximate flag.
1384  solution.setApproximate(costToGoal.value());
1385 
1386  // This solution is approximate and can not satisfy the objective.
1387  solution.setOptimized(objective_, approximateSolutionCost_, false);
1388 
1389  // Let the problem definition know that a new solution exists.
1390  pdef_->addSolutionPath(solution);
1391  }
1392  }
1393  };
1394 
1395  ompl::base::Cost AITstar::computeCostToGoToStartHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const
1396  {
1397  // We need to loop over all start vertices and see which is the closest one.
1398  ompl::base::Cost bestCost = objective_->infiniteCost();
1399  for (const auto &start : graph_.getStartVertices())
1400  {
1401  bestCost = objective_->betterCost(
1402  bestCost, objective_->motionCostHeuristic(vertex->getState(), start->getState()));
1403  }
1404  return bestCost;
1405  }
1406 
1407  ompl::base::Cost AITstar::computeCostToGoToGoalHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const
1408  {
1409  // We need to loop over all goal vertices and see which is the closest one.
1410  ompl::base::Cost bestCost = objective_->infiniteCost();
1411  for (const auto &goal : graph_.getGoalVertices())
1412  {
1413  bestCost = objective_->betterCost(
1414  bestCost, objective_->motionCostHeuristic(vertex->getState(), goal->getState()));
1415  }
1416  return bestCost;
1417  }
1418 
1419  ompl::base::Cost AITstar::computeCostToGoToGoal(const std::shared_ptr<aitstar::Vertex> &vertex) const
1420  {
1421  // We need to loop over all goal vertices and see which is the closest one.
1422  ompl::base::Cost bestCost = objective_->infiniteCost();
1423  for (const auto &goal : graph_.getGoalVertices())
1424  {
1425  bestCost =
1426  objective_->betterCost(bestCost, objective_->motionCost(vertex->getState(), goal->getState()));
1427  }
1428  return bestCost;
1429  }
1430 
1431  ompl::base::Cost AITstar::computeBestCostToComeFromGoalOfAnyStart() const
1432  {
1433  // We need to loop over all start vertices and see which is the closest one.
1434  ompl::base::Cost bestCost = objective_->infiniteCost();
1435  for (const auto &start : graph_.getStartVertices())
1436  {
1437  bestCost = objective_->betterCost(bestCost, start->getCostToComeFromGoal());
1438  }
1439  return bestCost;
1440  }
1441 
1442  std::size_t AITstar::countNumVerticesInForwardTree() const
1443  {
1444  std::size_t numVerticesInForwardTree = 0u;
1445  auto vertices = graph_.getVertices();
1446  for (const auto &vertex : vertices)
1447  {
1448  if (graph_.isStart(vertex) || vertex->hasForwardParent())
1449  {
1450  ++numVerticesInForwardTree;
1451  }
1452  }
1453  return numVerticesInForwardTree;
1454  }
1455 
1456  std::size_t AITstar::countNumVerticesInReverseTree() const
1457  {
1458  std::size_t numVerticesInReverseTree = 0u;
1459  auto vertices = graph_.getVertices();
1460  for (const auto &vertex : vertices)
1461  {
1462  if (graph_.isGoal(vertex) || vertex->hasReverseParent())
1463  {
1464  ++numVerticesInReverseTree;
1465  }
1466  }
1467  return numVerticesInReverseTree;
1468  }
1469 
1470  void AITstar::invalidateCostToComeFromGoalOfReverseBranch(const std::shared_ptr<aitstar::Vertex> &vertex)
1471  {
1472  vertex->unregisterExpansionDuringReverseSearch();
1473  // Update the cost of all reverse children and remove from open.
1474  for (const auto &child : vertex->getReverseChildren())
1475  {
1476  invalidateCostToComeFromGoalOfReverseBranch(child);
1477  child->setCostToComeFromGoal(objective_->infiniteCost());
1478  child->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1479  auto reverseQueuePointer = child->getReverseQueuePointer();
1480  if (reverseQueuePointer)
1481  {
1482  reverseQueue_.remove(reverseQueuePointer);
1483  child->resetReverseQueuePointer();
1484  }
1485  }
1486  }
1487 
1488  } // namespace geometric
1489 } // namespace ompl
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
void clear() override
Clears the algorithm's internal state.
Definition: AITstar.cpp:155
bool hasAStartState() const
Returns whether the graph has a goal state.
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:348
std::vector< std::shared_ptr< Vertex > > getVertices() const
Get all vertices.
A shared pointer wrapper for ompl::base::SpaceInformation.
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.
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:184
void pop()
Remove the top element.
Definition: BinaryHeap.h:190
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.
bool getUseKNearest() const
Whether the graph uses a k-nearest connection model. If false, it uses an r-disc model.
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:484
void setBatchSize(std::size_t batchSize)
Set the batch size.
Definition: AITstar.cpp:301
std::vector< aitstar::Edge > getEdgesInQueue() const
Get the edge queue.
Definition: AITstar.cpp:497
double value() const
The value of the cost.
Definition: Cost.h:152
Representation of a solution to a planning problem.
bool hasAGoalState() const
Returns whether the graph has a goal state.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:112
aitstar::Edge getNextEdgeInQueue() const
Get the next edge in the queue.
Definition: AITstar.cpp:519
void clear()
Clear the heap.
Definition: BinaryHeap.h:176
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:259
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
const std::vector< std::shared_ptr< Vertex > > & getStartVertices() const
Get the start vertices.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:239
std::size_t getNumberOfValidSamples() const
Returns the total number of valid samples found.
std::string name_
The name of this planner.
Definition: Planner.h:490
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void getContent(std::vector< _T > &content) const
Get the data stored in this heap.
Definition: BinaryHeap.h:271
void clear()
Resets the graph to its construction state, without resetting options.
std::vector< std::shared_ptr< Vertex > > addSamples(std::size_t numNewSamples)
Adds a batch of samples and returns the samples it has added.
double getRewireFactor() const
Get the reqire factor of the RGG.
A class to store the exit status of Planner::solve()
bool areApproximateSolutionsTracked() const
Get whether approximate solutions are tracked.
Definition: AITstar.cpp:334
void enablePruning(bool prune)
Set whether pruning is enabled or not.
Definition: AITstar.cpp:339
std::size_t getNumberOfSampledStates() const
Returns the total number of sampled states.
void setRepairReverseSearch(bool repairReverseSearch)
Enable LPA* repair of reverse search.
Definition: AITstar.cpp:359
void getPlannerData(base::PlannerData &data) const override
Get the planner data.
Definition: AITstar.cpp:256
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:504
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInReverseSearchTree() const
Get the vertices in the reverse search tree.
Definition: AITstar.cpp:539
bool isStart(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a start vertex.
void setUseKNearest(bool useKNearest)
Whether to use a k-nearest connection model. If false, it uses an r-disc model.
double getRewireFactor() const
Get the rewire factor of the RGG graph.
Definition: AITstar.cpp:316
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:152
void prune()
Prune all samples that can not contribute to a solution better than the current one.
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
const std::vector< std::shared_ptr< Vertex > > & getGoalVertices() const
Get the goal vertices.
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:204
void remove(Element *element)
Remove a specific element.
Definition: BinaryHeap.h:196
std::shared_ptr< aitstar::Vertex > getNextVertexInQueue() const
Get the next vertex in the queue.
Definition: AITstar.cpp:529
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInQueue() const
Get the vertex queue.
Definition: AITstar.cpp:504
PlannerInputStates pis_
Utility class to extract valid input states
Definition: Planner.h:487
bool isPruningEnabled() const
Get whether pruning is enabled or not.
Definition: AITstar.cpp:344
StatusType
The possible values of the status returned by a planner.
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:481
std::vector< std::shared_ptr< Vertex > > getNeighbors(const std::shared_ptr< Vertex > &vertex) const
Get neighbors of a vertex.
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...
std::size_t getBatchSize() const
Get the batch size.
Definition: AITstar.cpp:306
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:355
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
ompl::base::Cost bestCost() const
Get the cost of the incumbent solution.
Definition: AITstar.cpp:251
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:250
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG.
bool isGoal(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a goal vertex.
void setup() override
Additional setup that can only be done once a problem definition is set.
Definition: AITstar.cpp:104
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 addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override
Solves a motion planning problem.
Definition: AITstar.cpp:171
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 setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG graph.
Definition: AITstar.cpp:311
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:123
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
void trackApproximateSolutions(bool track)
Set whether to track approximate solutions.
Definition: AITstar.cpp:321