#include <chrono>
#include <functional>
-#include <jsoncpp/json/json.h>
+#include <json/json.h>
#include <random>
#include <vector>
#include "bcar.h"
*/
class RRTNode {
private:
- double c_ = 0;
RRTNode *p_ = nullptr;
unsigned int t_ = 0;
// -- from BicycleCar
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; }
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.
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;
std::vector<Obstacle> obstacles_;
std::vector<RRTNode> samples_;
std::vector<RRTNode> steered_;
- std::vector<double> log_path_cost_;
+ std::vector<RRTNode *> path_;
+ double log_path_time_ = 0.1;
+ unsigned int log_path_iter_ = 20;
/*! \brief Update and return elapsed time.
*/
/*! \brief Set uniform circle distribution for sampling.
*/
void set_sample_uniform_circle();
- protected:
+ 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 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::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
- 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*.
*/
- virtual 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
bool should_continue();
/*! \brief Run next RRT* iteration.
*/
- bool next();
+ virtual bool next();
/*! \brief Set sampling info.
Based on `sample_dist_type`, set proper distribution
// 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; }