Add middle node getter

diff --git a/base/main.cc b/base/main.cc
index 611b102bcd763c0ccc7158306673c68beb186b3d..bde7bbec6a431865c6372532c915df1964f9de8a 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -198,7 +198,7 @@ int main()
                 p.useSamplingInfo_ = true;
         }
         if (ps.cusp().size() > 0) {
-                p.goal(ps.cusp().front().front());
+                p.goal(ps.getMidd());
                 p.slot_cusp(ps.cusp().front()); // use first found solution
                 jvo["midd"][0] = p.goal()->x();
                 jvo["midd"][1] = p.goal()->y();

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index cb804d277dfbd0f6b81e875ace1caa2639f8b259..c107693bfc4a939ff9441678b8411b527145a971 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -25,6 +25,14 @@ ParallelSlot::ParallelSlot()
 {}
 
 // getter
+RRTNode *ParallelSlot::getMidd()
+{
+        if (this->cusp().size() > 0)
+                return this->cusp().front().front();
+        else
+                return nullptr;
+}
+
 std::vector<std::vector<RRTNode *>> &ParallelSlot::cusp()
 {
         return this->cusp_;

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index c7b7dc23c67e62110efea56099c6651df9df8766..f10b7def354653ce59f983d1fa2f26ab68ab3f89 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -57,6 +57,8 @@ class ParallelSlot {
                 ParallelSlot();
 
                 // getter
+                /** Get slot entry point */
+                RRTNode *getMidd();
                 std::vector<std::vector<RRTNode *>> &cusp();
                 float DH() const;
                 PolygonObstacle &slot();