Finish when nothing to steer in opt

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 130b1385b64753cc7bd4c685f8e1fd8554d5b2b1..7a7c194d3e7df5519b2cd4cb223bc037cce30745 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -788,6 +788,8 @@ bool RRTBase::optp_dijkstra(
                         ch_cost = dnodes[tmp.ni].c +
                                 this->cost(cusps[tmp.ni], cusps[i]);
                         steered = this->steer(cusps[tmp.ni], cusps[i]);
+                        if (steered.size() <= 0)
+                                break;
                         for (unsigned int j = 0; j < steered.size() - 1; j++)
                                 steered[j]->add_child(steered[j + 1], 1);
                         if (this->collide(