namespace rrts {
+/*! \brief Collision check based on enlarged occupancy grid.
+ *
+ */
+class RRTExt21 : public virtual RRTS {
+private:
+ unsigned int _grid_width = 0;
+ unsigned int _grid_height = 0;
+ float _grid_res = 0.0;
+ int8_t const *_grid_data = nullptr;
+ double _origin_x = 0.0;
+ double _origin_y = 0.0;
+ bool collide(RRTNode const &n);
+ bool collide_steered();
+public:
+ void set_grid_to_check(unsigned int w, unsigned int h, float r,
+ int8_t const *d, double x, double y);
+};
+
+/*! \brief Collision check based on occupancy grid.
+ *
+ * This approach expects obstacles to be represented by points and the collision
+ * occures whenever the point is inside the frame given by the car pose and the
+ * car size.
+ *
+ * \ingroup ext-col
+ */
+class RRTExt20 : public virtual RRTS {
+private:
+ std::vector<bcar::Point> const *_points_to_check = nullptr;
+ bool collide_steered();
+public:
+ void set_points_to_check(std::vector<bcar::Point> const *p);
+};
+
+/*! \brief Use Dubins paths-based steering procedure.
+ *
+ * \ingroup ext-steer
+ * \see https://github.com/AndrewWalker/Dubins-Curves
+ */
+class RRTExt19 : public virtual RRTS {
+private:
+ void steer(RRTNode const &f, RRTNode const &t);
+};
+
/*! \brief Finish when more than 1000 iterations.
*
* \ingroup ext-aux
/*! \brief Reeds & Shepp (build) and Euclidean + abs angle (search).
*
* Use Reeds & Shepp path length for building tree data structure and Euclidean
- * distance plus (abs) heading difference for searching it.
+ * distance + (abs) heading difference + 0.1 * backward-forward direction
+ * changes for searching it.
*
* \ingroup ext-cost
* \see https://doi.org/10.1109/TITS.2015.2477355
RRTExt2();
Json::Value json() const;
void json(Json::Value jvi);
+ void reset();
};
/* \brief Different costs extension.