Return path to first cusps if goal not found

diff --git a/base/main.cc b/base/main.cc
index da2c058d0f569cce5968f74b62c5fae4d5688c22..6ac49ededd13ea211dc867a3d4330864511e34bd 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -15,6 +15,7 @@ You should have received a copy of the GNU General Public License
 along with I am car. If not, see <http://www.gnu.org/licenses/>.
 */
 
+#include <algorithm>
 #include <iostream>
 #include <jsoncpp/json/json.h>
 #include <signal.h>
@@ -29,6 +30,7 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #define USE_GL
 #define USE_INTERRUPT
 #define USE_TMAX
+#define USE_TMAX_1ST_CUSPS
 
 bool run_planner = true;
 
@@ -136,6 +138,21 @@ int main()
                 nn = p.nn(p.nodes(), p.goal(), p.cost);
 #endif
                 tr = p.findt(nn);
+#ifdef USE_TMAX_1ST_CUSPS
+                while (p.opt_path()) {}
+                tr = p.findt(nn);
+                std::reverse(tr.begin(), tr.end());
+                for (auto n: tr) {
+                        if (n->s() != 0 &&
+                                        n->parent() &&
+                                        n->parent()->s() != 0 &&
+                                        sgn(n->s()) != sgn(n->parent()->s())) {
+                                nn = n;
+                                break;
+                        }
+                }
+                tr = p.findt(nn);
+#endif
         } else {
                 nn = p.goal();
                 tr = p.findt();