Fix ext13 (path opt) because of current changes

diff --git a/rrts/src/rrtext13.cc b/rrts/src/rrtext13.cc
index 1619d7bce3e583bffdcbb6e96cd89e30b1cbec41..7277bdb30495ecedb3c3177f8894eb1d3f0c56d9 100644 (file)
--- a/rrts/src/rrtext13.cc
+++ b/rrts/src/rrtext13.cc
@@ -37,7 +37,7 @@ RRTExt13::interesting_forward()
                                i = j;
                                break;
                        }
-                       if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+                       if (nj.p_is_cusp()) {
                                this->dn_.push_back(DijkstraNode(&njl));
                                this->dn_.back().i = this->dn_.size() - 1;
                        }
@@ -79,7 +79,7 @@ RRTExt13::interesting_backward()
                                i = j;
                                break;
                        }
-                       if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+                       if (nj.p_is_cusp()) {
                                this->dn_.push_back(DijkstraNode(&njl));
                                this->dn_.back().i = this->dn_.size() - 1;
                                this->dn_.back().d = goal_cc - njl.cc();
@@ -119,12 +119,14 @@ RRTExt13::dijkstra_forward()
                                        || ss != this->_steered.size()) {
                                continue;
                        }
+                       // The first steered is the same as f.
+                       this->_steered.erase(this->_steered.begin());
                        this->set_bc_pose_to(this->_steered.back());
                        bool td = this->_bc.drivable(t);
                        bool tn = this->_bc.edist(t) < 2.0 * this->_eta;
                        if (cost < t.cc() && td && tn) {
                                this->join_steered(&f);
-                               t.p(this->_nodes.back());
+                               t.p(this->_nodes.back(), true);
                                t.c(this->cost_build(this->_nodes.back(), t));
                                if (!this->dn_[i].vi()) {
                                        pq.push(this->dn_[i]);
@@ -162,13 +164,15 @@ RRTExt13::dijkstra_backward()
                                        || ss != this->_steered.size()) {
                                continue;
                        }
+                       // The first steered is the same as f.
+                       this->_steered.erase(this->_steered.begin());
                        this->set_bc_pose_to(this->_steered.back());
                        bool td = this->_bc.drivable(t);
                        bool tn = this->_bc.edist(t) < 2.0 * this->_eta;
                        if (cost < fd.d && td && tn) {
                                fd.d = cost;
                                this->join_steered(&f);
-                               t.p(this->_nodes.back());
+                               t.p(this->_nodes.back(), true);
                                t.c(this->cost_build(this->_nodes.back(), t));
                                this->recompute_cc_for_predecessors_and(&t);
                                if (!this->dn_[i].vi()) {