De-inhertied RRT-Connect

Author: twill777

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

@@ -75,7 +75,7 @@ namespace ompl
         {
         public:
             /** \brief Constructor */
-            AORRTC(const base::SpaceInformationPtr &si, bool addIntermediateStates = false);
+            AORRTC(const base::SpaceInformationPtr &si);
 
             ~AORRTC() override;
 
@@ -114,20 +114,6 @@ namespace ompl
                 return psk_;
             }
 
-            /** \brief Return true if the intermediate states generated along motions are to be added to the tree itself
-             */
-            bool getIntermediateStates() const
-            {
-                return addIntermediateStates_;
-            }
-
-            /** \brief Specify whether the intermediate states generated along motions are to be added to the tree
-             * itself */
-            void setIntermediateStates(bool addIntermediateStates)
-            {
-                addIntermediateStates_ = addIntermediateStates;
-            }
-
             /** \brief Attempt to simplify the current solution path. Stop computation when \e ptc becomes true at the
              * latest. */
             void simplifySolution(const base::PathPtr &p, const base::PlannerTerminationCondition &ptc);
@@ -154,9 +140,6 @@ namespace ompl
 
             base::PathPtr bestPath_{nullptr};
 
-            /** \brief Flag indicating whether intermediate states are added to the built tree of motions */
-            bool addIntermediateStates_;
-
             ompl::base::Cost bestCost_{std::numeric_limits<double>::infinity()};
 
             /** \brief Objective we're optimizing */

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

@@ -76,11 +76,11 @@ namespace ompl
         */
 
         /** \brief Modified RRT-Connect for AORRTC (AOXRRTConnect) */
-        class AOXRRTConnect : public ompl::geometric::RRTConnect
+        class AOXRRTConnect : public base::Planner
         {
         public:
             /** \brief Constructor */
-            AOXRRTConnect(const base::SpaceInformationPtr &si, bool addIntermediateStates = false);
+            AOXRRTConnect(const base::SpaceInformationPtr &si);
 
             ~AOXRRTConnect() override;
 
@@ -96,6 +96,22 @@ namespace ompl
                 return foundPath;
             }
 
+            /** \brief Set the range the planner is supposed to use.
+
+                This parameter greatly influences the runtime of the
+                algorithm. It represents the maximum length of a
+                motion to be added in the tree of motions. */
+            void setRange(double distance)
+            {
+                maxDistance_ = distance;
+            }
+
+            /** \brief Get the range the planner is using */
+            double getRange() const
+            {
+                return maxDistance_;
+            }
+
             /** \brief Set a different nearest neighbors datastructure */
             template <template <typename T> class NN>
             void setNearestNeighbors()
@@ -161,6 +177,20 @@ namespace ompl
                 bool start;
             };
 
+            /** \brief The state of the tree after an attempt to extend it */
+            enum GrowState
+            {
+                /// no progress has been made
+                TRAPPED,
+                /// progress has been made towards the randomly sampled state
+                ADVANCED,
+                /// the randomly sampled state was reached
+                REACHED
+            };
+
+            /** \brief Free the memory allocated by this planner */
+            void freeMemory();
+
             /** \brief Compute euclidian distance between motions */
             double euclideanDistanceFunction(const Motion *a, const Motion *b) const
             {
@@ -195,6 +225,12 @@ namespace ompl
             /** \brief The goal tree */
             TreeData tGoal_;
 
+            /** \brief A flag that toggles between expanding the start tree (true) or goal tree (false). */
+            bool startTree_{true};
+
+            /** \brief The maximum length of a motion to be added to a tree */
+            double maxDistance_{0.};
+
             /** \brief Maximum allowed cost resampling iterations before moving on */
             long int maxResampleAttempts_{100};
 
@@ -224,6 +260,12 @@ namespace ompl
 
             /** \brief Objective we're optimizing */
             base::OptimizationObjectivePtr opt_;
+
+            /** \brief The random number generator */
+            RNG rng_;
+
+            /** \brief The pair of states in each tree connected during planning.  Used for PlannerData computation */
+            std::pair<base::State *, base::State *> connectionPoint_;
         };
     }  // namespace geometric
 }  // namespace ompl

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

@@ -39,13 +39,11 @@
 #include "ompl/base/goals/GoalSampleableRegion.h"
 #include "ompl/tools/config/SelfConfig.h"
 
