Fix steer toward goal in rrts next method

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 6bcf151811721854a0d226499b0eb95eb4345016..85bd8ed043210fd2558e387556911d17d7e4b2b7 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -534,13 +534,15 @@ RRTS::next()
        this->join_steered(&this->_nodes.back());
        RRTNode* just_added = &this->_nodes.back();
        bool gf = false;
-       while (ss > 0 && just_added->p() != nullptr) {
+       while (ss > 0 && just_added != nullptr) {
                this->steer(*just_added, this->_goal);
                if (this->collide_steered()) {
                        ss--;
                        just_added = just_added->p();
                        continue;
                }
+               // The first of steered is the same as just_added.
+               this->_steered.erase(this->_steered.begin());
                this->join_steered(just_added);
                bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
                bool gd = this->goal_drivable_from(this->_nodes.back());