From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Sat, 25 May 2019 16:18:07 +0000 (+0200)
Subject: Add steer to goals section in T2
X-Git-Tag: v0.7.0~20^2~2
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/e48cf52e0f537ac2996d695e585b706111c492d6

Add steer to goals section in T2
---

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 444c498..0723be8 100644
--- 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();
 }