From 1f40bfce944badc91c1c2ada26b5df3daacd7507 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 2 Apr 2019 09:07:59 +0200
Subject: [PATCH] Add ROS service skeleton, TODOs

---
 base/ros.cc | 41 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 41 insertions(+)

diff --git a/base/ros.cc b/base/ros.cc
index 1484ff7..564a101 100644
--- a/base/ros.cc
+++ b/base/ros.cc
@@ -16,6 +16,7 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 */
 
 #include "ros/ros.h"
+#include "iamcar/Park.h"
 #include "obstacle_msgs/Obstacles.h"
 
 #include "compile.h"
@@ -26,11 +27,51 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #define USE_TMAX
 //#define USE_SLOTPLANNER
 
+bool park(
+        iamcar::Park::Request &req,
+        iamcar::Park::Response &res
+)
+{
+        PLANNER p(
+                new RRTNode(), // TODO get req.init, req.goal
+                new RRTNode()
+        );
+        p.ocost(p.root());
+        p.ocost(p.goal());
 
+        ParallelSlot ps = ParallelSlot();
+        ps.slot().add_bnode(new RRTNode()); // TODO req.slot
+        ps.setAll();
+        p.samplingInfo_ = ps.getSamplingInfo();
+        if (pi.x == pg.x && pi.y == pi.y && ps.slot().bnodes().size() > 0) {
+                p.goal(ps.getFP());
+                ps.fipr(ps.getFP());
+        }
+        if (ps.cusp().size() > 0) {
+                p.goal(ps.cusp().front().front());
+                p.slot_cusp(ps.cusp().front());
+        }
+        p.tstart();
+        p.tend();
+        while (!p.goal_found() && p.elapsed() < TMAX) {
+                p.next();
+                p.tend();
+                if (p.opt_path()) {
+                        if (ps.cusp().size() > 0)
+                                p.tlog(p.findt(p.slot_cusp().back()));
+                        else
+                                p.tlog(p.findt());
+                }
+        }
+        // TODO res.path
+        return true;
+}
 
 int main(int argc, char **argv)
 {
         ros::init(argc, argv, "iamcar");
+        ros::NodeHandle nh;
+        ros::ServiceServer park_srv = nh.advertiseService("park", park);
         ros::spin();
         return 0;
 }
-- 
2.39.2