-#ifndef RRTS_H
-#define RRTS_H
+/*! \file */
+#ifndef RRTS_RRTS_H
+#define RRTS_RRTS_H
#include <chrono>
#include <functional>
#include <json/json.h>
#include <random>
#include <vector>
-#include "bcar.h"
-
-#define ETA 1.0 // for steer, nv
-#define GAMMA(cV) ({ \
- __typeof__ (cV) _cV = (cV); \
- pow(log(_cV) / _cV, 1.0 / 3.0); \
-})
-
-/*! \brief Possible type of RRT node.
-
-\param cusp The node that is cusp (change in direction).
-\param connected The node that branches generated steered path.
-*/
-class RRTNodeType {
- public:
- static const unsigned int cusp = 1 << 0;
- static const unsigned int connected = 1 << 1;
+#include "bcar.hh"
+
+namespace rrts {
+using namespace bcar;
+
+/*! Compute elapsed time class. */
+class Ter {
+private:
+ std::chrono::high_resolution_clock::time_point tstart_;
+public:
+ void start();
+ double scnt() const;
};
-/*! \brief RRT node basic class.
-
-\param c Cumulative cost from RRT data structure root.
-\param p Pointer to parent RRT node.
-\param t Type of the RRT node (RRTNodeType).
-// -- from BicycleCar
-\param x Horizontal coordinate of rear axle center.
-\param y Vertical coordinate of rear axle center.
-\param h Heading of the car in the interval [-pi,+pi] radians.
-\param sp Speed of the car.
-\param st Steering of the car.
-*/
-class RRTNode {
- private:
- RRTNode *p_ = nullptr;
- unsigned int t_ = 0;
- // -- from BicycleCar
- // coordinates
- double x_ = 0;
- double y_ = 0;
- double h_ = 0;
- // moving
- double sp_ = 0;
- double st_ = 0;
- public:
- double c_ = 0;
- double cc = 0.0;
- // getters, setters
- double c() const { return this->c_; }
- void c(double c)
- {
- this->c_ = c;
- this->cc = this->p_->cc + this->c_;
- }
-
- RRTNode *p() const { return this->p_; }
- void p(RRTNode *p) { this->p_ = p; }
-
- bool t(unsigned int flag) { return this->t_ & flag; }
- void set_t(unsigned int flag) { this->t_ |= flag; }
- void clear_t(unsigned int flag) { this->t_ &= ~flag; }
-
- // -- from BicycleCar
- // getters, setters
- double x() const { return this->x_; }
- void x(double x) { this->x_ = x; }
+/*! Store RRT node. */
+class RRTNode : public virtual Pose, public virtual CarMove {
+private:
+ double c_ = 0.0;
+ double cc_ = 0.0;
+ RRTNode* p_ = nullptr;
+public:
+ /*! Get cost to parent. */
+ double c() const;
- double y() const { return this->y_; }
- void y(double y) { this->y_ = y; }
+ /*! Set cost to parent. */
+ void c(double c);
- double h() const { return this->h_; }
- void h(double h)
- {
- while (h < -M_PI)
- h += 2 * M_PI;
- while (h > +M_PI)
- h -= 2 * M_PI;
- this->h_ = h;
- }
+ /*! Get cumulative cost from root. */
+ double cc() const;
- double sp() const { return this->sp_; }
- void sp(double sp) { this->sp_ = sp; }
+ /*! Get parent node. */
+ RRTNode* p() const;
- double st() const { return this->st_; }
- void st(double st) { this->st_ = st; }
+ /*! Set parent node. */
+ void p(RRTNode& p);
- RRTNode();
- RRTNode(const BicycleCar &bc);
- bool operator==(const RRTNode& n);
- friend std::ostream &operator<<(
- std::ostream &out,
- const RRTNode &bc
- )
- {
- out << "[" << bc.x();
- out << "," << bc.y();
- out << "," << bc.h();
- out << "]";
- return out;
- }
+ bool operator==(RRTNode const& n);
};
-/*! \brief Polygon obstacle basic class.
-
-\param poly Border polygon of the obstacle.
-*/
-class Obstacle {
- private:
- std::vector<std::tuple<double, double>> poly_;
- public:
- // getters, setters
- std::vector<std::tuple<double, double>> &poly()
- {
- return this->poly_;
- }
-
- Obstacle();
+class RRTGoal : public virtual RRTNode, public virtual PoseRange {
};
-/*! \brief RRT* algorithm basic class.
-
-\param icnt RRT algorithm iterations counter.
-\param goals The vector of goal nodes.
-\param nodes The vector of all nodes in RRT data structure.
-\param samples The vector of all samples of RRT algorithm.
-\param sample_dist_type Random distribution type for sampling function (0 -
-normal, 1 - uniform, 2 - uniform circle)
-*/
+/*! RRT* algorithm basic class. */
class RRTS {
- protected:
- unsigned int icnt_ = 0;
- std::chrono::high_resolution_clock::time_point tstart_;
- double scnt_ = 0;
- double pcnt_ = 0;
- bool gf_ = false;
- int sample_dist_type_ = 0;
-
- std::vector<RRTNode> goals_;
- std::vector<RRTNode> nodes_;
- std::vector<Obstacle> obstacles_;
- std::vector<RRTNode> samples_;
- std::vector<RRTNode> steered_;
- std::vector<RRTNode *> path_;
- double log_path_time_ = 0.1;
- unsigned int log_path_iter_ = 20;
-
- /*! \brief Update and return elapsed time.
- */
- double elapsed();
- /*! \brief Log current path cost.
- */
- void log_path_cost();
- /*! \brief Set normal distribution for sampling.
- */
- void set_sample_normal(
- double x1, double x2,
- double y1, double y2,
- double h1, double h2
- );
- /*! \brief Set uniform distribution for sampling.
- */
- void set_sample_uniform(
- double x1, double x2,
- double y1, double y2,
- double h1, double h2
- );
- /*! \brief Set uniform circle distribution for sampling.
- */
- void set_sample_uniform_circle();
- RRTNode* use_nn; // Used for RRTExt12.
- std::vector<RRTNode> tmp_steered_;
- bool finishit = false;
- double path_cost_before_opt_ = 9999;
-
- BicycleCar bc;
- /*! \brief Store RRT node to tree data structure.
- */
- virtual void store_node(RRTNode n);
-
- // RRT procedures
- std::tuple<bool, unsigned int, unsigned int>
- collide(std::vector<std::tuple<double, double>> &poly);
- virtual std::tuple<bool, unsigned int, unsigned int>
- collide_steered_from(RRTNode &f);
- virtual std::tuple<bool, unsigned int, unsigned int>
- collide_tmp_steered_from(RRTNode &f);
- virtual std::tuple<bool, unsigned int, unsigned int>
- collide_two_nodes(RRTNode &f, RRTNode &t);
- void sample();
- std::default_random_engine gen_;
- std::normal_distribution<double> ndx_;
- std::normal_distribution<double> ndy_;
- std::normal_distribution<double> ndh_;
- std::uniform_real_distribution<double> udx_;
- std::uniform_real_distribution<double> udy_;
- std::uniform_real_distribution<double> udh_;
- std::uniform_int_distribution<unsigned int> udi1_;
- std::uniform_int_distribution<unsigned int> udi2_;
- virtual RRTNode *nn(RRTNode &t);
- virtual std::vector<RRTNode *> nv(RRTNode &t);
- void steer(RRTNode &f, RRTNode &t);
- void tmp_steer(RRTNode &f, RRTNode &t);
- virtual void steer1(RRTNode &f, RRTNode &t);
- virtual void steer2(RRTNode &f, RRTNode &t);
- /*! \brief Join steered nodes to RRT data structure
-
- \param f RRT node to join steered nodes to.
- */
- void join_steered(RRTNode *f);
- void join_tmp_steered(RRTNode *f);
- virtual bool goal_found(RRTNode &f);
- // RRT* procedures
- virtual bool connect();
- void rewire();
- public:
- /// ---
- std::vector<double> log_opt_time_;
- std::vector<double> log_path_cost_;
- struct { double x=0; double y=0; double b=0; double e=0; } entry;
- bool entry_set = false;
- struct { double x=0; double y=0; double h=0; } entry1;
- struct { double x=0; double y=0; double h=0; } entry2;
- bool entries_set = false;
- std::vector<RRTNode *> steered1_;
- std::vector<RRTNode *> steered2_;
- /// ---
- /*! \brief Initialize RRT algorithm if needed.
- */
- virtual void init();
- virtual void reset();
- /*! \brief Deinitialize RRT algorithm if needed.
- */
- virtual void deinit();
- /*! \brief Return path found by RRT*.
- */
- virtual std::vector<RRTNode *>& path()
- {
- return this->path_;
- }
- virtual void compute_path();
- /*! \brief Return ``true`` if algorithm should stop.
-
- Update counters (iteration, seconds, ...) and return if
- the current iteration should be the last one.
- */
- bool should_stop();
- /*! \brief Return ``true`` if the algorithm should finish.
-
- Finish means that the algorithm will not be resumed.
- */
- bool should_finish();
- /*! \brief Return ``true`` if the algorithm shoud break.
-
- Break means that the algorithm can be resumed.
- */
- bool should_break();
- /*! \brief Return ``true`` if algorithm should continue.
-
- `pcnt_` is set to `scnt_`, so the difference is 0 and it can
- start from scratch. After the `should_continue` is called,
- there must be `while (rrts.next()) {}` loop.
- */
- bool should_continue();
- /*! \brief Run next RRT* iteration.
- */
- virtual bool next();
- /*! \brief Set sampling info.
-
- Based on `sample_dist_type`, set proper distribution
- parameters. The distribution parameters are relative to `front`
- node in `nodes` (init).
-
- For normal sampling:
- \param x1 Mean x value.
- \param x2 Standard deviation of x.
- \param y1 Mean y value.
- \param y2 Standard deviation of y.
- \param h1 Mean h value.
- \param h2 Standard deviation of h.
-
- For uniform sampling:
- \param x1 Minimum x value.
- \param x2 Maximum x value.
- \param y1 Minimum y value.
- \param y2 Maximum y value.
- \param h1 Minimum h value.
- \param h2 Maximum h value.
-
- For uniform circle sampling:
- \param x1 Ignored.
- \param x2 Ignored.
- \param y1 Ignored.
- \param y2 Ignored.
- \param h1 Ignored.
- \param h2 Ignored.
- */
- void set_sample(
- double x1, double x2,
- double y1, double y2,
- double h1, double h2
- );
- /*! \brief Generate JSON output.
- */
- Json::Value json();
- /*! \brief Load JSON input.
- */
- void json(Json::Value jvi);
-
- // RRT procedures
- virtual double cost_build(RRTNode &f, RRTNode &t);
- virtual double cost_search(RRTNode &f, RRTNode &t);
-
- // getters, setters
- unsigned int icnt() const { return this->icnt_; }
- void icnt(unsigned int i) { this->icnt_ = i; }
- double scnt() const { return this->scnt_; }
- bool gf() const { return this->gf_; }
- void gf(bool f) { this->gf_ = f; }
- int sample_dist_type() const { return this->sample_dist_type_;}
- void sample_dist_type(int t) { this->sample_dist_type_ = t; }
- std::vector<RRTNode> &goals() { return this->goals_; }
- std::vector<RRTNode> &nodes() { return this->nodes_; }
- std::vector<Obstacle> &obstacles() { return this->obstacles_; }
- std::vector<RRTNode> &samples() { return this->samples_; }
- std::vector<RRTNode> &steered() { return this->steered_; }
-
- RRTS();
+protected:
+ BicycleCar bc_;
+ RRTGoal goal_;
+ unsigned int icnt_ = 0;
+ 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 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);
+ 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();
+ virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
+ virtual bool collide_steered() = 0;
+ virtual RRTNode sample() = 0;
+ virtual bool should_finish() const = 0;
+public:
+ RRTS();
+
+ /*! Get iterations counter. */
+ unsigned int icnt() const;
+
+ /*! Set iterations counter. */
+ void icnt(unsigned int i);
+
+ /*! Return elapsed time. */
+ double scnt() const;
+
+ /*! Generate JSON output. */
+ Json::Value json() const;
+
+ /*! Load JSON input. */
+ void json(Json::Value jvi);
+
+ /*! Run next RRT* iteration. */
+ virtual bool next();
+
+ /*! Reset the algorithm. */
+ virtual void reset();
};
-/*! \brief Compute cumulative cost of RRT node.
-
-\param t RRT node to compute cumulative cost to.
-*/
-double cc(RRTNode &t);
-
-#endif /* RRTS_H */
+} // namespace rrts
+#endif /* RRTS_RRTS_H */
projects / hubacji1 / rrts.git / blobdiff