Commits

Ioan Sucan  committed ce365e2 Merge

merge from default

  • Participants
  • Parent commits 032c050, 242b931
  • Branches lazy_prm

Comments (0)

Files changed (10)

File CMakeModules/PythonBindingsUtils.cmake

 find_package(GCCXML QUIET)
 
 if(APPLE)
-    # For some reason gccxml generates 32-bit binaries by default on OS X
-    # (maybe because the kernel is still 32-bit?)
-    set(PYOMPL_EXTRA_CFLAGS "-m64")
+    # The latest gccxml can be compiled with clang, but cannot be run by
+    # pretending to be clang. Instead, we have to explicitly tell it to
+    # pretend to be g++. Gccxml also mistakenly thinks that OS X is a 32-bit
+    # architecture.
+    set(PYOMPL_EXTRA_CFLAGS "--gccxml-compiler g++ -m64")
 endif(APPLE)
 
 if(PYTHON_FOUND AND Boost_PYTHON_LIBRARY)

File src/ompl/base/PlannerTerminationCondition.h

             /** \brief Construct a termination condition. By default, eval() will call the externally specified function \e fn to decide whether
                 the planner should terminate. The function \e fn does not always need to be specified, if a different implementation of eval() is
                 provided by a derived class. */
-            PlannerTerminationCondition(const PlannerTerminationConditionFn &fn = PlannerTerminationConditionFn()) : fn_(fn), terminate_(false)
+            PlannerTerminationCondition(const PlannerTerminationConditionFn &fn) : fn_(fn), terminate_(false)
             {
             }
 
                 the condition (the call to computeEval()) consists of
                 calling \e fn().*/
             PlannerThreadedTerminationCondition(const PlannerTerminationConditionFn &fn, double period);
+	    
+	    /** \brief Copy constructor creates another thread to fully duplicate the condition */
+            PlannerThreadedTerminationCondition(const PlannerThreadedTerminationCondition &other);
 
             virtual ~PlannerThreadedTerminationCondition(void);
 
             /** \brief Simply return the cached value for the termination condition (evalValue_) */
             virtual bool eval(void) const;
+            
+	    /** \brief Duplicate the evaluation thread when a condition is copied */
+	    PlannerThreadedTerminationCondition& operator=(const PlannerThreadedTerminationCondition &other);
 
         protected:
 

File src/ompl/base/src/PlannerTerminationCondition.cpp

 
 void ompl::base::PlannerThreadedTerminationCondition::stopEvalThread(void)
 {
+    terminate_ = true;
     if (thread_)
     {
-        thread_->interrupt();
         thread_->join();
         delete thread_;
         thread_ = NULL;
     startEvalThread();
 }
 
+ompl::base::PlannerThreadedTerminationCondition::PlannerThreadedTerminationCondition(const PlannerThreadedTerminationCondition &other) :
+    PlannerTerminationCondition(other), thread_(NULL), evalValue_(other.evalValue_), period_(other.period_)
+{
+    startEvalThread();
+}
+
 ompl::base::PlannerThreadedTerminationCondition::~PlannerThreadedTerminationCondition(void)
 {
-    terminate_ = true;
     stopEvalThread();
 }
 
+ompl::base::PlannerThreadedTerminationCondition& ompl::base::PlannerThreadedTerminationCondition::operator=(const PlannerThreadedTerminationCondition &other)
+{
+    if (this != &other)
+    {
+	stopEvalThread();
+	static_cast<PlannerTerminationCondition&>(*this) = static_cast<const PlannerTerminationCondition&>(other);
+	thread_ = NULL;
+	evalValue_ = other.evalValue_;
+	period_ = other.period_;  
+	startEvalThread();
+    }
+    return *this;
+}
+
 void ompl::base::PlannerThreadedTerminationCondition::periodicEval(void)
 {
+    // we want to check for termination at least once every ms;
+    // even though we may evaluate the condition itself more rarely
+
+    unsigned int count = 1;
     time::duration s = time::seconds(period_);
-    do
+    if (period_ > 0.001)
     {
-        evalValue_ = computeEval();
-        if ((*this)())
-            break;
-        boost::this_thread::sleep(s);
-    } while (!(*this)());
+	count = 0.5 + period_ / 0.001;
+	s = time::seconds(period_ / (double) count);
+    }
+
+    if (!(*this)())
+	do
+	{
+	    evalValue_ = computeEval();
+	    for (unsigned int i = 0 ; i < count ; ++i)
+	    {
+		if ((*this)())
+		    break;
+		boost::this_thread::sleep(s);
+	    }
+	} while (!(*this)());
 }
 
 ompl::base::PlannerTerminationCondition ompl::base::timedPlannerTerminationCondition(double duration)

File src/ompl/control/PathControl.h

             /** @name Path operations
                 @{ */
 
-            /** \brief Append \e state to the end of the path; it is assumed \e state is the last state, so no control is applied.
+            /** \brief Append \e state to the end of the path; it is assumed \e state is the first state, so no control is applied.
                 The memory for \e state is copied. There are no checks to make sure the number of controls and states make sense. */
             void append(const base::State *state);
 

File src/ompl/control/planners/pdst/PDST.h

 /* Author: Jonathan Sobieski, Mark Moll */
 
 
-
 #ifndef OMPL_CONTROL_PLANNERS_PDST_PDST_
 #define OMPL_CONTROL_PLANNERS_PDST_PDST_
 
             segments. Unlike most tree-based planners which expand from
             a randomly select endpoint of a path segment, PDST expands
             from a randomly selected point along a deterministically
