Add steer to goals section in T2

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 444c498b5ec63dcb3f6b61aa4ded85349748d398..0723be8a59fdebd48441839fa5e9e32fcd0cad4e 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -499,6 +499,7 @@ bool T2::next()
         }
         if (this->samples().size() <= 1)
                 return this->goal_found();
+        // steer to goal
         for (auto na: newly_added) {
                 pn = na;
                 en_add = true;
@@ -572,6 +573,50 @@ bool T2::next()
                         }
                 }
         }
+        // steer to goals
+        for (auto na: newly_added) {
+        for (auto go: this->goals()) {
+                pn = na;
+                en_add = true;
+                cusps = 0;
+                for (auto ns: this->steer(na, go)) {
+                        ns->rs(rs);
+                        if (!en_add) {
+                                delete ns;
+                        } else if (IS_NEAR(pn, ns)) {
+                                delete ns;
+                        } else {
+                                if (sgn(pn->s()) != sgn(ns->s()))
+                                        cusps++;
+                                if (cusps > 4)
+                                        en_add = false;
+                                this->nodes().push_back(ns);
+                                this->add_iy(ns);
+                                pn->add_child(ns, this->cost(pn, ns));
+                                if (this->collide(pn, ns)) {
+                                        pn->children().pop_back();
+                                        ns->remove_parent();
+                                        this->iy_[IYI(ns->y())].pop_back();
+                                        this->nodes().pop_back();
+                                        delete ns;
+                                        en_add = false;
+                                } else {
+                                        this->add_ixy(ns);
+                                        this->ocost(ns);
+                                        pn = ns;
+                                        RRTNode *oldgoal = this->goal();
+                                        this->goal(go);
+                                        if (this->goal_found(pn, CO)) {
+                                                this->goal_cost();
+                                                this->tlog(this->findt());
+                                                en_add = false;
+                                        } else {
+                                                this->goal(oldgoal);
+                                        }
+                                }
+                        }
+                }
+        }}
         return this->goal_found();
 }