From 66c97cf641798c50f3acafb95623e1918d71b809 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <luca@openrobotics.org>
Date: Tue, 28 Sep 2021 20:58:34 +0800
Subject: [PATCH] Add lane speed limit to graph parsing function (#124)

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>
Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../src/rmf_fleet_adapter/agv/parse_graph.cpp       |  8 +++++++-
 rmf_fleet_adapter_python/src/graph/graph.cpp        | 11 ++++++++---
 rmf_fleet_adapter_python/src/graph/lane.cpp         | 13 ++++++++++++-
 rmf_fleet_adapter_python/tests/unit/test_graph.py   |  8 ++++++--
 4 files changed, 33 insertions(+), 7 deletions(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp
index 633a6c756..d0cfefe41 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp
@@ -287,9 +287,15 @@ rmf_traffic::agv::Graph parse_graph(
         entry_event = Event::make(Lane::Dock(dock_name, duration));
       }
 
-      graph.add_lane(
+      auto& graph_lane = graph.add_lane(
         {begin, entry_event},
         {end, exit_event, std::move(constraint)});
+
+      if (const YAML::Node speed_limit_option = options["speed_limit"])
+      {
+        const double speed_limit = speed_limit_option.as<double>();
+        graph_lane.properties().speed_limit(speed_limit);
+      }
     }
     vnum += vnum_temp;
   }
diff --git a/rmf_fleet_adapter_python/src/graph/graph.cpp b/rmf_fleet_adapter_python/src/graph/graph.cpp
index 079a3486e..45f192a91 100644
--- a/rmf_fleet_adapter_python/src/graph/graph.cpp
+++ b/rmf_fleet_adapter_python/src/graph/graph.cpp
@@ -135,6 +135,9 @@ void bind_graph(py::module& m)
   // Lanes
   .def("add_lane",
     &Graph::add_lane,
+    py::arg("entry"),
+    py::arg("exit"),
+    py::arg("properties") = Lane::Properties(),
     py::call_guard<py::scoped_ostream_redirect,
     py::scoped_estream_redirect>())
   .def("add_dock_lane",
@@ -142,7 +145,8 @@ void bind_graph(py::module& m)
     const std::size_t w0,
     const std::size_t w1,
     std::string dock_name,
-    int seconds)
+    int seconds,
+    Lane::Properties properties)
     {
       self.add_lane(
         {
@@ -151,13 +155,14 @@ void bind_graph(py::module& m)
             Lane::Dock(dock_name,
             std::chrono::seconds(seconds)))
         },
-        w1);
-      self.add_lane(w1, w0);
+        w1, properties);
+      self.add_lane(w1, w0, properties);
     },
     py::arg("w0"),
     py::arg("w1"),
     py::arg("dock_name"),
     py::arg("dock_seconds") = 10,
+    py::arg("properties") = Lane::Properties(),
     py::call_guard<py::scoped_ostream_redirect,
     py::scoped_estream_redirect>())
   .def("add_bidir_lane",
diff --git a/rmf_fleet_adapter_python/src/graph/lane.cpp b/rmf_fleet_adapter_python/src/graph/lane.cpp
index d387516d9..ce88337ce 100644
--- a/rmf_fleet_adapter_python/src/graph/lane.cpp
+++ b/rmf_fleet_adapter_python/src/graph/lane.cpp
@@ -1,4 +1,5 @@
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 #include <pybind11/chrono.h>
 
 #include <memory>
@@ -36,7 +37,17 @@ void bind_lane(py::module& m)
   .def_property_readonly("exit",
     py::overload_cast<>(&Lane::exit),
     py::return_value_policy::reference_internal)
-  .def_property_readonly("index", &Lane::index);
+  .def_property_readonly("index", &Lane::index)
+  .def_property_readonly("properties",
+    py::overload_cast<>(&Lane::properties),
+    py::return_value_policy::reference_internal);
+
+  // LANE PROPERTIES ===========================================================
+  py::class_<Lane::Properties>(m_lane, "Properties")
+  .def(py::init<>())
+  .def_property("speed_limit",
+    py::overload_cast<>(&Lane::Properties::speed_limit, py::const_),
+    py::overload_cast<std::optional<double>>(&Lane::Properties::speed_limit));
 
   // DOORS =====================================================================
   py::class_<Lane::Door>(m_lane, "Door")
diff --git a/rmf_fleet_adapter_python/tests/unit/test_graph.py b/rmf_fleet_adapter_python/tests/unit/test_graph.py
index a0b99f1a2..fb21fcb18 100644
--- a/rmf_fleet_adapter_python/tests/unit/test_graph.py
+++ b/rmf_fleet_adapter_python/tests/unit/test_graph.py
@@ -332,9 +332,13 @@ def test_graph():
              [8, 9],
              [8, 10]]
 
+    # test speed limit std::optional lane property
+    properties = lane.Properties()
+    properties.speed_limit = 10.0
     # Test add_lane
-    rawr_graph.add_lane(lane.Node(0),
-                        lane.Node(1))
+    added_lane = rawr_graph.add_lane(lane.Node(0),
+                        lane.Node(1), properties)
+    assert added_lane.properties.speed_limit == 10.0
     assert rawr_graph.num_lanes == 1
 
     # Test add_bidir_lane