Feature/python binding planner

rmf_fleet_adapter_python/CMakeLists.txt

@@ -5,9 +5,9 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS on)
 
 find_package(ament_cmake REQUIRED)
 
-# Default to C++14
+# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

rmf_fleet_adapter_python/src/adapter.cpp

@@ -123,6 +123,7 @@ PYBIND11_MODULE(rmf_adapter, m) {
              [&](agv::RobotUpdateHandle& self){
                 return self.unstable().get_participant();
              },
+             py::return_value_policy::reference_internal,
              "Experimental API to access the schedule participant");
 
     // FLEETUPDATE HANDLE ======================================================
@@ -275,7 +276,12 @@ PYBIND11_MODULE(rmf_adapter, m) {
     py::class_<rclcpp::Node, std::shared_ptr<rclcpp::Node> >(m, "Node")
         .def("now", [](rclcpp::Node& self){
             return rmf_traffic_ros2::convert(self.now());
-        });
+        })
+        .def("use_sim_time", [](rclcpp::Node& self)
+            {
+              rclcpp::Parameter param("use_sim_time", true);
+              self.set_parameter(param);
+            });
 
     // Python rclcpp init and spin call
     m.def("init_rclcpp", [](){ rclcpp::init(0, nullptr); });

rmf_fleet_adapter_python/src/graph/graph.cpp

@@ -168,10 +168,15 @@ void bind_graph(py::module &m) {
       .def("get_lane", py::overload_cast<std::size_t>(&Graph::get_lane))
       .def("get_lane", py::overload_cast<std::size_t>(
           &Graph::get_lane, py::const_))
-      .def("lane_from", py::overload_cast<std::size_t, std::size_t>(
-        &Graph::lane_from))
-      .def("lane_from", py::overload_cast<std::size_t, std::size_t>(
-        &Graph::lane_from, py::const_))
+      .def(
+        "lane_from",
+        py::overload_cast<std::size_t, std::size_t>(&Graph::lane_from),
+        py::return_value_policy::reference_internal)
+      .def(
+        "lane_from",
+        py::overload_cast<std::size_t, std::size_t>(
+          &Graph::lane_from, py::const_),
+        py::return_value_policy::reference_internal)
       .def_property_readonly("num_lanes", &Graph::num_lanes)
       .def("lanes_from_waypoint",
           py::overload_cast<std::size_t>(&Graph::lanes_from, py::const_),

rmf_fleet_adapter_python/src/planner/planner.cpp

@@ -18,6 +18,9 @@ using Goal = Planner::Goal;
 using Options = Planner::Options;
 using Configuration = Planner::Configuration;
 using Result = Planner::Result;
+using Graph = rmf_traffic::agv::Graph;
+using VehicleTraits = rmf_traffic::agv::VehicleTraits;
+using Interpolate = rmf_traffic::agv::Interpolate;
 
 using TimePoint = std::chrono::time_point<std::chrono::system_clock,
                                           std::chrono::nanoseconds>;
@@ -45,6 +48,12 @@ Start make_start(TimePoint initial_time,
                initial_lane);
 }
 
+Planner make_planner(Configuration config)
+{
+  const auto default_options = Options{nullptr};
+  return Planner(config, default_options);
+}
+
 void bind_plan(py::module &m) {
   auto m_plan = m.def_submodule("plan");
 
@@ -144,4 +153,41 @@ void bind_plan(py::module &m) {
                     py::overload_cast<>(&Plan::Goal::orientation, py::const_),
                     py::overload_cast<double>(&Plan::Goal::orientation))
       .def("any_orientation", &Plan::Goal::any_orientation);
+
+  // Configuration =============================================================
+  py::class_<Configuration>(m_plan, "Configuration")
+      .def(py::init<Graph, VehicleTraits>(),
+           py::arg("graph"),
+           py::arg("traits"))
+      .def_property_readonly("graph", &Configuration::graph)
+      .def_property_readonly("traits", &Configuration::vehicle_traits);
+
+  // Options =============================================================
+  // TODO
+
+  // Planner ===================================================================
+  py::class_<Planner>(m_plan, "Planner")
+      .def(py::init([&](Configuration& config)
+          {
+            return new Planner(config, Planner::Options{nullptr} );
+          }),
+          py::arg("config"))
+      .def("get_plan_waypoints",
+          [&](Planner& self,
+              Start start,
+              Goal goal)
+          {
+            std::vector<Plan::Waypoint> waypoints;
+            const auto result = self.plan(start, goal);
+            if (result.success())
+            {
+              waypoints = result->get_waypoints();
+            }
+
+            return waypoints;
+
+          },
+          py::arg("start"), py::arg("goal"),
+          py::return_value_policy::reference_internal);
+
 }

rmf_fleet_adapter_python/tests/unit/test_planner.py

@@ -0,0 +1,77 @@
+#! /usr/bin/env python3 
+
+import rmf_adapter.vehicletraits as traits
+import rmf_adapter.geometry as geometry
+import rmf_adapter.graph as graph
+import rmf_adapter.graph.lane as lane
+import rmf_adapter.plan as plan
+
+import datetime
+
+"""
+                  10
+                   |
+                   |
+                   8------9
+                   |      |
+                   |      |
+     3------4------5------6------7
+                   |      |
+                   |      |
+                   1------2
+                   |
+                   |
+                   0
+"""
+map_name = "test_map"
+nav_graph = graph.Graph()
+nav_graph.add_waypoint(map_name, [0.0, -10.0])  # 0
+nav_graph.add_waypoint(map_name, [0.0, -5.0])  # 1
+nav_graph.add_waypoint(map_name, [5.0, -5.0])  # 2
+nav_graph.add_waypoint(map_name, [-10.0, 0])  # 3
+nav_graph.add_waypoint(map_name, [-5.0, 0.0])  # 4
+nav_graph.add_waypoint(map_name, [0.0, 0.0])  # 5
+nav_graph.add_waypoint(map_name, [5.0, 0.0])  # 6
+nav_graph.add_waypoint(map_name, [10.0, 0.0])  # 7
+nav_graph.add_waypoint(map_name, [0.0, 5.0]) \
+    .set_holding_point(True)  # 8
+nav_graph.add_waypoint(map_name, [5.0, 5.0]) \
+    .set_passthrough_point(True)  # 9
+nav_graph.add_waypoint(map_name, [0.0, 10.0]) \
+    .set_parking_spot(True)  # 10
+
+lanes = [[0, 1],
+         [1, 2],
+         [1, 5],
+         [2, 6],
+         [3, 4],
+         [4, 5],
+         [5, 6],
+         [6, 7],
+         [5, 8],
+         [6, 9],
+         [8, 9],
+         [8, 10]]
+for _lane in lanes:
+    nav_graph.add_bidir_lane(*_lane)
+assert nav_graph.num_lanes == 24
+
+profile = traits.Profile(geometry.make_final_convex_circle(1.0),
+    geometry.make_final_convex_circle(1.5))
+vehicle_traits = traits.VehicleTraits(
+    linear=traits.Limits(0.5, 0.3),
+    angular=traits.Limits(0.3, 0.4),
+    profile=profile)
+vehicle_traits.differential.reversible = False
+
+config = plan.Configuration(nav_graph, vehicle_traits)
+planner = plan.Planner(config)
+assert(planner)
+
+start = plan.Start(
+    datetime.datetime.today(),
+    0,
+    0.0)
+goal = plan.Goal(7)
+waypoints = planner.get_plan_waypoints(start, goal)
+assert(waypoints)