#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; }
std::vector<Obstacle> obstacles_;
std::vector<RRTNode> samples_;
std::vector<RRTNode> steered_;
+ std::vector<RRTNode *> path_;
double log_path_time_ = 0.1;
- std::vector<double> log_path_cost_;
+ unsigned int log_path_iter_ = 20;
/*! \brief Update and return elapsed time.
*/
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
// 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; }