private:
double _c = 0.0;
double _cc = 0.0;
- RRTNode* _p;
+ RRTNode* _p = nullptr;
unsigned int _cusp = 0;
int _segment_type = 0; // 0 ~ straight, 1 ~ left, -1 right
public:
/*! RRT* algorithm basic class. */
class RRTS {
protected:
- BicycleCar bc_;
- RRTGoal goal_;
- unsigned int icnt_ = 0;
- unsigned int _imax = 1000;
- Ter ter_;
- std::default_random_engine gen_;
- std::vector<RRTNode> nodes_;
- std::vector<RRTNode> steered_;
- std::vector<RRTNode*> path_;
- RRTNode* nn_ = nullptr;
- std::vector<RRTNode*> nv_;
- double cost_ = 0.0;
- double eta_ = 0.5;
- double time_ = 0.0;
- double last_goal_cc_ = 0.0;
- std::vector<RRTNode> last_path_;
- void recompute_cc(RRTNode* g);
+ BicycleCar _bc;
+ RRTGoal _goal;
+ unsigned int _icnt = 0;
+ unsigned int _icnt_max = 1000;
+ Ter _ter;
+ std::default_random_engine _gen;
+ std::vector<RRTNode> _nodes;
+ std::vector<RRTNode> _steered;
+ std::vector<RRTNode*> _path;
+ RRTNode* _nn = nullptr;
+ std::vector<RRTNode*> _nv;
+ double _cost = 0.0;
+ double _eta = 0.5;
+ double _time = 0.0;
+ std::vector<std::vector<RRTNode>> _logged_paths;
+protected:
+ double min_gamma_eta(void) const;
+ bool should_continue(void) const;
+ void recompute_cc_for_predecessors_and(RRTNode* g);
void recompute_path_cc();
- double min_gamma_eta() const;
- bool should_continue() const;
void join_steered(RRTNode* f);
- RRTNode& nn();
bool connect();
void rewire();
bool goal_drivable_from(RRTNode const& f);
+protected:
virtual void store(RRTNode n);
virtual double cost_build(RRTNode const& f, RRTNode const& t) const;
virtual double cost_search(RRTNode const& f, RRTNode const& t) const;
virtual void find_nn(RRTNode const& t);
virtual void find_nv(RRTNode const& t);
virtual void compute_path();
+protected:
virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
virtual bool collide_steered() = 0;
virtual RRTNode sample() = 0;
public:
RRTS();
- /*! Set internal bicycle car. */
- BicycleCar &bc();
+ /*! Set pose of the bicycle car used in the planner. */
+ void set_bc_pose_to(Pose const& p);
- /*! Set maximum number of iterations before reset. */
- void set_imax_reset(unsigned int i);
+ /*! Get goal. */
+ RRTGoal const& goal(void) const;
/*! Set goal. */
- void set_goal(double x, double y, double b, double e);
+ void goal(double x, double y, double b, double e);
- /*! Set start. */
- void set_start(double x, double y, double h);
+ /*! Get number of iterations. */
+ unsigned int icnt(void) const;
- /*! Get path. */
- std::vector<Pose> get_path() const;
+ /*! Set number of iterations. */
+ void icnt(unsigned int i);
- /*! Get path cost. */
- double get_path_cost() const;
+ /*! Get maximum number of iterations before reset. */
+ unsigned int icnt_max(void) const;
- /*! Get iterations counter. */
- unsigned int icnt() const;
+ /*! Set maximum number of iterations before reset. */
+ void icnt_max(unsigned int i);
- /*! Set iterations counter. */
- void icnt(unsigned int i);
+ /*! Start elapsed time counter. */
+ void tstart(void);
/*! Return elapsed time. */
double scnt() const;
+ /*! Set start. */
+ void start(double x, double y, double h);
+
+ /*! Get path. */
+ std::vector<Pose> path() const;
+
+ /*! Get path cost. */
+ double path_cost() const;
+
+ /*! Get cost of the last path. */
+ double last_path_cost(void) const;
+
+ /*! Get eta, the RRT* constant used in near vertices and steering. */
double eta() const;
- void eta(double e);
+ /*! Set eta. */
+ void eta(double e);
+public:
/*! Generate JSON output. */
- Json::Value json() const;
+ virtual Json::Value json(void) const;
/*! Load JSON input. */
- void json(Json::Value jvi);
+ virtual void json(Json::Value jvi);
- /*! Run next RRT* iteration. */
+ /*! Run next RRT* iteration. Return True if should continue. */
virtual bool next();
/*! Reset the algorithm. */