-            selected path segment. It is important to set the projection
+            selected path segment. Because of this, it is recommended
+            to increase the min. and max. control duration using
+            ompl::control::SpaceInformation::setMinMaxControlDuration.
+            It is important to set the projection
             the algorithm uses (setProjectionEvaluator() function). If
             no projection is set, the planner will attempt to use the
             default projection associated to the state space. An
             struct Motion
             {
             public:
-                Motion(base::State *state, Motion *parent, Control *control,
-                    unsigned int controlDuration, ompl::base::EuclideanProjection& projection,
-                    double priority, Cell* cell)
-                    : priority_(priority), cell_(cell), parent_(parent), state_(state),
-                    control_(control), controlDuration_(controlDuration), projection_(projection),
-                    heapElement_(NULL)
+                Motion(base::State *startState, base::State *endState, Control *control,
+                    unsigned int controlDuration, double priority, Motion* parent)
+                    : startState_(startState), endState_(endState), control_(control),
+                    controlDuration_(controlDuration), priority_(priority), parent_(parent),
+                    cell_(NULL), heapElement_(NULL), isSplit_(false)
                 {
                 }
-                Motion(base::State *state, const ompl::base::ProjectionEvaluatorPtr& pe)
-                    : priority_(0.), cell_(NULL), parent_(NULL), state_(state), control_(NULL),
-                    controlDuration_(0), projection_(pe->getDimension()), heapElement_(NULL)
+                /// constructor for start states
+                Motion(base::State *state)
+                    : startState_(state), endState_(state), control_(NULL), controlDuration_(0),
+                    priority_(0.), parent_(NULL), cell_(NULL), heapElement_(NULL), isSplit_(false)
                 {
-                    pe->project(state_, projection_);
                 }
-
+                /// The score is used to order motions in a priority queue.
                 double score(void) const
                 {
                     return priority_ / cell_->volume_;
                 }
                 void updatePriority(void)
                 {
-                    priority_ = priority_ * 2.0 + 1.0;
+                    priority_ = priority_ * 2. + 1.;
                 }
 
-                /// \brief Split a motion into two parts. The first part is
-                /// duration steps long (and is returned), the second part is the
-                /// remaing part of the motion and is kept in this object. It
-                /// is assumed the motion is contained within one cell.
-                Motion* split(const SpaceInformation* si, unsigned int duration,
-                    const ompl::base::ProjectionEvaluatorPtr& pe)
-                {
-                    Motion* motion = new Motion(si->allocState(), parent_,
-                        si->cloneControl(control_), duration, projection_,
-                        priority_, cell_);
-                    si->propagate(parent_->state_, control_, duration, motion->state_);
-                    pe->project(motion->state_, motion->projection_);
-                    cell_->motions_.push_back(motion);
-                    parent_ = motion;
-                    controlDuration_ -= duration;
-                    return motion;
-                }
-                /// \brief Split a motion into two parts. The first part is
-                /// duration steps long (and is returned), the second part is the
-                /// remaing part of the motion (kept in this object). The state
-                /// reached by the first part and corresponding projection and
-                /// cell are specified by the rest of the arguments.
-                Motion* split(const SpaceInformation* si, unsigned int duration,
-                    base::State* state, base::EuclideanProjection& projection, Cell* cell)
-                {
-                    Motion* motion = new Motion(state, parent_, si->cloneControl(control_),
-                        duration, projection, priority_, cell);
-                    cell->motions_.push_back(motion);
-                    parent_ = motion;
-                    controlDuration_ -= duration;
-                    return motion;
-                }
-
+                /// The starting point of this motion
+                ompl::base::State*               startState_;
+                /// The state reached by this motion
+                ompl::base::State*               endState_;
+                /// The control that was applied to arrive at this state from the parent
+                ompl::control::Control*          control_;
+                /// The duration that the control was applied to arrive at this state from the parent
+                unsigned int                     controlDuration_;
                 /// Priority for selecting this path to extend from in the future
                 double                           priority_;
-
-                /// pointer to the cell that contains this path
-                Cell*                            cell_;
-
                 /// Parent motion from which this one started
                 Motion*                          parent_;
-
-                /// The state achieved by this motion
-                ompl::base::State*               state_;
-
-                /// The control that was applied to arrive at this state from the parent
-                ompl::control::Control*          control_;
-
-                /// The duration that the control was applied to arrive at this state from the parent
-                unsigned int                     controlDuration_;
-
-                /// The projection for this Motion
-                ompl::base::EuclideanProjection  projection_;
-
+                /// Pointer to the cell that contains this path
+                Cell*                            cell_;
                 /// Handle to the element of the priority queue for this Motion
-                ompl::BinaryHeap<Motion *, MotionCompare>::Element* heapElement_;
+                ompl::BinaryHeap<Motion*, MotionCompare>::Element* heapElement_;
+                /// Whether this motion is the result of a split operation, in which case
+                /// its endState_ and control_ should not be freed.
+                bool                             isSplit_;
             };
 
             /// Cell is a Binary Space Partition
                 ~Cell()
                 {
                     if (left_)
+                    {
                         delete left_;
-                    if (right_)
                         delete right_;
+                    }
                 }
 
                 /// Subdivides this cell
             };
 
 
-            /// Inserts the motion into the appropriate cell
-            void insertSampleIntoBsp(Motion *motion, Cell *bsp,
-                base::State* scratch1, base::State* scratch2);
-
+            /// \brief Inserts the motion into the appropriate cells, splitting the motion as necessary.
+            /// \e motion is assumed to be fully contained within \e cell.
+            void addMotion(Motion *motion, Cell *cell,
+                base::State*, base::State*, base::EuclideanProjection&, base::EuclideanProjection&);
+            /// \brief Either update heap after motion's priority has changed or insert motion into heap.
+            void updateHeapElement(Motion* motion)
+            {
+                if (motion->heapElement_)
+                    priorityQueue_.update(motion->heapElement_);
+                else
+                    motion->heapElement_ = priorityQueue_.insert(motion);
+            }
             /// \brief Select a state along motion and propagate a new motion from there.
             /// Return NULL if no valid motion could be generated starting at the
             /// selected state.
-            Motion* propagateFrom(Motion* motion, bool& goalReached,
-                double& distanceToGoal, base::State* scratch);
+            Motion* propagateFrom(Motion* motion, base::State*, base::State*);
+            /// \brief Find the max. duration that the control_ in motion can be applied s.t.
+            /// the trajectory passes through state. This means that "ancestor" motions with
+            /// the same control_ are also considered. A pointer to the oldest ancestor with
+            /// the same control_ is returned. Upon return applying the control
+            /// \e ancestor->control_ for duration steps starting from the state
+            /// \e ancestor->startState_ should result in the state \e state.
+            unsigned int findDurationAndAncestor(Motion* motion, base::State* state,
+                base::State* scratch, Motion*& ancestor) const;
 
             void freeMotion(Motion* m)
             {
-                if (m->state_)
-                    si_->freeState(m->state_);
-                if (m->control_)
-                    siC_->freeControl(m->control_);
+                if (m->parent_)
+                {
+                    if (m->startState_ != m->parent_->endState_)
+                        si_->freeState(m->startState_);
+                    if (!m->isSplit_)
+                        siC_->freeControl(m->control_);
+                }
+                if (!m->isSplit_)
+                    si_->freeState(m->endState_);
                 delete m;
             }
 
 
             /// State sampler
             ompl::base::StateSamplerPtr              sampler_;
-
             /// Directed control sampler
             DirectedControlSamplerPtr                controlSampler_;
-
             /// SpaceInformation convenience pointer
             const SpaceInformation*                  siC_;
-
             // Random number generator
-            RNG rng_;
-
+            RNG                                      rng_;
             /// \brief Vector holding all of the start states supplied for the problem
             /// Each start motion is the root of its own tree of motions.
             std::vector<Motion*>                     startMotions_;
-
             /// Priority queue of motions
             ompl::BinaryHeap<Motion*, MotionCompare> priorityQueue_;
-
             /// Binary Space Partition
-            Cell *bsp_;
-
+            Cell*                                    bsp_;
             /// Projection evaluator for the problem
             ompl::base::ProjectionEvaluatorPtr       projectionEvaluator_;
-
             /// Number between 0 and 1 specifying the probability with which the goal should be sampled
-            double goalBias_;
-
+            double                                   goalBias_;
             /// Objected used to sample the goal
             ompl::base::GoalSampleableRegion*        goalSampler_;
-
             /// Iteration number and priority of the next Motion that will be generated
             unsigned int                             iteration_;
-
             /// Closest motion to the goal
             Motion*                                  lastGoalMotion_;
-
         };
     }
 }

File src/ompl/control/planners/pdst/src/PDST.cpp

 
     // depending on how the planning problem is set up, this may be necessary
     bsp_->bounds_ = projectionEvaluator_->getBounds();
-    goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion*>(pdef_->getGoal().get());
+    base::Goal *goal = pdef_->getGoal().get();
+    goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion*>(goal);
 
     // Ensure that we have a state sampler AND a control sampler
     if (!sampler_)
     // time then initializes hasSolution to false and isApproximate to true.
     double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
     bool hasSolution = lastGoalMotion_ != NULL;
-    bool isApproximate = hasSolution ? pdef_->getGoal()->isSatisfied(lastGoalMotion_->state_, &closestDistanceToGoal) : true;
-    unsigned ndim = projectionEvaluator_->getDimension();
+    bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
+    unsigned int ndim = projectionEvaluator_->getDimension();
 
     // If an exact solution has already been found, do not run for another iteration.
     if (hasSolution && !isApproximate)
     // Initialize tree with start state(s)
     while (const base::State *st = pis_.nextStart())
     {
-        Motion *startMotion = new Motion(si_->cloneState(st), projectionEvaluator_);
+        Motion *startMotion = new Motion(si_->cloneState(st));
         bsp_->addMotion(startMotion);
         startMotion->heapElement_ = priorityQueue_.insert(startMotion);
     }
         return base::PlannerStatus::INVALID_START;
     }
 
