Start from original goal

The problem is with parallel parking.

diff --git a/base/main.cc b/base/main.cc
index 3b1751d88e3a0ce7b324209cb2a4575cd5767be3..9779164f37564762067fa219b8f7c9afca4c0447 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -272,6 +272,8 @@ int main()
                         p.tlog(p.findt());
                 }
         }
+        if (ps.slotType() == PARALLEL)
+                p.tlog(p.findt(p.slot_cusp().back()));
 #elif defined USE_PTHREAD
         bool gf = false;
         RRTNode *ron = nullptr;