// ext4
#define GRID 1 // in meters
-#define GRID_WIDTH 20 // in meters
-#define GRID_HEIGHT 50 // in meters
+#define GRID_WIDTH 40 // in meters
+#define GRID_HEIGHT 40 // in meters
#define GRID_MAX_XI ((unsigned int) floor(GRID_WIDTH / GRID)) // min is 0
#define GRID_MAX_YI ((unsigned int) floor(GRID_HEIGHT / GRID)) // min is 0
+// ext9
+#define GRID_MAX_HI 60
+
+/*! Use Dijkstra-based path optimization, goal zone for interesting nodes. */
+class RRTExt13 : public virtual RRTS {
+ private:
+ public:
+ void reset();
+ std::vector<RRTNode *> orig_path_;
+ double orig_path_cost_ = 9999;
+ std::vector<RRTNode *> first_optimized_path_;
+ double first_optimized_path_cost_ = 9999;
+ void first_path_optimization();
+ void second_path_optimization();
+ void compute_path();
+ Json::Value json();
+ void json(Json::Value jvi);
+
+ // getter, setter
+ std::vector<RRTNode *> &orig_path()
+ {
+ return this->orig_path_;
+ };
+ double &orig_path_cost() { return this->orig_path_cost_; }
+ void orig_path_cost(double c) { this->orig_path_cost_ = c; }
+ std::vector<RRTNode *> &first_optimized_path()
+ {
+ return this->first_optimized_path_;
+ };
+ double &first_optimized_path_cost() {
+ return this->first_optimized_path_cost_;
+ }
+ void first_optimized_path_cost(double c) {
+ this->first_optimized_path_cost_ = c;
+ }
+};
+
+/*! \brief Different `steer` procedures.
+
+Use sampling in control input for `steer1`. Use R&S steering for `steer2`.
+*/
+class RRTExt12 : public virtual RRTS {
+ protected:
+ void steer1(RRTNode &f, RRTNode &t);
+ bool connect();
+ public:
+ bool next();
+};
+
+/*! \brief Goal Zone.
+
+*/
+class RRTExt11 : public virtual RRTS {
+ protected:
+ bool goal_found(RRTNode &f);
+};
+
+/*! \brief Different costs extension.
+
+Use different cost for bulding tree data structure and searching in the
+structure. The cost function is from Elbanhawi, Mohamed, Milan Simic, and Reza
+Jazar. “Randomized Bidirectional B-Spline Parameterization Motion Planning.”
+IEEE Transactions on Intelligent Transportation Systems 17, no. 2 (February
+2016): 406–19. https://doi.org/10.1109/TITS.2015.2477355.
+
+*/
+class RRTExt10 : public virtual RRTS {
+ public:
+ /*! \brief Reeds and Shepp path length.
+ */
+ double cost_build(RRTNode &f, RRTNode &t);
+ /*! \brief Heuristics distance.
+ */
+ double cost_search(RRTNode &f, RRTNode &t);
+};
+
+/*! \brief Use grid data structure to store RRT nodes.
+
+This approach speeds up the search process for the nearest neighbor and
+the near vertices procedures.
+*/
+class RRTExt9 : public virtual RRTS {
+ private:
+ class Cell {
+ private:
+ bool changed_ = false;
+ std::vector<RRTNode *> nodes_;
+ public:
+ void nn(RRTNode *t, RRTNode **nn, RRTS *p);
+ void store_node(RRTNode *n);
+
+ // getter, setter
+ bool changed() const
+ {
+ return this->changed_;
+ }
+ std::vector<RRTNode *> &nodes()
+ {
+ return this->nodes_;
+ }
+
+ Cell();
+ };
+ Cell grid_[GRID_MAX_XI][GRID_MAX_YI][GRID_MAX_HI];
+ double x_min_ = 0;
+ double x_max_ = 0;
+ double y_min_ = 0;
+ double y_max_ = 0;
+ double h_min_ = 0;
+ double h_max_ = 2 * M_PI;
+
+ unsigned int xi(RRTNode n);
+ unsigned int yi(RRTNode n);
+ unsigned int hi(RRTNode n);
+ public:
+ void init();
+ void deinit();
+ void store_node(RRTNode n);
+ RRTNode *nn(RRTNode &t);
+ std::vector<RRTNode *> nv(RRTNode &t);
+};
+
/*! \brief Use k-d tree data structure to store RRT nodes.
This approach speeds up the search process for the nearest neighbor and
void delete_kd_nodes(KdNode *n);
void store_node(RRTNode *n, KdNode *&r, int l);
void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
+ void nv(
+ std::vector<RRTNode*>& n,
+ RRTNode &t,
+ KdNode *r,
+ int l,
+ double const& d
+ );
public:
+ void delete_kd_nodes()
+ {
+ this->delete_kd_nodes(this->kdroot_);
+ this->kdroot_ = nullptr;
+ }
void init();
+ void reset();
void deinit();
void store_node(RRTNode n);
RRTNode *nn(RRTNode &t);
Cell();
};
Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
- unsigned int x_min_ = 0;
- unsigned int x_max_ = 0;
- unsigned int y_min_ = 0;
- unsigned int y_max_ = 0;
+ double x_min_ = 0;
+ double x_max_ = 0;
+ double y_min_ = 0;
+ double y_max_ = 0;
unsigned int xi(RRTNode n);
unsigned int yi(RRTNode n);
*/
class RRTExt3 : public virtual RRTS {
private:
- std::vector<RRTNode *> orig_path_;
- double orig_path_cost_;
public:
- std::vector<RRTNode *> path();
+ void reset();
+ std::vector<RRTNode *> orig_path_;
+ double orig_path_cost_ = 9999;
+ std::vector<RRTNode *> first_optimized_path_;
+ double first_optimized_path_cost_ = 9999;
+ void first_path_optimization();
+ void second_path_optimization();
+ void compute_path();
+ Json::Value json();
+ void json(Json::Value jvi);
// getter, setter
std::vector<RRTNode *> &orig_path()
};
double &orig_path_cost() { return this->orig_path_cost_; }
void orig_path_cost(double c) { this->orig_path_cost_ = c; }
+ std::vector<RRTNode *> &first_optimized_path()
+ {
+ return this->first_optimized_path_;
+ };
+ double &first_optimized_path_cost() {
+ return this->first_optimized_path_cost_;
+ }
+ void first_optimized_path_cost(double c) {
+ this->first_optimized_path_cost_ = c;
+ }
};
/*! \brief Use cute_c2 for collision detection.
// Collide RRT procedures
std::tuple<bool, unsigned int, unsigned int>
collide_steered_from(RRTNode &f);
+ std::tuple<bool, unsigned int, unsigned int>
+ collide_tmp_steered_from(RRTNode &f);
std::tuple<bool, unsigned int, unsigned int>
collide_two_nodes(RRTNode &f, RRTNode &t);
projects / hubacji1 / rrts.git / blobdiff