-    base::State *scratch = si_->allocState();
-    base::State *scratch2 = si_->allocState();
-
+    base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState();
+    base::EuclideanProjection tmpProj1(ndim), tmpProj2(ndim);
     while (!ptc)
     {
         // Get the top priority path.
         motionSelected->updatePriority();
         priorityQueue_.update(motionSelected->heapElement_);
 
-        Motion *newMotion = propagateFrom(motionSelected, hasSolution, distanceToGoal, scratch);
+        Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
         if (newMotion == NULL)
             continue;
+        addMotion(newMotion, bsp_, tmpState1, tmpState2, tmpProj1, tmpProj2);
 
         // Check if the newMotion reached the goal.
+        hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal);
         if (hasSolution)
         {
             closestDistanceToGoal = distanceToGoal;
         cellSelected->subdivide(ndim);
         motions.swap(cellSelected->motions_);
         for (std::vector<Motion*>::iterator m = motions.begin() ; m != motions.end() ; ++m)
-            insertSampleIntoBsp(*m, cellSelected, scratch, scratch2);
-        iteration_++;
+            addMotion(*m, cellSelected, tmpState1, tmpState2, tmpProj1, tmpProj2);
     }
-    si_->freeState(scratch);
-    si_->freeState(scratch2);
-
 
     if (lastGoalMotion_ != NULL)
         hasSolution = true;
     // If a solution path has been computed, save it in the problem definition object.
     if (hasSolution)
     {
-        double dt = siC_->getPropagationStepSize();
-        PathControl *path = new PathControl(si_);
+        Motion* m;
+        std::vector<unsigned int> durations(1,
+            findDurationAndAncestor(lastGoalMotion_, lastGoalMotion_->endState_, tmpState1, m));
+        std::vector<Motion*> mpath(1, m);
 
-        // Compute the path by going up the tree of motions.
-        std::vector<Motion*> mpath;
-        Motion *solution = lastGoalMotion_;
-        while (solution->parent_ != NULL)
+        while (m->parent_)
         {
-            mpath.push_back(solution);
-            solution = solution->parent_;
+            durations.push_back(findDurationAndAncestor(m->parent_, m->startState_, tmpState1, m));
+            mpath.push_back(m);
         }
 
-        // Add the solution path in order from the start state to the goal.
-        for (std::vector<Motion*>::reverse_iterator rIt = mpath.rbegin() ; rIt != mpath.rend() ; ++rIt)
-        {
-            if ((*rIt)->parent_ != NULL)
-                path->append((*rIt)->state_, (*rIt)->control_, (*rIt)->controlDuration_ * dt);
-            else
-                path->append((*rIt)->state_);
-        }
+        PathControl *path = new PathControl(si_);
+        double dt = siC_->getPropagationStepSize();
+        path->append(mpath.back()->endState_);
+        for (int i = (int) mpath.size() - 2; i > 0; i--)
+            path->append(mpath[i-1]->startState_, mpath[i]->control_, durations[i] * dt);
+        path->append(lastGoalMotion_->endState_, mpath[0]->control_, durations[0] * dt);
         pdef_->addSolutionPath(base::PathPtr(path), isApproximate, closestDistanceToGoal);
     }
 
+    si_->freeState(tmpState1);
+    si_->freeState(tmpState2);
+
     return base::PlannerStatus(hasSolution, isApproximate);
 }
 
-ompl::control::PDST::Motion* ompl::control::PDST::propagateFrom(Motion* motion, bool& goalReached, double& distanceToGoal, base::State* scratch)
+ompl::control::PDST::Motion* ompl::control::PDST::propagateFrom(
+    Motion* motion, base::State* start, base::State* rnd)
 {
-    base::Goal *goal = pdef_->getGoal().get();
+    // sample a point along the trajectory given by motion
+    unsigned int prevDuration = motion->controlDuration_;
+    if (motion->controlDuration_ > 1)
+        prevDuration = rng_.uniformInt(1, motion->controlDuration_);
+    if (prevDuration == motion->controlDuration_)
+        start = motion->endState_;
+    else
+        siC_->propagate(motion->startState_, motion->control_, prevDuration, start);
+    // generate a random state
+    if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
+        goalSampler_->sampleGoal(rnd);
+    else
+        sampler_->sampleUniform(rnd);
+    // generate a random control
+    Control *control = siC_->allocControl();
+    unsigned int duration = controlSampler_->sampleTo(control, motion->control_, start, rnd);
+    // return new motion if duration is large enough
+    return (duration < siC_->getMinControlDuration()) ? NULL
+        : new Motion(si_->cloneState(start), si_->cloneState(rnd),
+                     control, duration, ++iteration_, motion);
+}
 
