Updated order to perform straight line check before search and fixed PDT

visualize support
 - Switch order of Extend and Connect step in search to start with
   straight line check
 - Unified initial and aox planners into one aox planner
 - Cleaned up planner restart functionality for pdt visualize
 - Should fix nearest neighbour crash

Author: twill777

src/ompl/geometric/planners/rrt/AORRTC.h

@@ -42,7 +42,6 @@
 #include "ompl/base/OptimizationObjective.h"
 #include "ompl/geometric/PathSimplifier.h"
 
-#include "ompl/geometric/planners/rrt/RRTConnect.h"
 #include "ompl/geometric/planners/rrt/AOXRRTConnect.h"
 
 namespace ompl
@@ -88,6 +87,8 @@ namespace ompl
 
             void setup() override;
 
+            void reset(bool solvedProblem);
+
             void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override;
 
             void setRange(double distance)
@@ -138,7 +139,6 @@ namespace ompl
             /** \brief The random number generator */
             RNG rng_;
 
-            std::shared_ptr<ompl::geometric::RRTConnect> init_planner;
             std::shared_ptr<ompl::geometric::AOXRRTConnect> aox_planner;
 
             /** \brief Free the memory allocated by this planner */
@@ -164,7 +164,6 @@ namespace ompl
 
             double initCost_;
             ompl::base::PlannerStatus solve_status;
-            ompl::base::PlannerStatus aox_solve_status;
         };
     }  // namespace geometric
 }  // namespace ompl

src/ompl/geometric/planners/rrt/AOXRRTConnect.h

@@ -108,8 +108,26 @@ namespace ompl
                 setup();
             }
 
+            /** \brief Check if the inner loop planner met its  condition to terminate */
+            bool internalResetCondition()
+            {
+                bool shouldReset = false;
+                if (tStart_ && tGoal_) {
+                    // Reset if we have met our maximum internal vertices
+                    shouldReset = shouldReset || (tStart_->size() + tGoal_->size() >= maxInternalVertices);
+                } else {
+                    // If our trees don't exist, we're not in the middle of a search anyways
+                    shouldReset = true;
+                }
+                // Reset if we have attempted our maximum internal samples
+                shouldReset = shouldReset || (sampleAttempts >= maxInternalSamples);
+                return shouldReset;
+            }
+
             void setup() override;
 
+            void reset(bool solvedProblem);
+
             void setPathCost(double pc);
 
         protected:
@@ -143,8 +161,14 @@ namespace ompl
                 bool start;
             };
 
