Change steer step to 0.5 m

diff --git a/src/rrtext12.cc b/src/rrtext12.cc
index 4b66370d1b42549ecf389f66ba125f808bd901f8..4dacd107e8bc13869f9f7d0effa6909346f126f3 100644 (file)
--- a/src/rrtext12.cc
+++ b/src/rrtext12.cc
@@ -11,7 +11,7 @@ void RRTExt12::steer1(RRTNode &f, RRTNode &t)
         auto max_steer = bc.st();
         // map t.h() in -PI...+PI to -max_steer...+max_steer
         bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer);
-        bc.sp((this->udy_(this->gen_) > 0.5) ? 0.2 : -0.2);
+        bc.sp((this->udy_(this->gen_) > 0.5) ? 0.5 : -0.5);
         this->steered().clear();
         while (true) {
                 bc.next();
@@ -59,7 +59,7 @@ bool RRTExt12::connect()
         if (sqrt(
                 pow(f->x() - t->x(), 2)
                 + pow(f->y() - t->y(), 2)
-        ) > 0.2)
+        ) > 0.5)
                 return false;
         // cont.
         this->store_node(this->steered().front());

diff --git a/src/rrtext8.cc b/src/rrtext8.cc
index 8cd461e1cc967fa58a6ce5b80112e5bfdbe4a8ae..7101888228bfacb9a12ddacfe91a1181da54f3be 100644 (file)
--- a/src/rrtext8.cc
+++ b/src/rrtext8.cc
@@ -142,7 +142,7 @@ RRTNode *RRTExt8::nn(RRTNode &t)
 
 std::vector<RRTNode *> RRTExt8::nv(RRTNode &t)
 {
-        double cost = std::min(GAMMA(this->nodes().size()), 0.2);
+        double cost = std::min(GAMMA(this->nodes().size()), 0.5);
         std::vector<RRTNode *> n;
         this->nv(n, t, this->kdroot_, 0, cost);
         return n;

diff --git a/src/rrts.cc b/src/rrts.cc
index 6ed3d69803aa247dc229c416995ba6a155b2acb0..2856dce6b2b911c8fa5bc73ba2ffd7b5cabc70da 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -355,7 +355,7 @@ void RRTS::steer(RRTNode &f, RRTNode &t)
         double q0[] = {f.x(), f.y(), f.h()};
         double q1[] = {t.x(), t.y(), t.h()};
         ReedsSheppStateSpace rsss(this->bc.mtr());
-        rsss.sample(q0, q1, 0.2, cb_rs_steer, &this->steered());
+        rsss.sample(q0, q1, 0.5, cb_rs_steer, &this->steered());
 }
 void RRTS::tmp_steer(RRTNode &f, RRTNode &t)
 {
@@ -363,7 +363,7 @@ void RRTS::tmp_steer(RRTNode &f, RRTNode &t)
     double q0[] = {f.x(), f.y(), f.h()};
     double q1[] = {t.x(), t.y(), t.h()};
     ReedsSheppStateSpace rsss(this->bc.mtr());
-    rsss.sample(q0, q1, 0.2, cb_rs_steer, &this->tmp_steered_);
+    rsss.sample(q0, q1, 0.5, cb_rs_steer, &this->tmp_steered_);
 }
 
 void RRTS::steer1(RRTNode &f, RRTNode &t)