RRT.h

Go to the documentation of this file.

#ifndef __RRT_H__
#define __RRT_H__

#include <vector>
#include <string>
#include <iostream>
#include <stdio.h>

#include "PathPlanner.h"
#include "Algorithm.h"
#include "Roadmap.h"

namespace PathEngine {

  class RRT
    : public Algorithm {
  public:
    typedef boost::function1<bool, const Configuration &> extraConnectionCheckFunc;

  private:
    enum ExtendResult {
      Advanced, 
      Trapped,  
      Reached   
    };

    RoadmapPtr Tstart_;

    RoadmapPtr Tgoal_;

    RoadmapPtr TlastExtended_;

    RoadmapPtr Ta_, Tb_;

    int extend(RoadmapPtr tree, Configuration& qRand, bool reverse=false);

    int connect(RoadmapPtr tree, const Configuration& qNew, bool reverse=false);

    void swapTrees();

    double eps_;

    int times_;

    bool extendFromStart_;

    bool extendFromGoal_;

    extraConnectionCheckFunc extraConnectionCheckFunc_;

  public:
    RRT(PathPlanner* planner);

    ~RRT();

    bool calcPath();

    void extendFromStart(bool b) { extendFromStart_ = b; }

    void extendFromGoal(bool b) { extendFromGoal_ = b; }

    void extractPath();

    void extractPath(std::vector<Configuration>& o_path);

    RoadmapPtr getForwardTree() { return Tstart_; }

    void setForwardTree(RoadmapPtr tree);

    RoadmapPtr getBackwardTree() { return Tgoal_; }

    RoadmapPtr getLastExtendedTree() { return TlastExtended_; }

    void setBackwardTree(RoadmapPtr tree);

    bool extendOneStep();

    void epsilon(double e) { eps_ = e; }

    void setExtraConnectionCheckFunc(extraConnectionCheckFunc i_func);
  };
};


#endif // __RRT_H__