-    if (motion->controlDuration_ > 1)
+void ompl::control::PDST::addMotion(Motion *motion, Cell *bsp, base::State* prevState, base::State* state,
+    base::EuclideanProjection& prevProj, base::EuclideanProjection& proj)
+{
+    // If the motion is at most 1 step, then it cannot be split across cell bounds.
+    if (motion->controlDuration_ <= 1)
     {
-        // sample a point along the trajectory given by motion
-        unsigned int duration = rng_.uniformInt(1, motion->controlDuration_);
-        if (duration != motion->controlDuration_)
-        {
-            // split motion into two motions
-            motion = motion->split(siC_, duration, projectionEvaluator_);
-            motion->heapElement_ = priorityQueue_.insert(motion);
-        }
+        projectionEvaluator_->project(motion->endState_, proj);
+        bsp->stab(proj)->addMotion(motion);
+        updateHeapElement(motion);
+        return;
     }
 
-    // generate a random state
-    if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
-        goalSampler_->sampleGoal(scratch);
-    else
-        sampler_->sampleUniform(scratch);
-
-    // generate a random control
-    Control *randomControl = siC_->allocControl();
-    // ignore returned duration. It's only the direction we care about; we
-    // always propagate for (at most) siC_->getMaxControlDuration() steps
-    controlSampler_->sampleTo(randomControl, motion->control_,
-        motion->state_, scratch);
-
-    // propagate using the random control and split when crossing cell bounds
-    Motion *m = new Motion(si_->allocState(), motion, randomControl, 0, motion->projection_, iteration_, NULL),
-        *prevm;
-    base::State* prevState = si_->cloneState(motion->state_);
-    Cell *prevCell = motion->cell_;
-    base::EuclideanProjection prevProjection(projectionEvaluator_->getDimension());
-    std::vector<Motion*> newMotions;
-    unsigned index = 1, totalDuration = 0;
-
-    while (true)
-    {
-        siC_->propagate(prevState, randomControl, 1, m->state_);
-        if (!si_->isValid(m->state_))
-        {
-            std::swap(prevState, m->state_);
-            goalReached = goal->isSatisfied(m->state_, &distanceToGoal);
-            break;
-        }
-        projectionEvaluator_->project(m->state_, m->projection_);
-        m->cell_ = bsp_->stab(m->projection_);
-        if (m->cell_ != prevCell)
-        {
-            if (m->controlDuration_ > 0) // the first propagate call is allowed to cross a cell boundary
-            {
-                // start a new motion, save previous one
-                prevm = m;
-                m = new Motion(prevm->state_, prevm, siC_->cloneControl(randomControl),
-                    1, prevm->projection_, iteration_, m->cell_);
-                prevm->cell_= prevCell;
-                prevm->state_ = si_->cloneState(prevState);
-                prevm->projection_ = prevProjection;
-                totalDuration += prevm->controlDuration_;
-                newMotions.push_back(prevm);
-            }
-            prevCell = m->cell_;
-        }
-        else
-            ++(m->controlDuration_);
-        if ((goalReached = goal->isSatisfied(m->state_, &distanceToGoal))
-            || index == siC_->getMaxControlDuration())
-            break;
-        std::swap(prevState, m->state_);
-        prevProjection.swap(m->projection_);
-        ++index;
-    } // end while
-
-    si_->freeState(prevState);
-    // add last motion if it has non-zero duration
-    if (m->controlDuration_ > 0)
-    {
-        totalDuration += m->controlDuration_;
-        newMotions.push_back(m);
-    }
-    else
-        freeMotion(m);
-
-    // check if total motion duration is long enough
-    if (totalDuration >= siC_->getMinControlDuration())
-    {
-        // if so, add motin segments to the priority queue and bsp.
-        for (std::size_t i = 0; i < newMotions.size() ; ++i)
-        {
-            newMotions[i]->heapElement_ = priorityQueue_.insert(newMotions[i]);
-            newMotions[i]->cell_->motions_.push_back(newMotions[i]);
-        }
-        return newMotions.back();
-    }
-    else
-    {
-        // otherwise, clean up
-        for (std::size_t i = 0 ; i < newMotions.size() ; ++i)
-            freeMotion(newMotions[i]);
-        goalReached = false;
-        distanceToGoal = std::numeric_limits<double>::infinity();
-        return NULL;
-    }
-}
-
-void ompl::control::PDST::insertSampleIntoBsp(Motion *motion, Cell *bsp, base::State* prevState, base::State* state)
-{
-    bsp->stab(motion->projection_)->addMotion(motion);
-    priorityQueue_.update(motion->heapElement_);
-
-    // If the motion is at most 1 step, then it cannot be split across cell
-    // bounds. So we're done.
-    if (motion->controlDuration_ <= 1)
-        return;
-
-    // motion can cross at most one cell boundary, since bsp has just been split
-    // into two cells.
-    si_->copyState(prevState, motion->parent_->state_);
-    base::EuclideanProjection proj(projectionEvaluator_->getDimension()),
-        prevProj(projectionEvaluator_->getDimension());
     Cell *cell = NULL, *prevCell = NULL;
-
-    motion->cell_ = bsp->stab(motion->projection_);
-    priorityQueue_.update(motion->heapElement_);
-    for (unsigned int i = 0 ; i < motion->controlDuration_ ; ++i)
+    si_->copyState(prevState, motion->startState_);
+    // propagate the motion, check for cell boundary crossings, and split as necessary
+    for (unsigned int i = 0, duration = 0 ; i < motion->controlDuration_ - 1 ; ++i, ++duration)
     {
         siC_->propagate(prevState, motion->control_, 1, state);
         projectionEvaluator_->project(state, proj);
-        if (!prevCell)
+        cell = bsp->stab(proj);
+        if (duration > 0 && cell != prevCell)
         {
-            prevCell = bsp->stab(proj);
-            // if cell at the start and end are the same, then we don't split
-            if (prevCell == motion->cell_)
-                break;
-        }
-        else
-        {
-            cell = bsp->stab(proj);
-            if (cell != prevCell)
-            {
-                motion = motion->split(siC_, i, si_->cloneState(prevState), prevProj, prevCell);
-                motion->heapElement_ = priorityQueue_.insert(motion);
-                break;
-            }
+            Motion* newMotion = new Motion(motion->startState_, si_->cloneState(prevState),
+                motion->control_, duration, motion->priority_, motion->parent_);
+            newMotion->isSplit_ = true;
+            prevCell->addMotion(newMotion);
+            updateHeapElement(newMotion);
+            motion->startState_ = newMotion->endState_;
+            motion->controlDuration_ -= duration;
+            motion->parent_ = newMotion;
+            duration = 0;
         }
         std::swap(state, prevState);
         std::swap(cell, prevCell);
         proj.swap(prevProj);
     }
+    prevCell->addMotion(motion);
+    updateHeapElement(motion);
+}
+
+
+unsigned int ompl::control::PDST::findDurationAndAncestor(Motion* motion, base::State* state,
+    base::State* scratch, Motion*& ancestor) const
+{
+    const double eps = std::numeric_limits<float>::epsilon();
+    unsigned int duration;
+    ancestor = motion;
+    if (state == motion->endState_ || motion->controlDuration_ == 0 ||
+        si_->distance(motion->endState_, state) < eps)
+        duration = motion->controlDuration_;
+    else if (motion->controlDuration_ > 0 &&
+        si_->distance(motion->startState_, state) < eps)
+        duration = 0;
+    else
+    {
+        duration = 1;
+        si_->copyState(scratch, motion->startState_);
+        for (; duration <= motion->controlDuration_; ++duration)
+        {
+            siC_->propagate(scratch, motion->control_, 1, scratch);
+            if (si_->distance(scratch, state) < eps)
+                break;
+        }
+    }
+    if (duration <= motion->controlDuration_)
+    {
+        // ancestor points to the start of a split motion; duration is computed
+        // relative to start state of ancestor motion
+        while (ancestor->parent_ && ancestor->control_ == ancestor->parent_->control_)
+        {
+            ancestor = ancestor->parent_;
+            duration += ancestor->controlDuration_;
+        }
+        return duration;
+    }
+    // motion is no longer the parent of the motion starting at
+    // state because it has been split afterwards, so look for
+    // the starting point in the parent of motion.
+    return findDurationAndAncestor(motion->parent_, state, scratch, ancestor);
 }
 
 void ompl::control::PDST::clear(void)
     iteration_ = 1;
     lastGoalMotion_ = NULL;
     freeMemory();
-    bsp_ = new Cell(1.0, projectionEvaluator_->getBounds(), 0);
+    bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
 }
 
 void ompl::control::PDST::freeMemory(void)
     sc.configureProjectionEvaluator(projectionEvaluator_);
     if (bsp_)
         delete bsp_;
-    bsp_ = new Cell(1.0, projectionEvaluator_->getBounds(), 0);
+    bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
     lastGoalMotion_ = NULL;
 }
 
 {
     base::Planner::getPlannerData(data);
 
-    double propagationStepSize = siC_->getPropagationStepSize();
+    double dt = siC_->getPropagationStepSize();
+    base::State* scratch = si_->allocState();
     std::vector<Motion*> motions;
+    motions.reserve(priorityQueue_.size());
     priorityQueue_.getContent(motions);
 
     // Add goal vertex
     if (lastGoalMotion_ != NULL)
-        data.addGoalVertex(lastGoalMotion_->state_);
+        data.addGoalVertex(lastGoalMotion_->endState_);
 
     for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it)
