#ifndef RRTS_H
#define 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;
+};
+
/*! \brief RRT node basic class.
\param c Cumulative cost from RRT data structure root.
\param p Pointer to parent RRT node.
-\param ch The vector of pointers to children RRT nodes.
+\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 : public BicycleCar {
+class RRTNode {
private:
- double c_ = 0;
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; }
+ 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; }
+
+ double y() const { return this->y_; }
+ void y(double y) { this->y_ = y; }
+
+ 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;
+ }
+
+ double sp() const { return this->sp_; }
+ void sp(double sp) { this->sp_ = sp; }
+
+ double st() const { return this->st_; }
+ void st(double st) { this->st_ = st; }
+
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;
+ }
};
/*! \brief Polygon obstacle basic class.
\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)
*/
class RRTS {
- private:
+ 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);
- std::tuple<bool, unsigned int, unsigned int>
+ virtual std::tuple<bool, unsigned int, unsigned int>
collide_steered_from(RRTNode &f);
- std::tuple<bool, unsigned int, unsigned int>
+ 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);
- double cost(RRTNode &f, RRTNode &t);
- double cost_build(RRTNode &f, RRTNode &t);
- double cost_search(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_;
- RRTNode *nn(RRTNode &t);
- std::vector<RRTNode *> nv(RRTNode &t);
+ 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);
- bool goal_found(RRTNode &f);
+ void join_tmp_steered(RRTNode *f);
+ virtual bool goal_found(RRTNode &f);
// RRT* procedures
- bool connect();
+ 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*.
*/
- std::vector<RRTNode *> path();
+ 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.
*/
- bool next();
+ virtual bool next();
/*! \brief Set sampling info.
- There is normal distribution sampling for `x`, `y`, and
- `h` parameters of RRT node.
+ Based on `sample_dist_type`, set proper distribution
+ parameters. The distribution parameters are relative to `front`
+ node in `nodes` (init).
- \param mx Mean x value.
- \param dx Standard deviation of x.
- \param my Mean y value.
- \param dy Standard deviation of y.
- \param mh Mean h value.
- \param dh Standard deviation of h.
+ 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 mx, double dx,
- double my, double dy,
- double mh, double dh
+ 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_; }
projects / hubacji1 / rrts.git / blobdiff