private:
double _c = 0.0;
double _cc = 0.0;
- RRTNode* _p;
- unsigned int _cusp = 0;
+ RRTNode* _p = nullptr;
+ unsigned int _cusp_cnt = 0;
int _segment_type = 0; // 0 ~ straight, 1 ~ left, -1 right
+ bool _p_is_cusp = false;
public:
/*! Get cost to parent. */
double c() const;
RRTNode* p() const;
/*! Set parent node. */
+ void p(RRTNode& p, bool can_be_too_close);
void p(RRTNode& p);
- /*! Get number of backward-forward direction changes. */
- unsigned int cusp() const;
+ /*! Get number of backward-forward direction changes.
+ *
+ * The parent node may be cusp to this node, i.e. parent may have sp ==
+ * 0 or sgn(sp) differs from sgn(this->sp). In such a situation, the
+ * number of this->cusp_cnt is incremented by one.
+ */
+ unsigned int cusp_cnt() const;
/*! Set number of backward-forward direction changes. */
- void cusp(RRTNode const& p);
+ void cusp_cnt(RRTNode const& p);
/*! \brief Get Reeds & Shepp segment type.
*
/*! Set Reeds & Shepp segment type. */
void st(int st);
+ /*! Return true if the parent is cusp node and false otherwise. */
+ bool p_is_cusp(void) const;
+
+ /*! Set if the parent node is cusp (direction changed from parent). */
+ void p_is_cusp(bool isit);
+
+ /*! Return true if p would be cusp if set as parent of this. */
+ bool would_be_cusp_if_parent(RRTNode const& p) const;
+
bool operator==(RRTNode const& n);
};
/*! 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:
+ /*! \brief Return nodes from f to t inclusive.
+ *
+ * The "inclusive" means that f is at the same pose (x, y, h) as
+ * this->_steered.front() and t is at the same pose (x, y, h) as
+ * this->_steered.back().
+ */
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);
+ /*! Set bicycle car dimensions. */
+ void set_bc_to_become(std::string what);
+
+ /*! 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 init pose. */
+ void set_init_pose_to(Pose const& p);
+
+ /*! 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. */
projects / hubacji1 / iamcar2.git / blobdiff