-ompl::geometric::AORRTC::AORRTC(const base::SpaceInformationPtr &si, bool addIntermediateStates)
+ompl::geometric::AORRTC::AORRTC(const base::SpaceInformationPtr &si)
   : base::Planner(si, "AORRTC")
 {
     specs_.approximateSolutions = true;
     specs_.directed = true;
-
-    addIntermediateStates_ = addIntermediateStates;
 }
 
 ompl::geometric::AORRTC::~AORRTC()
@@ -73,7 +71,6 @@ void ompl::geometric::AORRTC::setup()
     aox_planner->setProblemDefinition(pdef_);
     aox_planner->setName("AOXRRTConnect");
     aox_planner->setRange(getRange());
-    aox_planner->setIntermediateStates(getIntermediateStates());
 
     aox_planner->setup();
 

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

@@ -44,19 +44,15 @@
 
 #include <stdexcept>
 
-ompl::geometric::AOXRRTConnect::AOXRRTConnect(const base::SpaceInformationPtr &si, bool addIntermediateStates)
-  : ompl::geometric::RRTConnect(si, addIntermediateStates)
+ompl::geometric::AOXRRTConnect::AOXRRTConnect(const base::SpaceInformationPtr &si)
+  : base::Planner(si, "AOXRRTConnect")
 {
     specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
     specs_.directed = true;
 
     Planner::declareParam<double>("range", this, &AOXRRTConnect::setRange, &AOXRRTConnect::getRange, "0.:1.:10000.");
-    Planner::declareParam<bool>("intermediate_states", this, &AOXRRTConnect::setIntermediateStates,
-                                &AOXRRTConnect::getIntermediateStates, "0,1");
 
     connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
-    distanceBetweenTrees_ = std::numeric_limits<double>::infinity();
-    addIntermediateStates_ = addIntermediateStates;
 }
 
 ompl::geometric::AOXRRTConnect::~AOXRRTConnect()
@@ -96,6 +92,33 @@ void ompl::geometric::AOXRRTConnect::setup()
     reset(true);
 }
 
+void ompl::geometric::AOXRRTConnect::freeMemory()
+{
+    std::vector<Motion *> motions;
+
+    if (tStart_)
+    {
+        tStart_->list(motions);
+        for (auto &motion : motions)
+        {
+            if (motion->state != nullptr)
+                si_->freeState(motion->state);
+            delete motion;
+        }
+    }
+
+    if (tGoal_)
+    {
+        tGoal_->list(motions);
+        for (auto &motion : motions)
+        {
+            if (motion->state != nullptr)
+                si_->freeState(motion->state);
+            delete motion;
+        }
+    }
+}
+
 void ompl::geometric::AOXRRTConnect::reset(bool solvedProblem)
 {
     tStart_->clear();
@@ -263,42 +286,14 @@ ompl::geometric::AOXRRTConnect::GrowState ompl::geometric::AOXRRTConnect::growTr
         }
     }
 
-    if (addIntermediateStates_)
-    {
-        const base::State *astate = tgi.start ? nmotion->state : dstate;
-        const base::State *bstate = tgi.start ? dstate : nmotion->state;
-
-        std::vector<base::State *> states;
-        const unsigned int count = si_->getStateSpace()->validSegmentCount(astate, bstate);
-
-        if (si_->getMotionStates(astate, bstate, states, count, true, true))
-            si_->freeState(states[0]);
-
-        for (std::size_t i = 1; i < states.size(); ++i)
-        {
-            auto *motion = new Motion;
-            motion->state = states[i];
-            motion->parent = nmotion;
-            motion->root = nmotion->root;
-            motion->cost = si_->distance(nmotion->state, motion->state) + nmotion->cost;
-            tree->add(motion);
-
-            nmotion = motion;
-        }
+    auto *motion = new Motion(si_);
+    si_->copyState(motion->state, dstate);
+    motion->parent = nmotion;
+    motion->root = nmotion->root;
+    motion->cost = si_->distance(nmotion->state, motion->state) + nmotion->cost;
+    tree->add(motion);
 
-        tgi.xmotion = nmotion;
-    }
-    else
-    {
-        auto *motion = new Motion(si_);
-        si_->copyState(motion->state, dstate);
-        motion->parent = nmotion;
-        motion->root = nmotion->root;
-        motion->cost = si_->distance(nmotion->state, motion->state) + nmotion->cost;
-        tree->add(motion);
-
-        tgi.xmotion = motion;
-    }
+    tgi.xmotion = motion;
 
     si_->freeState(root_motion->state);
     delete root_motion;