- Uniform sampling. (Normal sampling is still default.)
- Compound extensions 18 - 33.
- Path optimization in backward direction.
+- Goal found check for zone instead of Euclidean distance and max. angle
+ extension.
### Changed
- Reformat lists in readme.
- Add `RRTExt3` to compound extensions 18 - 33.
-- Goal found check for zone instead of Euclidean distance and max. angle.
## 0.3.1 - 2020-01-03
### Fixed
8. "nn" 3D [K-d tree][] for nodes storage,
9. "nn" 3D grid for nodes storage,
10. "cost" RS-B -- Reeds & Shepp (build), B-Spline paper (search)
+11. "goal zone" gz -- Use drivable of libbcar to check if goal found.
[cute c2]: https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
[K-d tree]: https://en.wikipedia.org/wiki/K-d_tree
There is a list of classes with reference to extensions used. The extensio
number accomply to class `RRTCEn` where `n` is:
-1. cute, Dijkstra, RS-H.
-2. cute, Dijkstra, RS-E.
-3. cute, Dijkstra, RS-RS.
+1. cute, gz, Dijkstra, RS-H.
+2. cute, gz, Dijkstra, RS-E.
+3. cute, gz, Dijkstra, RS-RS.
4. RS-H, cute, 2D grid.
5. cute, 2D grid, RS-E.
6. cute, 2D grid, RS-RS.
14. cute, Dijkstra, 3D tree, RS-H.
15. cute, Dijkstra, 3D grid, RS-RS.
16. cute, Dijkstra, 3D grid, RS-H.
-17. cute, Dijkstra, RS-B.
-
-18. cute, Dijkstra, 2D grid, RS-RS
-19. cute, Dijkstra, 2D grid, RS-E
-20. cute, Dijkstra, 2D grid, RS-H
-21. cute, Dijkstra, 2D grid, RS-B
-
-22. cute, Dijkstra, 2D tree, RS-RS
-23. cute, Dijkstra, 2D tree, RS-E
-24. cute, Dijkstra, 2D tree, RS-H
-25. cute, Dijkstra, 2D tree, RS-B
-
-26. cute, Dijkstra, 3D grid, RS-RS
-27. cute, Dijkstra, 3D grid, RS-E
-28. cute, Dijkstra, 3D grid, RS-H
-29. cute, Dijkstra, 3D grid, RS-B
-
-30. cute, Dijkstra, 3D tree, RS-RS
-31. cute, Dijkstra, 3D tree, RS-E
-32. cute, Dijkstra, 3D tree, RS-H
-33. cute, Dijkstra, 3D tree, RS-B
+17. cute, gz, Dijkstra, RS-B.
+
+18. cute, gz, Dijkstra, 2D grid, RS-RS
+19. cute, gz, Dijkstra, 2D grid, RS-E
+20. cute, gz, Dijkstra, 2D grid, RS-H
+21. cute, gz, Dijkstra, 2D grid, RS-B
+
+22. cute, gz, Dijkstra, 2D tree, RS-RS
+23. cute, gz, Dijkstra, 2D tree, RS-E
+24. cute, gz, Dijkstra, 2D tree, RS-H
+25. cute, gz, Dijkstra, 2D tree, RS-B
+
+26. cute, gz, Dijkstra, 3D grid, RS-RS
+27. cute, gz, Dijkstra, 3D grid, RS-E
+28. cute, gz, Dijkstra, 3D grid, RS-H
+29. cute, gz, Dijkstra, 3D grid, RS-B
+
+30. cute, gz, Dijkstra, 3D tree, RS-RS
+31. cute, gz, Dijkstra, 3D tree, RS-E
+32. cute, gz, Dijkstra, 3D tree, RS-H
+33. cute, gz, Dijkstra, 3D tree, RS-B
# Contribute
Use [OneFlow][3] branching model and keep the [changelog][4].
class RRTCE33
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt8
, public RRTExt10
};
class RRTCE32
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt8
, public RRTExt1
};
class RRTCE31
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt8
, public RRTExt5
};
class RRTCE30
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt8
, public RRTExt6
class RRTCE29
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt9
, public RRTExt10
};
class RRTCE28
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt9
, public RRTExt1
};
class RRTCE27
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt9
, public RRTExt5
};
class RRTCE26
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt9
, public RRTExt6
class RRTCE25
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt7
, public RRTExt10
};
class RRTCE24
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt7
, public RRTExt1
};
class RRTCE23
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt7
, public RRTExt5
};
class RRTCE22
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt7
, public RRTExt6
class RRTCE21
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt4
, public RRTExt10
};
class RRTCE20
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt4
, public RRTExt1
};
class RRTCE19
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt4
, public RRTExt5
};
class RRTCE18
: public RRTExt2
+ , public RRTExt11
, public RRTExt3
, public RRTExt4
, public RRTExt6
}
};
-class RRTCE17 : public RRTExt2, public RRTExt3, public RRTExt10 {};
+class RRTCE17
+ : public RRTExt2
+ , public RRTExt11
+ , public RRTExt3
+ , public RRTExt10
+{};
+
class RRTCE16
: public RRTExt2
, public RRTExt3
RRTExt4::deinit();
}
};
-class RRTCE3 : public RRTExt2, public RRTExt3, public RRTExt6 {};
-class RRTCE2 : public RRTExt2, public RRTExt3, public RRTExt5 {};
-class RRTCE1 : public RRTExt2, public RRTExt3, public RRTExt1 {};
+class RRTCE3
+ : public RRTExt2
+ , public RRTExt11
+ , public RRTExt3
+ , public RRTExt6
+{};
+class RRTCE2
+ : public RRTExt2
+ , public RRTExt11
+ , public RRTExt3
+ , public RRTExt5
+{};
+class RRTCE1
+ : public RRTExt2
+ , public RRTExt11
+ , public RRTExt3
+ , public RRTExt1
+{};
#endif /* RRTCE_H */
// ext9
#define GRID_MAX_HI 60
+/*! \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
\param f RRT node to join steered nodes to.
*/
void join_steered(RRTNode *f);
- bool goal_found(RRTNode &f);
+ virtual bool goal_found(RRTNode &f);
// RRT* procedures
bool connect();
void rewire();
--- /dev/null
+#include "rrtext.h"
+
+bool RRTExt11::goal_found(RRTNode &f)
+{
+ auto &g = this->goals().front();
+ double cost = this->cost_build(f, g);
+ if (g.drivable(f)) {
+ this->steer(f, g);
+ if (std::get<0>(this->collide_steered_from(f)))
+ return false;
+ this->join_steered(&f);
+ if (g.p() == nullptr || cc(f) + cost < cc(g)) {
+ g.p(&f);
+ g.c(cost);
+ }
+ return true;
+ }
+ return false;
+}
bool RRTS::goal_found(RRTNode &f)
{
- bool found = false;
auto &g = this->goals().front();
- bool in_zone = false;
double cost = this->cost_build(f, g);
- double h_d = f.h() - g.h();
- if (h_d < -M_PI/2 || h_d > M_PI/2)
- return false;
- double max_dist = g.mtr() * 2 * sin(M_PI/2 / 2); // mtr circle chord
- if (sqrt(pow(f.x() - g.x(), 2) + pow(f.y() - g.y(), 2)) > max_dist)
- return false;
- double a = atan2(f.y() - g.y(), f.x() - g.x()) - g.h();
- while (a < 0)
- a += 2 * M_PI;
- if (0 <= a && a < M_PI/2) { // left front g
- BicycleCar zone_border(g);
- zone_border.rotate(g.ccl().x(), g.ccl().y(), h_d);
- a = atan2(f.y() - zone_border.y(), f.x() - zone_border.x());
- a -= g.h();
- while (a < 0)
- a += 2 * M_PI;
- double ub = zone_border.h() - g.h();
- while (ub < 0)
- ub += 2 * M_PI;
- if (0 <= a && a <= ub)
- in_zone = true;
- } else if (M_PI/2 <= a && a < M_PI) { // left rear g
- BicycleCar zone_border(g);
- zone_border.rotate(g.ccl().x(), g.ccl().y(), h_d);
- a = atan2(f.y() - zone_border.y(), f.x() - zone_border.x());
- if (sqrt(
- pow(f.y() - zone_border.y(), 2)
- + pow(f.x() - zone_border.x(), 2)
- ) > 0)
- a -= M_PI;
- a -= zone_border.h();
- while (a < 0)
- a += 2 * M_PI;
- double ub = g.h() - zone_border.h();
- double lb = zone_border.h();
- if (lb <= a && a <= ub)
- in_zone = true;
- } else if (M_PI <= a && a < 3 * M_PI/2) { // right rear g
- BicycleCar zone_border(g);
- zone_border.rotate(g.ccr().x(), g.ccr().y(), h_d);
- a = atan2(f.y() - zone_border.y(), f.x() - zone_border.x());
- if (sqrt(
- pow(f.y() - zone_border.y(), 2)
- + pow(f.x() - zone_border.x(), 2)
- ) > 0)
- a -= M_PI;
- a -= g.h();
- double ub = zone_border.h() - g.h();
- double lb = g.h();
- if (lb <= a && a <= ub)
- in_zone = true;
- } else if (3 * M_PI/2 <= a && a < 2 * M_PI) { // right front g
- BicycleCar zone_border(g);
- zone_border.rotate(g.ccr().x(), g.ccr().y(), h_d);
- a = atan2(f.y() - zone_border.y(), f.x() - zone_border.x());
- a -= zone_border.h();
- while (a < 0)
- a += 2 * M_PI;
- double ub = g.h() - zone_border.h();
- while (ub < 0)
- ub += 2 * M_PI;
- if (0 <= a && a <= ub)
- in_zone = true;
- } else {
- // Not happenning, as ``a`` < 2 * M_PI.
- }
- if (in_zone) {
- found = true;
+ double edist = sqrt(
+ pow(f.x() - g.x(), 2)
+ + pow(f.y() - g.y(), 2)
+ );
+ double adist = std::abs(f.h() - g.h());
+ if (edist < 0.05 && adist < M_PI / 32) {
if (g.p() == nullptr || cc(f) + cost < cc(g)) {
g.p(&f);
g.c(cost);
}
+ return true;
}
- return found;
+ return false;
}
// RRT* procedures
&& &rrts.goals().front() == rrts.path().back()
);
}
-
-WVTEST_MAIN("goal found zone")
-{
- class P : public RRTS {
- public:
- bool goal_found(RRTNode &f) {
- return RRTS::goal_found(f);
- }
- };
- double tmp_double_1 = 0;
- double tmp_double_2 = 0;
- P p;
- p.goals().push_back(BicycleCar());
- BicycleCar &g = p.goals().front();
- // TODO set g.x, g.y to different values
- // TODO set g.h to cover all 4 quadrants
- RRTNode n;
- n.x(g.x());
- n.y(g.y());
- n.h(g.h());
- WVPASS(p.goal_found(n)); // pass the same pose
-
- n = RRTNode(g);
- n.rotate(g.ccr().x(), g.ccr().y(), -M_PI/2);
- WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
- tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
- tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
- WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
- WVPASS(p.goal_found(n)); // pass right corner case
- n.rotate(g.ccr().x(), g.ccr().y(), -0.01);
- WVPASS(!p.goal_found(n)); // fail right corner case
-
- n = RRTNode(g);
- n.rotate(g.ccl().x(), g.ccl().y(), M_PI/2);
- WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
- tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
- tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
- WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
- WVPASS(p.goal_found(n)); // pass left corner case
- n.rotate(g.ccl().x(), g.ccl().y(), 0.01);
- WVPASS(!p.goal_found(n)); // fail left corner case
-
- n = RRTNode(g);
- n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
- n.st(0);
- n.next();
- WVPASS(p.goal_found(n)); // pass forward corner case
- n.sp(0.01);
- n.next();
- WVPASS(!p.goal_found(n)); // fail forward corner case
-
- for (double a = 0; a > -M_PI/2; a -= 0.01) {
- n = RRTNode(g);
- n.rotate(g.ccr().x(), g.ccr().y(), a);
- WVPASS(p.goal_found(n)); // pass drivable border
- }
- for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
- // + 0.1 -- compensate for Euclid. dist. check
- n = RRTNode(g);
- n.x(n.x() + 0.1*cos(n.h()));
- n.y(n.y() + 0.1*sin(n.h()));
- n.rotate(n.ccr().x(), n.ccr().y(), a);
- WVPASS(p.goal_found(n)); // pass near drivable border
- }
- for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
- // = -0.1 -- compensate for near goal
- n = RRTNode(g);
- n.x(n.x() - 0.1*cos(n.h()));
- n.y(n.y() - 0.1*sin(n.h()));
- n.rotate(n.ccr().x(), n.ccr().y(), a);
- WVPASS(!p.goal_found(n)); // fail near drivable border
- }
- for (double a = 0; a < M_PI / 2; a += 0.01) {
- n = RRTNode(g);
- n.rotate(g.ccl().x(), g.ccl().y(), a);
- WVPASS(p.goal_found(n)); // pass drivable border
- }
- for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
- // - 0.1 -- compensate for Euclid. dist. check
- n = RRTNode(g);
- n.x(n.x() + 0.1*cos(n.h()));
- n.y(n.y() + 0.1*sin(n.h()));
- n.rotate(n.ccl().x(), n.ccl().y(), a);
- WVPASS(p.goal_found(n)); // pass near drivable border
- }
- for (double a = 0.1; a < M_PI / 2; a += 0.01) {
- // = 0.1 -- compensate for near goal
- n = RRTNode(g);
- n.x(n.x() - 0.1*cos(n.h()));
- n.y(n.y() - 0.1*sin(n.h()));
- n.rotate(n.ccl().x(), n.ccl().y(), a);
- WVPASS(!p.goal_found(n)); // fail near drivable border
- }
-
- n = RRTNode(g);
- n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
- n.sp(n.sp() * -1);
- n.st(0);
- n.next();
- WVPASS(p.goal_found(n)); // pass backward corner case
- n.sp(-0.01);
- n.next();
- WVPASS(!p.goal_found(n)); // fail backward corner case
-
- n = RRTNode(g);
- n.rotate(g.ccr().x(), g.ccr().y(), M_PI/2);
- WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
- tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
- tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
- WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
- WVPASS(p.goal_found(n)); // pass right corner case
- n.rotate(g.ccr().x(), g.ccr().y(), 0.01);
- WVPASS(!p.goal_found(n)); // fail right corner case
-
- n = RRTNode(g);
- n.rotate(g.ccl().x(), g.ccl().y(), -M_PI/2);
- WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
- tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
- tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
- WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
- WVPASS(p.goal_found(n)); // pass left corner case
- n.rotate(g.ccl().x(), g.ccl().y(), -0.01);
- WVPASS(!p.goal_found(n)); // fail left corner case
-
- for (double a = 0; a < M_PI / 2; a += 0.01) {
- n = RRTNode(g);
- n.rotate(g.ccr().x(), g.ccr().y(), a);
- WVPASS(p.goal_found(n)); // pass drivable border
- }
- for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
- // - 0.1 -- compensate for Euclid. dist. check
- n = RRTNode(g);
- n.x(n.x() - 0.1*cos(n.h()));
- n.y(n.y() - 0.1*sin(n.h()));
- n.rotate(n.ccr().x(), n.ccr().y(), a);
- WVPASS(p.goal_found(n)); // pass near drivable border
- }
- for (double a = 0.1; a < M_PI / 2; a += 0.01) {
- // = 0.1 -- compensate for near goal
- n = RRTNode(g);
- n.x(n.x() + 0.1*cos(n.h()));
- n.y(n.y() + 0.1*sin(n.h()));
- n.rotate(n.ccr().x(), n.ccr().y(), a);
- WVPASS(!p.goal_found(n)); // fail near drivable border
- }
- for (double a = 0; a > -M_PI/2; a -= 0.01) {
- n = RRTNode(g);
- n.rotate(g.ccl().x(), g.ccl().y(), a);
- WVPASS(p.goal_found(n)); // pass drivable border
- }
- for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
- // + 0.1 -- compensate for Euclid. dist. check
- n = RRTNode(g);
- n.x(n.x() - 0.1*cos(n.h()));
- n.y(n.y() - 0.1*sin(n.h()));
- n.rotate(n.ccl().x(), n.ccl().y(), a);
- WVPASS(p.goal_found(n)); // pass near drivable border
- }
- for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
- // = -0.1 -- compensate for near goal
- n = RRTNode(g);
- n.x(n.x() + 0.1*cos(n.h()));
- n.y(n.y() + 0.1*sin(n.h()));
- n.rotate(n.ccl().x(), n.ccl().y(), a);
- WVPASS(!p.goal_found(n)); // fail near drivable border
- }
-}
projects / hubacji1 / rrts.git / commitdiff