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 public:
  40         /*! Get cost to parent. */
  41         double c() const;
  42
  43         /*! Set cost to parent. */
  44         void c(double c);
  45
  46         /*! Get cumulative cost from root. */
  47         double cc() const;
  48
  49         /*! Get parent node. */
  50         RRTNode* p() const;
  51
  52         /*! Set parent node. */
  53         void p(RRTNode& p);
  54
  55         bool operator==(RRTNode const& n);
  56 };
  57
  58 class RRTGoal : public virtual RRTNode, public virtual PoseRange {
  59 public:
  60         using PoseRange::PoseRange;
  61 };
  62
  63 /*! RRT* algorithm basic class. */
  64 class RRTS {
  65 protected:
  66         BicycleCar bc_;
  67         RRTGoal goal_;
  68         unsigned int icnt_ = 0;
  69         Ter ter_;
  70         std::default_random_engine gen_;
  71         std::vector<RRTNode> nodes_;
  72         std::vector<RRTNode> steered_;
  73         std::vector<RRTNode*> path_;
  74         RRTNode* nn_ = nullptr;
  75         std::vector<RRTNode*> nv_;
  76         double cost_ = 0.0;
  77         double eta_ = 0.5;
  78         double time_ = 0.0;
  79         double last_goal_cc_ = 0.0;
  80         std::vector<RRTNode> last_path_;
  81         void recompute_cc(RRTNode* g);
  82         void recompute_path_cc();
  83         double min_gamma_eta() const;
  84         bool should_continue() const;
  85         void join_steered(RRTNode* f);
  86         RRTNode& nn();
  87         bool connect();
  88         void rewire();
  89         bool goal_drivable_from(RRTNode const& f);
  90         virtual void store(RRTNode n);
  91         virtual double cost_build(RRTNode const& f, RRTNode const& t) const;
  92         virtual double cost_search(RRTNode const& f, RRTNode const& t) const;
  93         virtual void find_nn(RRTNode const& t);
  94         virtual void find_nv(RRTNode const& t);
  95         virtual void compute_path();
  96         virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
  97         virtual bool collide_steered() = 0;
  98         virtual RRTNode sample() = 0;
  99         virtual bool should_finish() const = 0;
 100 public:
 101         RRTS();
 102
 103         /*! Get iterations counter. */
 104         unsigned int icnt() const;
 105
 106         /*! Set iterations counter. */
 107         void icnt(unsigned int i);
 108
 109         /*! Return elapsed time. */
 110         double scnt() const;
 111
 112         /*! Generate JSON output. */
 113         Json::Value json() const;
 114
 115         /*! Load JSON input. */
 116         void json(Json::Value jvi);
 117
 118         /*! Run next RRT* iteration. */
 119         virtual bool next();
 120
 121         /*! Reset the algorithm. */
 122         virtual void reset();
 123 };
 124
 125 } // namespace rrts
 126 #endif /* RRTS_RRTS_H */