-    {
-        Motion *child = *it;
-        Motion *parent = child->parent_;
+        if (!(*it)->isSplit_)
+        {
+            // We only add one edge for each motion that has been split into smaller segments
+            Motion *cur = *it, *ancestor;
+            unsigned int duration = findDurationAndAncestor(cur, cur->endState_, scratch, ancestor);
 
-        if (parent == NULL)
-            data.addStartVertex(base::PlannerDataVertex(child->state_));
-        else if (data.hasControls())
-            data.addEdge(base::PlannerDataVertex(parent->state_),
-                base::PlannerDataVertex(child->state_),
-                PlannerDataEdgeControl(child->control_,
-                    child->controlDuration_ * propagationStepSize));
-        else
-            data.addEdge(base::PlannerDataVertex(parent->state_),
-                base::PlannerDataVertex(child->state_));
-    }
+            if (cur->parent_ == NULL)
+                data.addStartVertex(base::PlannerDataVertex(cur->endState_));
+            else if (data.hasControls())
+            {
+                data.addEdge(base::PlannerDataVertex(ancestor->startState_),
+                    base::PlannerDataVertex(cur->endState_),
+                    PlannerDataEdgeControl(cur->control_, duration * dt));
+                if (ancestor->parent_)
+                {
+                    // Include an edge between start state of parent ancestor motion and
+                    // the start state of the ancestor motion, which lies somewhere on
+                    // the parent ancestor motion.
+                    cur = ancestor;
+                    duration = findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
+                    data.addEdge(base::PlannerDataVertex(ancestor->startState_),
+                        base::PlannerDataVertex(cur->startState_),
+                        PlannerDataEdgeControl(ancestor->control_, duration * dt));
+                }
+            }
+            else
+            {
+                data.addEdge(base::PlannerDataVertex(ancestor->startState_),
+                    base::PlannerDataVertex(cur->endState_));
+                if (ancestor->parent_)
+                {
+                    // Include an edge between start state of parent ancestor motion and
+                    // the start state of the ancestor motion, which lies somewhere on
+                    // the parent ancestor motion.
+                    cur = ancestor;
+                    duration = findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
+                    data.addEdge(base::PlannerDataVertex(ancestor->startState_),
+                        base::PlannerDataVertex(cur->startState_));
+                }
+            }
+        }
+
+    si_->freeState(scratch);
 }
 
 void ompl::control::PDST::Cell::subdivide(unsigned int spaceDimension)
 {
-    double childVolume = 0.5 * volume_;
+    double childVolume = .5 * volume_;
     unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
-    splitValue_ = 0.5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);
+    splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);
 
     left_ = new Cell(childVolume, bounds_, nextSplitDimension);
     left_->bounds_.high[splitDimension_] = splitValue_;

File src/ompl/geometric/planners/pdst/PDST.h

             struct Motion
             {
             public:
-                Motion(base::State *state, Motion *parent, base::EuclideanProjection::size_type projectionDim,
-                    double priority, Cell* cell)
-                    : priority_(priority), cell_(cell), parent_(parent), state_(state),
-                    projection_(projectionDim), heapElement_(NULL)
+                Motion(base::State *startState, base::State *endState, double priority, Motion* parent)
+                    : startState_(startState), endState_(endState), priority_(priority),
+                    parent_(parent), cell_(NULL), heapElement_(NULL), isSplit_(false)
                 {
                 }
-                Motion(base::State *state, Motion *parent, base::EuclideanProjection projection,
-                    double priority, Cell* cell)
-                    : priority_(priority), cell_(cell), parent_(parent), state_(state),
-                    projection_(projection), heapElement_(NULL)
+                /// constructor for start states
+                Motion(base::State *state)
+                    : startState_(state), endState_(state), priority_(0.),
+                    parent_(NULL), cell_(NULL), heapElement_(NULL), isSplit_(false)
                 {
                 }
-                Motion(base::State *state, const ompl::base::ProjectionEvaluatorPtr& pe)
-                    : priority_(0.), cell_(NULL), parent_(NULL), state_(state),
-                    projection_(pe->getDimension()), heapElement_(NULL)
-                {
-                    pe->project(state_, projection_);
-                }
-
+                /// The score is used to order motions in a priority queue.
                 double score(void) const
                 {
                     return priority_ / cell_->volume_;
                 {
                     priority_ = priority_ * 2.0 + 1.0;
                 }
-
-                /// \brief Split a motion into two parts. The first part is
-                /// duration steps long (and is returned), the second part is the
-                /// remaing part of the motion and is kept in this object. It
-                /// is assumed the motion is contained within one cell.
-                Motion* split(const base::SpaceInformationPtr& si, double length,
-                    const ompl::base::ProjectionEvaluatorPtr& pe)
+                Motion* ancestor() const
                 {
-                    Motion* motion = new Motion(si->allocState(), parent_,
-                        pe->getDimension(), priority_, cell_);
-                    si->getStateSpace()->interpolate(parent_->state_, state_, length, motion->state_);
-                    pe->project(motion->state_, motion->projection_);
-                    cell_->motions_.push_back(motion);
-                    parent_ = motion;
-                    return motion;
+                    Motion* m = const_cast<Motion*>(this);
+                    while (m->parent_ && m->parent_->endState_ == m->startState_)
+                        m = m->parent_;
+                    return m;
                 }
 
+                /// The starting point of this motion
+                ompl::base::State*               startState_;
+                /// The state reached by this motion
+                ompl::base::State*               endState_;
                 /// Priority for selecting this path to extend from in the future
                 double                           priority_;
-
+                /// Parent motion from which this one started
+                Motion*                          parent_;
                 /// pointer to the cell that contains this path
                 Cell*                            cell_;
-
-                /// Parent motion from which this one started
-                Motion*                          parent_;
-
-                /// The state achieved by this motion
-                ompl::base::State*               state_;
-
-                /// The projection for this Motion
-                ompl::base::EuclideanProjection  projection_;
-
                 /// Handle to the element of the priority queue for this Motion
                 ompl::BinaryHeap<Motion *, MotionCompare>::Element *heapElement_;
+                /// Whether this motion is the result of a split operation, in which case
+                /// its endState_ should not be freed.
+                bool                             isSplit_;
             };
 
             /// Cell is a Binary Space Partition
                 ~Cell()
                 {
                     if (left_)
+                    {
                         delete left_;
-                    if (right_)
                         delete right_;
+                    }
                 }
 
                 /// Subdivides this cell
 
 
             /// Inserts the motion into the appropriate cell
-            void insertSampleIntoBsp(Motion *motion, Cell *bsp,base::State* state,
-                base::EuclideanProjection& projection);
-
+            void addMotion(Motion *motion, Cell *cell, base::State*, base::EuclideanProjection&);
+            /// \brief Either update heap after motion's priority has changed or insert motion into heap.
+            void updateHeapElement(Motion* motion)
+            {
+                if (motion->heapElement_)
+                    priorityQueue_.update(motion->heapElement_);
+                else
+                    motion->heapElement_ = priorityQueue_.insert(motion);
+            }
             /// \brief Select a state along motion and propagate a new motion from there.
             /// Return NULL if no valid motion could be generated starting at the
             /// selected state.
-            Motion* propagateFrom(Motion* motion, base::State* state, base::EuclideanProjection& projection);
-
-            /// \brief Split motion into segments such that each segment is contained in a cell.
-            Cell* split(const Cell* bsp, Cell* startCell, Motion* motion, int& numSegments,
-                base::State* state, base::EuclideanProjection& projection);
+            Motion* propagateFrom(Motion* motion, base::State*, base::State*);
 
             void freeMotion(Motion* m)
             {
-                if (m->state_)
-                    si_->freeState(m->state_);
+                if (m->parent_ && m->startState_ != m->parent_->endState_)
+                    si_->freeState(m->startState_);
+                if (!m->isSplit_)
+                    si_->freeState(m->endState_);
                 delete m;
             }
 
 
             /// State sampler
             ompl::base::StateSamplerPtr              sampler_;
-
             // Random number generator
-            RNG rng_;
-
+            RNG                                      rng_;
             /// \brief Vector holding all of the start states supplied for the problem
             /// Each start motion is the root of its own tree of motions.
             std::vector<Motion*>                     startMotions_;
-
             /// Priority queue of motions
             ompl::BinaryHeap<Motion*, MotionCompare> priorityQueue_;
-
             /// Binary Space Partition
-            Cell *bsp_;
-
+            Cell*                                    bsp_;
             /// Projection evaluator for the problem
             ompl::base::ProjectionEvaluatorPtr       projectionEvaluator_;
-
             /// Number between 0 and 1 specifying the probability with which the goal should be sampled
-            double goalBias_;
-
+            double                                   goalBias_;
             /// Objected used to sample the goal
             ompl::base::GoalSampleableRegion*        goalSampler_;
-
             /// Iteration number and priority of the next Motion that will be generated
             unsigned int                             iteration_;
-
             /// Closest motion to the goal
             Motion*                                  lastGoalMotion_;
-
         };
     }
 }