-            /** \brief Compute distance between motions (actually distance between contained states) */
-            double distanceFunction(const Motion *a, const Motion *b) const
+            /** \brief Compute euclidian distance between motions */
+            double euclideanDistanceFunction(const Motion *a, const Motion *b) const
+            {
+                return si_->distance(a->state, b->state);
+            }
+
+            /** \brief Compute AOX distance between motions */
+            double aoxDistanceFunction(const Motion *a, const Motion *b) const
             {
                 auto space_diff = si_->distance(a->state, b->state);
                 auto cost_diff = a->cost - b->cost;
@@ -163,23 +187,39 @@ namespace ompl
             /** \brief State sampler */
             base::InformedSamplerPtr sampler_;
 
+            std::size_t sampleAttempts{0};
+
             /** \brief The start tree */
             TreeData tStart_;
 
             /** \brief The goal tree */
             TreeData tGoal_;
 
-            /** \brief Maximum allowed cost resampling iterations */
-            int maxResampleAttempts_{100};
+            /** \brief Maximum allowed cost resampling iterations before moving on */
+            long int maxResampleAttempts_{100};
+
+            /** \brief Maximum allowed total vertices in trees before the search is restarted */
+            std::size_t maxInternalVertices{10000};
+
+            /** \brief Increment by which maxVertices is increased */
+            std::size_t maxInternalVerticesIncrement{10000};
+
+            /** \brief Maximum samples tried before the search is restarted */
+            std::size_t maxInternalSamples{10000000};
+            
+            /** \brief Increment by which maxSamples is increased */
+            std::size_t maxInternalSamplesIncrement{10000000};
 
             base::State *startState{nullptr};
             base::State *goalState{nullptr};
 
             /** \brief Best cost found so far by algorithm */
-            base::Cost bestCost_{std::numeric_limits<double>::quiet_NaN()};
+            base::Cost bestCost_{std::numeric_limits<double>::infinity()};
 
+            /** \brief Path found by the algorithm */
             base::PathPtr foundPath{nullptr};
 
+            /** \brief Outer loop termination condition for AORRTC */
             base::PlannerTerminationCondition _ptc{nullptr};
 
             /** \brief Objective we're optimizing */

src/ompl/geometric/planners/rrt/src/AORRTC.cpp

@@ -68,43 +68,41 @@ ompl::base::Cost ompl::geometric::AORRTC::bestCost() const
 void ompl::geometric::AORRTC::setup()
 {
     Planner::setup();
-    init_planner = std::make_shared<ompl::geometric::RRTConnect>(si_);
-    init_planner->setProblemDefinition(pdef_);
-    init_planner->setName("Initial RRTConnect");
-    init_planner->setRange(getRange());
-    init_planner->setIntermediateStates(getIntermediateStates());
 
     aox_planner = std::make_shared<ompl::geometric::AOXRRTConnect>(si_);
     aox_planner->setProblemDefinition(pdef_);
     aox_planner->setName("AOXRRTConnect");
     aox_planner->setRange(getRange());
     aox_planner->setIntermediateStates(getIntermediateStates());
 
-    init_planner->setup();
     aox_planner->setup();
 
     psk_ = std::make_shared<PathSimplifier>(si_);
-
     initCost_ = std::numeric_limits<double>::infinity();
+    solve_status = base::PlannerStatus::TIMEOUT;
+}
 
+// Reset our planner to begin a new search
+void ompl::geometric::AORRTC::reset(bool solvedProblem)
+{
+    aox_planner->clearQuery();
+    aox_planner->setFoundPath(nullptr);
+    aox_planner->reset(solvedProblem);
     solve_status = base::PlannerStatus::TIMEOUT;
-    aox_solve_status = base::PlannerStatus::TIMEOUT;
 }
 
 void ompl::geometric::AORRTC::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
 {
     pdef_ = pdef;
 
-    if (init_planner != nullptr)
+    if (aox_planner != nullptr)
     {
-        init_planner->setProblemDefinition(pdef);
         aox_planner->setProblemDefinition(pdef);
     }
 }
 
 void ompl::geometric::AORRTC::freeMemory()
 {
-    init_planner->clear();
     aox_planner->clear();
 }
 
@@ -126,21 +124,21 @@ ompl::base::PlannerStatus ompl::geometric::AORRTC::solve(const base::PlannerTerm
 {
     do
     {
+        // If we already have a solution (and therefore a reasonable cost range)
         if (pdef_->hasExactSolution())
         {
+            // Inform the planner about our cost bound
             aox_planner->setPathCost(bestCost_.value());
+            
+            // Try to solve the planning problem
+            solve_status = aox_planner->solve(ptc);
 
-            OMPL_INFORM("%s: Best cost is %.5f", getName().c_str(), bestCost_);
-            aox_solve_status = aox_planner->solve(ptc);
-
-            if (aox_solve_status != base::PlannerStatus::EXACT_SOLUTION)
-            {
-                OMPL_INFORM("%s: Restarted before finding a solution", getName().c_str());
-            }
-            else
+            if (solve_status == base::PlannerStatus::EXACT_SOLUTION)
             {
+                // If we found a solution, extract it and update our path
                 const base::PathPtr path = aox_planner->getFoundPath();
 
+                // Update our best path if our new path is better
                 if (path->length() < bestCost_.value())
                 {
                     bestPath_ = path;
@@ -149,34 +147,29 @@ ompl::base::PlannerStatus ompl::geometric::AORRTC::solve(const base::PlannerTerm
                     pdef_->addSolutionPath(bestPath_, false, 0.0, getName());
                 }
 
+                // Simplify our solution
                 simplifySolution(path, ptc);
-                OMPL_INFORM("%s: AOX search simplified to cost %.5f", getName().c_str(), path->length());
 
+                // Again, update our best path if our new (simplified) path is better
                 if (path->length() < bestCost_.value())
                 {
+                    OMPL_INFORM("%s: AOX search simplified to cost %.5f", getName().c_str(), path->length());
+
                     bestPath_ = path;
                     bestCost_ = ompl::base::Cost(path->length());
 
                     pdef_->addSolutionPath(bestPath_, false, 0.0, getName());
                 }
-
-                aox_planner->clearQuery();
-                aox_planner->setProblemDefinition(pdef_);
-            }
-
-            aox_planner->setFoundPath(nullptr);
-            aox_solve_status = base::PlannerStatus::TIMEOUT;
+            }            
         }
 
+        // Find an initial solution
         else
         {
-            solve_status = init_planner->solve(ptc);
+            solve_status = aox_planner->solve(ptc);
 
-            if (solve_status != base::PlannerStatus::EXACT_SOLUTION)
-            {
-                OMPL_INFORM("%s: Initial search restarted before finding a solution", getName().c_str());
-            }
-            else
+            // If we found a solution, extract it and update our path
+            if (solve_status == base::PlannerStatus::EXACT_SOLUTION)
             {
                 initCost_ = pdef_->getSolutionPath()->length();
                 bestPath_ = pdef_->getSolutionPath();
@@ -192,19 +185,48 @@ ompl::base::PlannerStatus ompl::geometric::AORRTC::solve(const base::PlannerTerm
                 pdef_->addSolutionPath(bestPath_, false, 0.0, getName());
             }
         }
+
+        // If we ran into errors, exit early
+        if (solve_status == base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE ||
+            solve_status == base::PlannerStatus::INVALID_START ||
+            solve_status == base::PlannerStatus::INVALID_GOAL)
+        {
+            return solve_status;
+        }
+
+        // If we found a solution, we can reset our planner
+        if (solve_status == base::PlannerStatus::EXACT_SOLUTION)
+        {
+            reset(true);
+        }
+
+        // If we didn't find a solution on this iteration of solve, reset our planner.
+        // (PDT visualize calls solve for each iteration of the search,
+        // so we need to check if our planner actually needs to reset)
+        else if (aox_planner->internalResetCondition())
+        {
+            OMPL_INFORM("%s: Restarted before finding a solution", getName().c_str());
+            reset(false);
+        }
+
+        // Something has surely gone wrong if you have found a zero-length path
+        if (bestCost_.value() == 0)
+        {
+            OMPL_ERROR("%s: Zero-length path found. May have a common start/goal.", getName().c_str());
+            return base::PlannerStatus::INVALID_GOAL;
+        }
     } while (!ptc);
 
-    return solve_status;
+    // Now that we're done, return whether or not we timed out
+    if (pdef_->hasExactSolution())
+    {
+        return base::PlannerStatus::EXACT_SOLUTION;
+    } else {
+        return base::PlannerStatus::TIMEOUT;
+    }
 }
 
 void ompl::geometric::AORRTC::getPlannerData(base::PlannerData &data) const
 {
-    if (pdef_->hasExactSolution())
-    {
-        aox_planner->getPlannerData(data);
-    }
-    else
-    {
-        init_planner->getPlannerData(data);
-    }
+    aox_planner->getPlannerData(data);
 }