From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 22 May 2019 07:08:35 +0000 (+0200)
Subject: Add middle node getter
X-Git-Tag: v0.7.0~20^2~12
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/683cdc3006c761e19a9cffd3450ea78e7da8e39b

Add middle node getter
---

diff --git a/base/main.cc b/base/main.cc
index 611b102..bde7bbe 100644
--- 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 cb804d2..c107693 100644
--- 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 c7b7dc2..f10b7de 100644
--- 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();