File src/ompl/geometric/planners/pdst/src/PDST.cpp

 #include "ompl/geometric/planners/pdst/PDST.h"
 
 ompl::geometric::PDST::PDST(const base::SpaceInformationPtr &si)
-    : base::Planner(si, "PDST"), bsp_(NULL),
-    goalBias_(0.05), goalSampler_(NULL), iteration_(1), lastGoalMotion_(NULL)
+    : base::Planner(si, "PDST"), bsp_(NULL), goalBias_(0.05),
+    goalSampler_(NULL), iteration_(1), lastGoalMotion_(NULL)
 {
     Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1.");
 }
     // time then initializes hasSolution to false and isApproximate to true.
     double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
     bool hasSolution = lastGoalMotion_ != NULL;
-    bool isApproximate = hasSolution ? goalSampler_->isSatisfied(lastGoalMotion_->state_, &closestDistanceToGoal) : true;
+    bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
     unsigned ndim = projectionEvaluator_->getDimension();
 
     // If an exact solution has already been found, do not run for another iteration.
     // Initialize tree with start state(s)
     while (const base::State *st = pis_.nextStart())
     {
-        Motion *startMotion = new Motion(si_->cloneState(st), projectionEvaluator_);
+        Motion *startMotion = new Motion(si_->cloneState(st));
         bsp_->addMotion(startMotion);
         startMotion->heapElement_ = priorityQueue_.insert(startMotion);
     }
         return base::PlannerStatus::INVALID_START;
     }
 
-    base::State* state = si_->allocState();
-    base::EuclideanProjection projection(projectionEvaluator_->getDimension());
+    base::State* tmpState1 = si_->allocState(), *tmpState2 = si_->allocState();
+    base::EuclideanProjection tmpProj(ndim);
     while (!ptc)
     {
         // Get the top priority path.
         motionSelected->updatePriority();
         priorityQueue_.update(motionSelected->heapElement_);
 
-        Motion *newMotion = propagateFrom(motionSelected, state, projection);
+        Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
+        addMotion(newMotion, bsp_, tmpState1, tmpProj);
 
         // Check if the newMotion reached the goal.
-        if (goal->isSatisfied(newMotion->state_, &distanceToGoal))
+        hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal);
+        if (hasSolution)
         {
             closestDistanceToGoal = distanceToGoal;
             lastGoalMotion_ = newMotion;
         cellSelected->subdivide(ndim);
         motions.swap(cellSelected->motions_);
         for (std::vector<Motion*>::iterator m = motions.begin() ; m != motions.end() ; ++m)
-            insertSampleIntoBsp(*m, cellSelected, state, projection);
-        iteration_++;
+            addMotion(*m, cellSelected, tmpState1, tmpProj);
     }
-    si_->freeState(state);
-
 
     if (lastGoalMotion_ != NULL)
         hasSolution = true;
         PathGeometric *path = new PathGeometric(si_);
 
         // Compute the path by going up the tree of motions.
-        std::vector<Motion*> mpath;
-        Motion *solution = lastGoalMotion_;
-        while (solution->parent_ != NULL)
+        std::vector<base::State*> spath(1,  lastGoalMotion_->endState_);
+        Motion *m = lastGoalMotion_;
+        while (m)
         {
-            mpath.push_back(solution);
-            solution = solution->parent_;
+            m = m->ancestor();
+            spath.push_back(m->startState_);
+            m = m->parent_;
         }
 
         // Add the solution path in order from the start state to the goal.
-        for (std::vector<Motion*>::reverse_iterator rIt = mpath.rbegin(); rIt != mpath.rend(); ++rIt)
-            path->append((*rIt)->state_);
+        for (std::vector<base::State*>::reverse_iterator rIt = spath.rbegin(); rIt != spath.rend(); ++rIt)
+            path->append((*rIt));
         pdef_->addSolutionPath(base::PathPtr(path), isApproximate, closestDistanceToGoal);
     }
 
+    si_->freeState(tmpState1);
+    si_->freeState(tmpState2);
+
     return base::PlannerStatus(hasSolution, isApproximate);
 }
 
-ompl::geometric::PDST::Motion* ompl::geometric::PDST::propagateFrom(Motion* motion, base::State* state, base::EuclideanProjection& projection)
+ompl::geometric::PDST::Motion* ompl::geometric::PDST::propagateFrom(
+    Motion* motion, base::State* start, base::State* rnd)
 {
-    if (motion->parent_)
-    {
-        // sample a point along the trajectory given by motion
-        double length = rng_.uniform01();
-        // split motion into two motions
-        motion = motion->split(si_, length, projectionEvaluator_);
-        motion->heapElement_ = priorityQueue_.insert(motion);
-    }
-
+    // sample a point along the trajectory given by motion
+    si_->getStateSpace()->interpolate(motion->startState_, motion->endState_, rng_.uniform01(), start);
     // generate a random state
     if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
-        goalSampler_->sampleGoal(state);
+        goalSampler_->sampleGoal(rnd);
     else
-        sampler_->sampleUniform(state);
-
-    std::pair<base::State*, double> lastValid = std::make_pair(state, 0.);
-    si_->checkMotion(motion->state_, state, lastValid);
-    projectionEvaluator_->project(state, projection);
-
-    Motion* m = new Motion(si_->cloneState(state), motion, projection, iteration_, bsp_->stab(projection));
-    int numSegments = si_->getStateSpace()->validSegmentCount(motion->state_, m->state_);
-
-    m->heapElement_ = priorityQueue_.insert(m);
-    m->cell_->motions_.push_back(m);
-
-    if (numSegments > 1)
-    {
-        si_->getStateSpace()->interpolate(motion->state_, m->state_, 1.0 / (double)numSegments, state);
-        projectionEvaluator_->project(state, projection);
-        Cell* cell = bsp_->stab(projection);
-        while (cell != motion->cell_ && numSegments > 1)
-            cell = split(bsp_, cell, m, numSegments, state, projection);
-    }
-    return m;
+        sampler_->sampleUniform(rnd);
+    // compute longest valid segment from start towards rnd
+    std::pair<base::State*, double> lastValid = std::make_pair(rnd, 0.);
+    si_->checkMotion(start, rnd, lastValid);
+    return new Motion(si_->cloneState(start), si_->cloneState(rnd), ++iteration_, motion);
 }
 
