#include <chrono>
#include <cmath>
#include <pthread.h>
+#include <queue>
#include <random>
#include <vector>
#include "obstacle.h"
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;
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];
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,