#ifndef RRTBASE_H
#define RRTBASE_H
+#include <array>
#include <chrono>
#include <cmath>
#include <pthread.h>
+#include <queue>
#include <random>
#include <vector>
#include "obstacle.h"
#define NOFNODES 20000
#define IXSIZE 100
-#define IXSTEP (1.0 * ((VMAX) - (VMIN)) / IXSIZE)
+#define IXSTEP (1.0 * ((HMAX) - (HMIN)) / IXSIZE)
#define IXI(x) ({ __typeof__ (x) _x = (x); \
std::abs((int) floor(_x / IXSTEP)); })
#define IYSIZE 100
-#define IYSTEP (1.0 * ((HMAX) - (HMIN)) / IYSIZE)
-#define IYI(y) ({ __typeof__ (y) _y = (y); \
- std::abs((int) floor(_y / IYSTEP)); })
+#define IYSTEP (1.0 * ((VMAX) - (VMIN)) / IYSIZE)
+#define IYI(y) ({ \
+ __typeof__ (y) _y = (y); \
+ (int) floor((_y - VMIN) / IYSTEP); \
+})
+
+#define GLVERTEX(n) ((n)->x() * glplwscale), ((n)->y() * glplhscale)
class Cell {
private:
private:
RRTNode *root_ = nullptr;
RRTNode *goal_ = nullptr;
+ std::vector<RRTNode *> goals_;
std::vector<RRTNode *> nodes_;
std::vector<RRTNode *> dnodes_;
+ std::queue<RRTNode *> firsts_;
PolygonObstacle frame_;
std::vector<RRTNode *> samples_;
std::vector<CircleObstacle> *cobstacles_;
std::vector<float> slog_; // seconds of trajectories
std::vector<std::vector<RRTNode *>> tlog_; // trajectories
std::vector<RRTNode *> slot_cusp_; // cusp nodes in slot
-
+ protected:
std::default_random_engine gen_;
+ std::normal_distribution<float> ndx_;
+ std::normal_distribution<float> ndy_;
+ std::normal_distribution<float> ndh_;
public:
const float GOAL_FOUND_DISTANCE = 0.2;
const float GOAL_FOUND_ANGLE = M_PI / 32;
+ float HMIN = -20;
+ float HMAX = 20;
+ float VMIN = -5;
+ float VMAX = 30;
~RRTBase();
RRTBase();
// getter
RRTNode *root();
RRTNode *goal();
+ std::vector<RRTNode *> &goals();
std::vector<RRTNode *> &nodes();
std::vector<RRTNode *> &dnodes();
+ std::queue<RRTNode *> &firsts();
PolygonObstacle &frame();
std::vector<RRTNode *> &samples();
- std::vector<RRTNode *> iy_[IYSIZE];
+ std::array<std::vector<RRTNode *>, IYSIZE> iy_;
Cell ixy_[IXSIZE][IYSIZE];
std::vector<CircleObstacle> *co();
std::vector<SegmentObstacle> *so();
// setter
void root(RRTNode *node);
void goal(RRTNode *node);
+ void goals(std::vector<RRTNode *> g);
bool logr(RRTNode *root);
float ocost(RRTNode *n);
bool tlog(std::vector<RRTNode *> t);
bool goal_found(
RRTNode *node,
float (*cost)(RRTNode *, RRTNode *));
+ bool goal_found(
+ RRTNode *node,
+ RRTNode *goal
+ );
bool collide(RRTNode *init, RRTNode *goal);
bool optp_dijkstra(
std::vector<RRTNode *> &cusps,
bool rebase(RRTNode *nr);
std::vector<RRTNode *> findt();
std::vector<RRTNode *> findt(RRTNode *n);
+ int XI(RRTNode *n);
+ int YI(RRTNode *n);
// RRT Framework
- SamplingInfo samplingInfo_;
- bool useSamplingInfo_ = false;
+ void setSamplingInfo(SamplingInfo si);
RRTNode *sample();
float cost(RRTNode *init, RRTNode *goal);
RRTNode *nn(RRTNode *rs);