-void ompl::geometric::PDST::insertSampleIntoBsp(Motion *motion, Cell *bsp, base::State* state, base::EuclideanProjection& projection)
+void ompl::geometric::PDST::addMotion(Motion *motion, Cell *bsp, base::State* state, base::EuclideanProjection& proj)
 {
-    bsp->stab(motion->projection_)->addMotion(motion);
-    priorityQueue_.update(motion->heapElement_);
+    projectionEvaluator_->project(motion->endState_, proj);
+    bsp->stab(proj)->addMotion(motion);
+    updateHeapElement(motion);
 
     // If the motion corresponds to a start state, then it cannot be split across cell
     // bounds. So we're done.
     if (!motion->parent_)
         return;
 
-    // if cell of this motion and its parent are the same, we are done
-    Cell* cell = motion->parent_->cell_;
-    if (cell == bsp)
-        cell = bsp->stab(motion->parent_->projection_);
-    if (cell == motion->cell_)
-        return;
+    // Otherwise, split into smaller segments s.t. endpoints project into same cell
+    int numSegments = si_->getStateSpace()->validSegmentCount(motion->startState_, motion->endState_);
+    Cell *startCell;
+    projectionEvaluator_->project(motion->startState_, proj);
+    startCell = bsp->stab(proj);
 
-    // motion can cross at most one cell boundary, since bsp has just been split
-    // into two cells.
-    int numSegments = si_->getStateSpace()->validSegmentCount(motion->parent_->state_, motion->state_);
+    while (startCell != motion->cell_ && numSegments > 1)
+    {
+        Cell *nextStartCell = motion->cell_, *cell;
+        int i = 0, j = numSegments, k;
+        // Find the largest k such that the interpolated state at k/numSegments is
+        // still in startCell. The variables i and j bracket the range that k
+        // can be in.
+        while (j - i > 1)
+        {
+            k = (i + j) / 2;
+            si_->getStateSpace()->interpolate(motion->startState_, motion->endState_,
+                (double)k / (double)numSegments, state);
+            projectionEvaluator_->project(state, proj);
+            cell = bsp->stab(proj);
+            if (cell == startCell)
+                i = k;
+            else
+            {
+                j = k;
+                nextStartCell = cell;
+            }
+        }
 
-    if (numSegments > 1)
-    {
-        base::EuclideanProjection projection(projectionEvaluator_->getDimension());
-        si_->getStateSpace()->interpolate(motion->parent_->state_, motion->state_, 1.0 / (double)numSegments, state);
-        projectionEvaluator_->project(state, projection);
-        cell = bsp->stab(projection);
-        if (cell != motion->cell_)
-            split(bsp, cell, motion, numSegments, state, projection);
+        Motion* m = new Motion(motion->startState_, si_->cloneState(state), motion->priority_, motion->parent_);
+        startCell->addMotion(m);
+        m->heapElement_ = priorityQueue_.insert(m);
+        m->isSplit_ = true;
+        motion->startState_ = m->endState_;
+        motion->parent_ = m;
+        numSegments -= k;
+        startCell = nextStartCell;
     }
 }
 
-ompl::geometric::PDST::Cell* ompl::geometric::PDST::split(const Cell* bsp, Cell* startCell, Motion* motion, int& numSegments, base::State* state, base::EuclideanProjection& projection)
-{
-    Cell* nextStartCell = motion->cell_;
-    Cell* cell;
-    int i = 0, j = numSegments, k = 1;
-    // Find the largest k such that the interpolated state at k/numSegments is
-    // still in the startCell. The variables i and j bracket the range that k
-    // can be in. Initially, k=1. The loop will always be entered, because it
-    // is assumed that numSegments>1 (this is checked for in the calling
-    // contexts) and, hence, intially j-i>1.
-    while (j - i > 1)
-    {
-        k = (i + j) / 2;
-        si_->getStateSpace()->interpolate(motion->parent_->state_, motion->state_, (double)k / (double)numSegments, state);
-        projectionEvaluator_->project(state, projection);
-        cell = bsp->stab(projection);
-        if (cell == startCell)
-            i = k;
-        else
-        {
-            j = k;
-            nextStartCell = cell;
-        }
-    }
-
-    Motion* m = new Motion(si_->cloneState(state), motion->parent_, projection, iteration_, startCell);
-    motion->parent_ = m;
-    m->heapElement_ = priorityQueue_.insert(m);
-    m->cell_->motions_.push_back(m);
-    numSegments -= k;
-    // return the cell of the interpolated state at (k+1)/numSegments.
-    return nextStartCell;
-}
-
 void ompl::geometric::PDST::clear(void)
 {
     Planner::clear();
     iteration_ = 1;
     lastGoalMotion_ = NULL;
     freeMemory();
-    bsp_ = new Cell(1.0, projectionEvaluator_->getBounds(), 0);
+    bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
 }
 
 void ompl::geometric::PDST::freeMemory(void)
     sc.configureProjectionEvaluator(projectionEvaluator_);
     if (bsp_)
         delete bsp_;
-    bsp_ = new Cell(1.0, projectionEvaluator_->getBounds(), 0);
+    bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
     lastGoalMotion_ = NULL;
 }
 
 
     // Add goal vertex
     if (lastGoalMotion_ != NULL)
-        data.addGoalVertex(lastGoalMotion_->state_);
+        data.addGoalVertex(lastGoalMotion_->endState_);
 
     for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it)
-    {
-        Motion *child = *it;
-        Motion *parent = child->parent_;
-
-        if (parent == NULL)
-            data.addStartVertex(base::PlannerDataVertex(child->state_));
-        else
-            data.addEdge(base::PlannerDataVertex(parent->state_),
-                base::PlannerDataVertex(child->state_));
-    }
+        if (!(*it)->isSplit_)
+        {
+            Motion *cur = *it, *ancestor = cur->ancestor();
+            if (!cur->parent_)
+                data.addStartVertex(base::PlannerDataVertex(cur->endState_));
+            else
+            {
+                data.addEdge(base::PlannerDataVertex(ancestor->startState_),
+                    base::PlannerDataVertex(cur->endState_));
+                // add edge connecting start state of ancestor's parent to start state of ancestor
+                if (ancestor->parent_)
+                    data.addEdge(base::PlannerDataVertex(ancestor->parent_->ancestor()->startState_),
+                        base::PlannerDataVertex(ancestor->startState_));
+            }
+        }
 }
 
 void ompl::geometric::PDST::Cell::subdivide(unsigned int spaceDimension)
 {
-    double childVolume = 0.5 * volume_;
+    double childVolume = .5 * volume_;
     unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
-    splitValue_ = 0.5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);
+    splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);
 
     left_ = new Cell(childVolume, bounds_, nextSplitDimension);
     left_->bounds_.high[splitDimension_] = splitValue_;

File tests/geometric/2dmap/2dmap.cpp

 #include "ompl/geometric/planners/rrt/LazyRRT.h"
 #include "ompl/geometric/planners/est/EST.h"
 #include "ompl/geometric/planners/prm/PRM.h"
-#include "ompl/geometric/planners/prm/LazyPRM.h"
 
 #include "../../BoostTestTeamCityReporter.h"
 #include "../../base/PlannerTest.h"
 using namespace ompl;
 
 static const double SOLUTION_TIME = 1.0;
+static const bool VERBOSE = true;
 
 /** \brief A base class for testing planners */
 class TestPlanner
     {
     }
 
+    /* test a planner in a planar environment with circular obstacles */
     double test2DCircles(const Circles2D &circles, bool show = false, double *time = NULL, double *pathLength = NULL)
     {
         /* instantiate space information */
 	    ompl::time::point startTime = ompl::time::now();
 
 	    if (planner->solve(SOLUTION_TIME))
-	    {
+	    {                
 		ompl::time::duration elapsed = ompl::time::now() - startTime;
 		good++;
 		if (time)
 		    *time += ompl::time::seconds(elapsed);
 		if (show)
 		    printf("Found solution in %f seconds!\n", ompl::time::seconds(elapsed));
-
+                
 		geometric::PathGeometric *path = static_cast<geometric::PathGeometric*>(pdef->getSolutionPath().get());
 
 		/* make the solution more smooth */
 		startTime = ompl::time::now();
 		sm->simplify(*path, SOLUTION_TIME);
 		elapsed = ompl::time::now() - startTime;
-		
 		if (pathLength)
 		    *pathLength += path->length();
-		
 		if (time)
 		    *time += ompl::time::seconds(elapsed);
-	    }
-	}
-	
+            }
+        }
+
 	if (pathLength)
 	    *pathLength /= (double)circles.getQueryCount();
 	if (time)
 
 	return (double)good / (double)circles.getQueryCount();
     }
