Remove high resolution steering

Because 0.01 is too fine for IS_NEAR macro and it caused backward move
for Dubins paths.

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index a3c5f60461768e20b4db4cb00b7345cca1038566..cdaca08a1cce42e46b4e36d9baf08ea0387b045e 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -454,47 +454,6 @@ bool T2::next()
                         }
                 }
         }
-        en_add = true;
-        cusps = 0;
-        for (auto ns: this->steer(pn, rs, 0.01)) {
-                ns->rs(rs);
-                if (!en_add) {
-                        delete ns;
-                } else if (IS_NEAR(pn, ns)) {
-                        delete ns;
-                } else {
-                        if (sgn(ns->s()) == 0 || sgn(pn->s()) != sgn(ns->s()))
-                                cusps++;
-                        if (cusps > 4)
-                                en_add = false;
-                        nvs = this->nv(
-                                        ns,
-                                        MIN(
-                                                GAMMA_RRTSTAR(
-                                                        this->nodes().size()),
-                                                0.2)); // TODO const
-                        this->nodes().push_back(ns);
-                        this->add_iy(ns);
-                        // connect
-                        if (!this->connect(pn, ns, nvs)) {
-                                this->iy_[IYI(ns->y())].pop_back();
-                                this->nodes().pop_back();
-                                delete ns;
-                                en_add = false;
-                        } else {
-                                this->add_ixy(ns);
-                                // rewire
-                                this->rewire(nvs, ns);
-                                pn = ns;
-                                newly_added.push_back(pn);
-                                if (this->goal_found(pn, CO)) {
-                                        this->goal_cost();
-                                        this->tlog(this->findt());
-                                        en_add = false;
-                                }
-                        }
-                }
-        }
         if (this->samples().size() <= 1)
                 return this->goal_found();
         // steer to goal
@@ -535,41 +494,6 @@ bool T2::next()
                                 }
                         }
                 }
-                en_add = true;
-                cusps = 0;
-                for (auto ns: this->steer(pn, this->goal(), 0.01)) {
-                        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;
-                                        if (this->goal_found(pn, CO)) {
-                                                this->goal_cost();
-                                                this->tlog(this->findt());
-                                                en_add = false;
-                                        }
-                                }
-                        }
-                }
         }
         // steer to goals
         for (auto na: newly_added) {