api/rrts.h

   1 #ifndef RRTS_H
   2 #define RRTS_H
   3
   4 #include <functional>
   5 #include <vector>
   6 #include "bcar.h"
   7
   8 /*! \brief RRT node basic class.
   9
  10 \param c Cumulative cost from RRT data structure root.
  11 \param p Pointer to parent RRT node.
  12 \param ch The vector of pointers to children RRT nodes.
  13 */
  14 class RRTNode : public BicycleCar {
  15         private:
  16                 double c_ = 0;
  17                 RRTNode *p_ = nullptr;
  18         public:
  19                 // getters, setters
  20                 double c() const { return this->c_; }
  21                 void c(double c) { this->c_ = c; }
  22
  23                 RRTNode *p() const { return this->p_; }
  24                 void p(RRTNode *p) { this->p_ = p; }
  25
  26                 RRTNode();
  27 };
  28
  29 /*! \brief RRT* algorithm basic class.
  30
  31 \param icnt RRT algorithm iterations counter.
  32 \param goals The vector of goal nodes.
  33 \param nodes The vector of all nodes in RRT data structure.
  34 \param samples The vector of all samples of RRT algorithm.
  35 */
  36 class RRTS {
  37         private:
  38                 unsigned int icnt_ = 0;
  39
  40                 std::vector<RRTNode> goals_;
  41                 std::vector<RRTNode> nodes_;
  42                 std::vector<RRTNode> samples_;
  43
  44                 // RRT procedures
  45                 bool collide();
  46                 double cost(RRTNode &f, RRTNode &t);
  47                 void sample();
  48                         std::default_random_engine gen_;
  49                         std::normal_distribution<double> ndx_;
  50                         std::normal_distribution<double> ndy_;
  51                         std::normal_distribution<double> ndh_;
  52                 RRTNode &nn(RRTNode &t);
  53                 std::vector<std::reference_wrapper<RRTNode>> nv(RRTNode &t);
  54                 bool steer(RRTNode &f, RRTNode &t);
  55                 // RRT* procedures
  56                 bool connect();
  57                 void rewire();
  58         public:
  59                 /*! \brief Return path found by RRT*.
  60                 */
  61                 std::vector<std::reference_wrapper<RRTNode>> path();
  62                 /*! \brief Run next RRT* iteration.
  63                 */
  64                 bool next();
  65
  66                 // getters, setters
  67                 std::vector<RRTNode> &goals() { return this->goals_; }
  68                 std::vector<RRTNode> &nodes() { return this->nodes_; }
  69                 std::vector<RRTNode> &samples() { return this->samples_; }
  70
  71                 RRTS();
  72 };
  73
  74 #endif /* RRTS_H */