+  
     
+    /* test a planner in a planar grid environment where some cells are occupied */
     bool test2DEnv(const Environment2D &env, bool show = false, double *time = NULL, double *pathLength = NULL)
     {
         bool result = true;
         return result;
     }
 
-protected:
-
     virtual base::PlannerPtr newPlanner(const base::SpaceInformationPtr &si) = 0;
 
 };
 
 };
 
-class LazyPRMTest : public TestPlanner
-{
-protected:
-
-    base::PlannerPtr newPlanner(const base::SpaceInformationPtr &si)
-    {
-        geometric::LazyPRM *prm = new geometric::LazyPRM(si);
-        return base::PlannerPtr(prm);
-    }
-
-};
-
 class PlanTest
 {
 public:
 
-    void simpleTest(void)
+    void simpleTest(TestPlanner *p)
     {
         geometric::SimpleSetup2DMap s(env_);
+        s.setPlanner(p->newPlanner(s.getSpaceInformation()));
         s.setup();
         base::PlannerTest pt(s.getPlanner());
         pt.test();
         }
     }
 
-    void runAllTests(void)
+    void runAllTests(TestPlanner *p, double min_success, double max_avgtime)
     {
 	double success    = 0.0;
 	double avgruntime = 0.0;
 	double avglength  = 0.0;
-	
-	simpleTest();
-	
-	TestPlanner *p = new RRTTest();
+
+        if (verbose_)
+            printf("\n========= Running simple test\n\n");
+        simpleTest(p);
+
+        if (verbose_)
+            printf("\n========= Running 2D map test\n\n");        
 	run2DMapTest(p, &success, &avgruntime, &avglength);
-	BOOST_CHECK(success >= 99.0);
-	BOOST_CHECK(avgruntime < 0.01);
+	BOOST_CHECK(success >= min_success);
+	BOOST_CHECK(avgruntime < max_avgtime);
 	BOOST_CHECK(avglength < 100.0);
 
 	success    = 0.0;
 	avgruntime = 0.0;
 	avglength  = 0.0;
+
+        if (verbose_)
+            printf("\n========= Running 2D circles test\n\n");
+	run2DCirclesTest(p, &success, &avgruntime, &avglength);
+
+	BOOST_CHECK(success >= min_success);
+        // this problem is a little more difficult than the one above, so we allow more time for its solution
+	BOOST_CHECK(avgruntime < max_avgtime * 2.0);
+	BOOST_CHECK(avglength < 25.0);
+    }
     
-	run2DCirclesTest(p, &success, &avgruntime, &avglength);
-	BOOST_CHECK(success >= 99.0);
-	BOOST_CHECK(avgruntime < 0.01);
-	BOOST_CHECK(avglength < 10.0);
-	
+    template<typename T>
+    void runAllTests(double min_success, double max_avgtime)
+    {
+	TestPlanner *p = new T();
+	runAllTests(p, min_success, max_avgtime);
 	delete p;
     }
     
 
     PlanTest(void)
     {
-        verbose_ = false;
+        verbose_ = VERBOSE;
         boost::filesystem::path path(TEST_RESOURCES_DIR);
         loadEnvironment((path / "env1.txt").string().c_str(), env_);
 
     bool          verbose_;
 };
 
-BOOST_FIXTURE_TEST_SUITE( MyPlanTestFixture, PlanTest )
+BOOST_FIXTURE_TEST_SUITE(MyPlanTestFixture, PlanTest)
 
-BOOST_AUTO_TEST_CASE(geometric_RRT)
-{
-    runAllTests();
-}
+#define MACHINE_SPEED_FACTOR 3.0
 
+// define boost tests for a planner assuming the naming convention is followed 
+#define OMPL_PLANNER_TEST(Name, MinSuccess, MaxAvgTime)                 \
+    BOOST_AUTO_TEST_CASE(geometric_##Name)				\
+    {									\
+	if (VERBOSE)							\
+	    printf("\n\n\n*****************************\nTesting %s ...\n", #Name); \
+	runAllTests<Name##Test>(MinSuccess, MaxAvgTime * MACHINE_SPEED_FACTOR); \
+	if (VERBOSE)							\
+	    printf("Done with %s.\n", #Name);				\
+    }
 
-BOOST_AUTO_TEST_CASE(geometric_RRTConnect)
-{
-    runAllTests();
-}
+OMPL_PLANNER_TEST(RRT, 99.0, 0.01)
+OMPL_PLANNER_TEST(RRTConnect, 99.0, 0.01)
+OMPL_PLANNER_TEST(pRRT, 99.0, 0.02)  
 
-BOOST_AUTO_TEST_CASE(geometric_pRRT)
-{
-    runAllTests();
-}
+OMPL_PLANNER_TEST(TRRT, 99.0, 0.01)
 
-BOOST_AUTO_TEST_CASE(geometric_TRRT)
-{
-    runAllTests();
-}
+// LazyRRT is a not so great, so we use more relaxed bounds
+OMPL_PLANNER_TEST(LazyRRT, 90.0, 0.2)
 
-BOOST_AUTO_TEST_CASE(geometric_pSBL)
-{
-    runAllTests();
-}
+OMPL_PLANNER_TEST(pSBL, 99.0, 0.02)
+OMPL_PLANNER_TEST(SBL, 99.0, 0.02)
 
+OMPL_PLANNER_TEST(KPIECE1, 99.0, 0.01)
+OMPL_PLANNER_TEST(LBKPIECE1, 99.0, 0.02)
+OMPL_PLANNER_TEST(BKPIECE1, 99.0, 0.01)
 
-BOOST_AUTO_TEST_CASE(geometric_KPIECE1)
-{
-    runAllTests();
-}
-
-BOOST_AUTO_TEST_CASE(geometric_LBKPIECE1)
-{
-    runAllTests();
-}
-
-BOOST_AUTO_TEST_CASE(geometric_BKPIECE1)
-{
-    runAllTests();
-}
-
-BOOST_AUTO_TEST_CASE(geometric_EST)
-{
-    runAllTests();
-}
-
-BOOST_AUTO_TEST_CASE(geometric_LazyRRT)
-{
-    runAllTests();
-}
-
-BOOST_AUTO_TEST_CASE(geometric_PRM)
-{
-    runAllTests();
-}
-
-BOOST_AUTO_TEST_CASE(geometric_LazyPRM)
-{
-    runAllTests();
-}
-
-BOOST_AUTO_TEST_CASE(geometric_SBL)
-{
-    runAllTests();
-}
+OMPL_PLANNER_TEST(EST, 99.0, 0.02)
+OMPL_PLANNER_TEST(PRM, 99.0, 0.02)
 
 BOOST_AUTO_TEST_SUITE_END()

File tests/test_loop.sh

+#!/bin/bash
+
+control_c()
+# run if user hits control-c
+{
+  echo -en "\n*** User requested termination ***\n"
+  exit 1
+}
+
+# This script simply runs a test many times in gdb
+TEST_EXE=../build/bin/test_2dmap_geometric
+
+COUNT=100
+if [ $# -gt 0 ]
+then
+    COUNT=$1
+fi
+
+trap control_c SIGINT
+rm -f ompl_run_test_codes
+
+for i in $(seq $COUNT)
+do
+    echo "Running test $i of $COUNT" > ompl_run_test_count
+    gdb --return-child-result --batch --ex run --args $TEST_EXE
+    RET_CODE=$?
+    echo $RET_CODE >> ompl_run_test_codes
+    if [ $RET_CODE -ne 0 ]
+    then
+	echo "Error found!"
+    fi
+done
+rm -f ompl_run_test_count