#ifndef RRTBASE_H
#define RRTBASE_H
+#include <array>
#include <chrono>
#include <cmath>
+#include <pthread.h>
+#include <queue>
+#include <random>
#include <vector>
#include "obstacle.h"
#include "rrtnode.h"
+#include "slotplanner.h"
+
+#define NOFNODES 20000
+
+#define IXSIZE 100
+#define IYSIZE 100
+
+#define GLVERTEX(n) ((n)->x() * glplwscale), ((n)->y() * glplhscale)
+
+// Nearest neighbor
+struct mcnn { // min-cost nearest neighbour
+ float mc;
+ RRTNode *nn;
+};
+#pragma omp declare reduction \
+ (minn: struct mcnn: omp_out = \
+ omp_in.mc < omp_out.mc ? omp_in : omp_out) \
+ initializer \
+ (omp_priv(omp_orig))
+
+// Near vertices
+#define GAMMA_RRTSTAR(card_V) ({ \
+ __typeof__ (card_V) _card_V = (card_V); \
+ pow(log(_card_V) / _card_V, 1.0/3.0); \
+})
+
+# pragma omp declare reduction \
+ (merge: std::vector<RRTNode *>: \
+ omp_out.insert(omp_out.end(), omp_in.begin(), omp_in.end()))
+
+class Cell {
+ private:
+ std::vector<RRTNode *> nodes_;
+ pthread_mutex_t m_;
+ bool changed_ = false;
+ public:
+ Cell();
+
+ // getter
+ bool changed();
+ std::vector<RRTNode *> nodes();
+
+ // other
+ void add_node(RRTNode *n);
+};
class RRTBase {
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<SegmentObstacle> *sobstacles_;
std::chrono::high_resolution_clock::time_point tend_;
std::vector<float> clog_; // costs of trajectories
+ std::vector<float> nlog_; // #nodes of RRT
+ std::vector<std::vector<RRTEdge *>> rlog_; // log tree
+ 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 = 1;
- const float GOAL_FOUND_ANGLE = 2 * M_PI;
- const float TMAX = 100; // computation time limit
+ 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();
RRTBase(RRTNode *init, RRTNode *goal);
// 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<CircleObstacle> *cos();
- std::vector<SegmentObstacle> *sos();
+ std::array<std::vector<RRTNode *>, IYSIZE> iy_;
+ Cell ixy_[IXSIZE][IYSIZE];
+ std::vector<CircleObstacle> *co();
+ std::vector<SegmentObstacle> *so();
std::vector<float> &clog();
+ std::vector<float> &nlog();
+ std::vector<std::vector<RRTEdge *>> &rlog();
+ std::vector<float> &slog();
std::vector<std::vector<RRTNode *>> &tlog();
+ std::vector<RRTNode *> &slot_cusp();
bool goal_found();
float elapsed();
+ std::vector<RRTNode *> traj_cusp();
// 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);
void tstart();
void tend();
bool link_obstacles(
std::vector<CircleObstacle> *cobstacles,
std::vector<SegmentObstacle> *sobstacles);
+ bool add_iy(RRTNode *n);
+ bool add_ixy(RRTNode *n);
+ bool goal_found(bool f);
+ void slot_cusp(std::vector<RRTNode *> sc);
// other
+ bool glplot();
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,
+ std::vector<int> &npi);
+ bool optp_rrp(
+ std::vector<RRTNode *> &cusps,
+ std::vector<int> &npi);
+ bool optp_smart(
+ std::vector<RRTNode *> &cusps,
+ std::vector<int> &npi);
+ bool opt_path();
+ bool rebase(RRTNode *nr);
std::vector<RRTNode *> findt();
+ std::vector<RRTNode *> findt(RRTNode *n);
+ int XI(RRTNode *n);
+ int YI(RRTNode *n);
+
+ // RRT Framework
+ void defaultSamplingInfo();
+ void setSamplingInfo(SamplingInfo si);
+ RRTNode *sample();
+ float cost(RRTNode *init, RRTNode *goal);
+ RRTNode *nn(RRTNode *rs);
+ std::vector<RRTNode *> nv(RRTNode *node, float dist);
+ std::vector<RRTNode *> steer(RRTNode *init, RRTNode *goal);
+ std::vector<RRTNode *> steer(
+ RRTNode *init,
+ RRTNode *goal,
+ float step);
+
+ // virtuals - implemented by child classes
+ virtual bool next() = 0;
};
#endif