Change steer step to 0.5 m

[hubacji1/rrts.git] / src / rrtext12.cc

diff --git a/src/rrtext12.cc b/src/rrtext12.cc
index 1565faf91258ffa1d43f861286299ac3092402df..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());
@@ -74,7 +74,7 @@ bool RRTExt12::next()
         if (this->icnt_ == 0)
                 this->tstart_ = std::chrono::high_resolution_clock::now();
         bool next = true;
-        if (this->scnt_ > this->log_path_time_)
+        if (this->icnt_ > this->log_path_iter_)
             this->log_path_cost();
         if (this->should_stop())
                 return false;