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