Load parking slot from ROS request

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

diff --git a/base/ros.cc b/base/ros.cc
index eec45d6546796f0ec5ac12163f305f03a5e7adcb..b0e5595c5216474e058bdbe5c110bb8cf82ea58c 100644 (file)
--- a/base/ros.cc
+++ b/base/ros.cc
@@ -93,7 +93,8 @@ bool park(
         p.ocost(p.goal());
 
         ParallelSlot ps = ParallelSlot();
-        ps.slot().add_bnode(new RRTNode()); // TODO req.slot
+        for (auto n: req.slot.bnodes)
+                ps.slot().add_bnode(new RRTNode(n.x, n.y, 0));
         ps.setAll();
         p.samplingInfo_ = ps.getSamplingInfo();
         if (pi.x == pg.x && pi.y == pi.y && ps.slot().bnodes().size() > 0) {