9 /*! \brief RRT node basic class.
11 \param c Cumulative cost from RRT data structure root.
12 \param p Pointer to parent RRT node.
13 \param ch The vector of pointers to children RRT nodes.
15 class RRTNode : public BicycleCar {
18 RRTNode *p_ = nullptr;
21 double c() const { return this->c_; }
22 void c(double c) { this->c_ = c; }
24 RRTNode *p() const { return this->p_; }
25 void p(RRTNode *p) { this->p_ = p; }
30 /*! \brief Polygon obstacle basic class.
32 \param poly Border polygon of the obstacle.
36 std::vector<std::tuple<double, double>> poly_;
39 std::vector<std::tuple<double, double>> &poly()
47 /*! \brief RRT* algorithm basic class.
49 \param icnt RRT algorithm iterations counter.
50 \param goals The vector of goal nodes.
51 \param nodes The vector of all nodes in RRT data structure.
52 \param samples The vector of all samples of RRT algorithm.
56 unsigned int icnt_ = 0;
58 std::vector<RRTNode> goals_;
59 std::vector<RRTNode> nodes_;
60 std::vector<Obstacle> obstacles_;
61 std::vector<RRTNode> samples_;
62 std::vector<RRTNode> steered_;
65 bool collide(std::vector<std::tuple<double, double>> &poly);
66 bool collide_steered_from(RRTNode &f);
67 bool collide_two_nodes(RRTNode &f, RRTNode &t);
68 double cost(RRTNode &f, RRTNode &t);
70 std::default_random_engine gen_;
71 std::normal_distribution<double> ndx_;
72 std::normal_distribution<double> ndy_;
73 std::normal_distribution<double> ndh_;
74 RRTNode *nn(RRTNode &t);
75 std::vector<RRTNode *> nv(RRTNode &t);
76 void steer(RRTNode &f, RRTNode &t);
81 /*! \brief Return path found by RRT*.
83 std::vector<RRTNode *> path();
84 /*! \brief Run next RRT* iteration.
87 /*! \brief Set sampling info.
89 There is normal distribution sampling for `x`, `y`, and
90 `h` parameters of RRT node.
92 \param mx Mean x value.
93 \param dx Standard deviation of x.
94 \param my Mean y value.
95 \param dy Standard deviation of y.
96 \param mh Mean h value.
97 \param dh Standard deviation of h.
100 double mx, double dx,
101 double my, double dy,
106 std::vector<RRTNode> &goals() { return this->goals_; }
107 std::vector<RRTNode> &nodes() { return this->nodes_; }
108 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
109 std::vector<RRTNode> &samples() { return this->samples_; }
110 std::vector<RRTNode> &steered() { return this->steered_; }
115 /*! \brief Compute cumulative cost of RRT node.
117 \param t RRT node to compute cumulative cost to.
119 double cc(RRTNode &t);