Add tmp steered

diff --git a/api/rrts.h b/api/rrts.h
index f4f1f58e4e058821fc29387fc1815e0af296cd7c..f5e297841597434dd53c64fb0e2b0206a97866fc 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -169,6 +169,7 @@ class RRTS {
                 */
                 void set_sample_uniform_circle();
         protected:
+                std::vector<RRTNode> tmp_steered_;
                 bool finishit = false;
                 double path_cost_before_opt_ = 9999;
 
@@ -197,6 +198,7 @@ class RRTS {
                 virtual RRTNode *nn(RRTNode &t);
                 virtual std::vector<RRTNode *> nv(RRTNode &t);
                 void steer(RRTNode &f, RRTNode &t);
+                void tmp_steer(RRTNode &f, RRTNode &t);
                 virtual void steer1(RRTNode &f, RRTNode &t);
                 virtual void steer2(RRTNode &f, RRTNode &t);
                 /*! \brief Join steered nodes to RRT data structure
@@ -204,6 +206,7 @@ class RRTS {
                 \param f RRT node to join steered nodes to.
                 */
                 void join_steered(RRTNode *f);
+                void join_tmp_steered(RRTNode *f);
                 virtual bool goal_found(RRTNode &f);
                 // RRT* procedures
                 bool connect();

diff --git a/src/rrts.cc b/src/rrts.cc
index 10d60fdfcd8b4654ec77136c78a54fd81ced45d1..9d750e45cd8be271143b699d274621546b8ea97f 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -7,6 +7,17 @@ template <typename T> int sgn(T val) {
         return (T(0) < val) - (val < T(0));
 }
 
+double edist(RRTNode const& n1, RRTNode const& n2)
+{
+    auto dx = n2.x() - n1.x();
+    auto dy = n2.y() - n1.y();
+    return sqrt(dx*dx + dy*dy);
+}
+double edist(RRTNode const* n1, RRTNode const* n2)
+{
+    return edist(*n1, *n2);
+}
+
 RRTNode::RRTNode()
 {
 }
@@ -323,6 +334,14 @@ void RRTS::steer(RRTNode &f, RRTNode &t)
         ReedsSheppStateSpace rsss(this->bc.mtr());
         rsss.sample(q0, q1, 0.2, cb_rs_steer, &this->steered());
 }
+void RRTS::tmp_steer(RRTNode &f, RRTNode &t)
+{
+    this->tmp_steered_.clear();
+    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_);
+}
 
 void RRTS::steer1(RRTNode &f, RRTNode &t)
 {
@@ -345,6 +364,17 @@ void RRTS::join_steered(RRTNode *f)
                 f = t;
         }
 }
+void RRTS::join_tmp_steered(RRTNode *f)
+{
+        while (this->tmp_steered_.size() > 0) {
+                this->store_node(this->tmp_steered_.front());
+                RRTNode *t = &this->nodes().back();
+                t->p(f);
+                t->c(this->cost_build(*f, *t));
+                this->tmp_steered_.erase(this->tmp_steered_.begin());
+                f = t;
+        }
+}
 
 bool RRTS::goal_found(RRTNode &f)
 {
@@ -380,6 +410,18 @@ bool RRTS::connect()
                         cost = this->cost_search(*n, *t);
                 }
         }
+        // steer from f->t and then continue with the steered.
+        this->tmp_steer(*f, *t);
+        if (this->tmp_steered_.size() > 0) {
+                auto col = this->collide_tmp_steered_from(*f);
+                if (std::get<0>(col))
+                        return false;
+                this->join_tmp_steered(f);
+                f = &this->nodes().back();
+        }
+        if (edist(f, t) > 0.2)
+                return false;
+        // cont.
         this->store_node(this->steered().front());
         t = &this->nodes().back();
         t->p(f);