From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 12 Jul 2021 14:08:08 +0000 (+0200)
Subject: Change steer step to 0.5 m
X-Git-Tag: v0.7.0~1
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/c4670804d20193d574c3f86e282a07ceddbbfd09

Change steer step to 0.5 m
---

diff --git a/src/rrtext12.cc b/src/rrtext12.cc
index 4b66370..4dacd10 100644
--- 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 8cd461e..7101888 100644
--- 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 6ed3d69..2856dce 100644
--- 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)