Add goal_found method for two RRTNodes

[hubacji1/iamcar.git] / base / main.cc

diff --git a/base/main.cc b/base/main.cc
index 422e3308ad572ecb0b803a4927875fe50c67346e..8c544c198ec498930ace88e3fac16a0d7935e75f 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -197,10 +197,10 @@ int main()
                 p.samplingInfo_ = ps.getSamplingInfo();
                 p.useSamplingInfo_ = true;
         }
-        p.goals(ps.goals());
         if (ps.cusp().size() > 0) {
                 p.goal(ps.getMidd());
                 p.slot_cusp(ps.cusp().front()); // use first found solution
+                p.goals(ps.goals());
                 jvo["midd"][0] = p.goal()->x();
                 jvo["midd"][1] = p.goal()->y();
                 jvo["midd"][2] = p.goal()->h();