rrts/incl/rrts.hh

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 /*! \brief RRT* structure and default procedures.
   8  *
   9  * \file
  10  */
  11 #ifndef RRTS_RRTS_H
  12 #define RRTS_RRTS_H
  13
  14 #include <chrono>
  15 #include <functional>
  16 #include <json/json.h>
  17 #include <random>
  18 #include <vector>
  19 #include "bcar.hh"
  20
  21 namespace rrts {
  22 using namespace bcar;
  23
  24 /*! Compute elapsed time class. */
  25 class Ter {
  26 private:
  27         std::chrono::high_resolution_clock::time_point _tstart;
  28 public:
  29         void start();
  30         double scnt() const;
  31 };
  32
  33 /*! Store RRT node. */
  34 class RRTNode : public virtual Pose, public virtual CarMove {
  35 private:
  36         double _c = 0.0;
  37         double _cc = 0.0;
  38         RRTNode* _p = nullptr;
  39         unsigned int _cusp = 0;
  40         int _segment_type = 0;  // 0 ~ straight, 1 ~ left, -1 right
  41 public:
  42         /*! Get cost to parent. */
  43         double c() const;
  44
  45         /*! Set cost to parent. */
  46         void c(double c);
  47
  48         /*! Get cumulative cost from root. */
  49         double cc() const;
  50
  51         /*! Get parent node. */
  52         RRTNode* p() const;
  53
  54         /*! Set parent node. */
  55         void p(RRTNode& p);
  56
  57         /*! Get number of backward-forward direction changes. */
  58         unsigned int cusp() const;
  59
  60         /*! Set number of backward-forward direction changes. */
  61         void cusp(RRTNode const& p);
  62
  63         /*! \brief Get Reeds & Shepp segment type.
  64          *
  65          * Segment type is:
  66          * - 0 for straight,
  67          * - 1 for turning left,
  68          * - and -1 for turning right.
  69          */
  70         int st(void);
  71
  72         /*! Set Reeds & Shepp segment type. */
  73         void st(int st);
  74
  75         bool operator==(RRTNode const& n);
  76 };
  77
  78 class RRTGoal : public virtual RRTNode, public virtual PoseRange {
  79 public:
  80         using PoseRange::PoseRange;
  81 };
  82
  83 /*! RRT* algorithm basic class. */
  84 class RRTS {
  85 protected:
  86         BicycleCar _bc;
  87         RRTGoal _goal;
  88         unsigned int _icnt = 0;
  89         unsigned int _icnt_max = 1000;
  90         Ter _ter;
  91         std::default_random_engine _gen;
  92         std::vector<RRTNode> _nodes;
  93         std::vector<RRTNode> _steered;
  94         std::vector<RRTNode*> _path;
  95         RRTNode* _nn = nullptr;
  96         std::vector<RRTNode*> _nv;
  97         double _cost = 0.0;
  98         double _eta = 0.5;
  99         double _time = 0.0;
 100         std::vector<std::vector<RRTNode>> _logged_paths;
 101 protected:
 102         double min_gamma_eta(void) const;
 103         bool should_continue(void) const;
 104         void recompute_cc_for_predecessors_and(RRTNode* g);
 105         void recompute_path_cc();
 106         void join_steered(RRTNode* f);
 107         bool connect();
 108         void rewire();
 109         bool goal_drivable_from(RRTNode const& f);
 110 protected:
 111         virtual void store(RRTNode n);
 112         virtual double cost_build(RRTNode const& f, RRTNode const& t) const;
 113         virtual double cost_search(RRTNode const& f, RRTNode const& t) const;
 114         virtual void find_nn(RRTNode const& t);
 115         virtual void find_nv(RRTNode const& t);
 116         virtual void compute_path();
 117 protected:
 118         virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
 119         virtual bool collide_steered() = 0;
 120         virtual RRTNode sample() = 0;
 121         virtual bool should_finish() const = 0;
 122 public:
 123         RRTS();
 124
 125         /*! Set pose of the bicycle car used in the planner. */
 126         void set_bc_pose_to(Pose const& p);
 127
 128         /*! Get goal. */
 129         RRTGoal const& goal(void) const;
 130
 131         /*! Set goal. */
 132         void goal(double x, double y, double b, double e);
 133
 134         /*! Get number of iterations. */
 135         unsigned int icnt(void) const;
 136
 137         /*! Set number of iterations. */
 138         void icnt(unsigned int i);
 139
 140         /*! Get maximum number of iterations before reset. */
 141         unsigned int icnt_max(void) const;
 142
 143         /*! Set maximum number of iterations before reset. */
 144         void icnt_max(unsigned int i);
 145
 146         /*! Start elapsed time counter. */
 147         void tstart(void);
 148
 149         /*! Return elapsed time. */
 150         double scnt() const;
 151
 152         /*! Set start. */
 153         void start(double x, double y, double h);
 154
 155         /*! Get path. */
 156         std::vector<Pose> path() const;
 157
 158         /*! Get path cost. */
 159         double path_cost() const;
 160
 161         /*! Get cost of the last path. */
 162         double last_path_cost(void) const;
 163
 164         /*! Get eta, the RRT* constant used in near vertices and steering. */
 165         double eta() const;
 166
 167         /*! Set eta. */
 168         void eta(double e);
 169
 170         /*! Generate JSON output. */
 171         Json::Value json() const;
 172
 173         /*! Load JSON input. */
 174         void json(Json::Value jvi);
 175 public:
 176         /*! Run next RRT* iteration. Return True if should continue. */
 177         virtual bool next();
 178
 179         /*! Reset the algorithm. */
 180         virtual void reset();
 181 };
 182
 183 } // namespace rrts
 184 #endif /* RRTS_RRTS_H */