Fix forward dijkstra algorithm

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 8897216871c4dea33e7c13dca44c2159efa95a02..5be98881726a428952e24f8f00312102ad546597 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -120,12 +120,14 @@ RRTExt13::dijkstra_forward()
                        if (this->steered_.size() == 0) {
                                break;
                        }
-                       if (this->collide_steered()) {
+                       unsigned int ss = this->steered_.size();
+                       if (this->collide_steered()
+                                       || ss != this->steered_.size()) {
                                continue;
                        }
                        this->bc_.set_pose(this->steered_.back());
                        bool td = this->bc_.drivable(t);
-                       bool tn = this->bc_.edist(t) < this->eta_;
+                       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());