Feature/add unstable participant api (#11)

* add api html docs and slight cleanup on readme

* change doc path to index.html

* update README

* Update shapes.cpp

* regenerate docs and fix test

* missing battery

* create adpt.schedule for particpant and traj

* add set_charger_wp()

* add update_idle_pos and lanes_from_waypoint

* add set_infinite_delay()

* fix nfn name of  set_infinite_delay()

README.md

@@ -14,15 +14,15 @@ For API Docs, see [the documentation](https://osrf.github.io/rmf_fleet_adapter_p
 
 ```bash
 source install/setup.bash
-colcon build --packages-select rmf_fleet_adapter_python
+colcon build --packages-select pybind_ament rmf_fleet_adapter_python
 ```
 
 ## Running Examples
 
 Integration test script for rmf loop and delivery task:
 
 ```shell
-ros2 run rmf_adapter_python test_adapter
+ros2 run rmf_fleet_adapter_python test_adapter
 ```
 
 You may then probe the effects on the ROS2 graph by subscribing to the following topics with `ros2 topic echo <TOPIC_NAME>`:

docs/gen_docs.sh

@@ -6,4 +6,5 @@ pydoc3 -w rmf_adapter \
           rmf_adapter.graph \
           rmf_adapter.nodes \
           rmf_adapter.easy_traffic_light \
-          rmf_adapter.battery
+          rmf_adapter.battery \
+          rmf_adapter.schedule

docs/index.html

@@ -1,2 +1 @@
 <meta http-equiv="refresh" content="0; url=https://osrf.github.io/rmf_fleet_adapter_python/rmf_adapter.html" />
-

rmf_fleet_adapter_python/CMakeLists.txt

@@ -45,6 +45,7 @@ pybind11_add_module(rmf_adapter
   src/nodes/nodes.cpp
   src/vehicletraits/vehicletraits.cpp
   src/battery/battery.cpp
+  src/schedule/schedule.cpp
 )
 target_link_libraries(rmf_adapter PRIVATE
   rmf_fleet_adapter::rmf_fleet_adapter
@@ -54,7 +55,7 @@ target_include_directories(rmf_adapter
   PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
-    ${rmf_traffic_ros2_INCLUDE_DIRS}
+    ${rmf_traffic_INCLUDE_DIRS}
     ${rmf_traffic_ros2_INCLUDE_DIRS}
     ${rmf_battery_INCLUDE_DIRS}
     ${rmf_task_INCLUDE_DIRS}

rmf_fleet_adapter_python/scripts/test_delivery.py

@@ -10,6 +10,7 @@
 import rmf_adapter.battery as battery
 import rmf_adapter.plan as plan
 import rmf_adapter.type as Type
+import rmf_adapter.schedule as schedule
 
 from rmf_fleet_adapter_python.test_utils import MockRobotCommand
 from rmf_fleet_adapter_python.test_utils import MockDispenser, MockIngestor
@@ -231,14 +232,36 @@ def updater_inserter(handle_obj, updater):
         0, 0, 5, 5, 6, 6, 7,
         6, 5, 5, 8, 8, 10], error_msg
 
+    # check if unstable partcipant works
+    # this is helpful for to update the robot when it is
+    print("Update a custom itinerary to fleet adapter")
+    traj = schedule.make_trajectory(
+        robot_traits,
+        adapter.now(),
+        [[3, 0, 0], [1, 1, 0], [2, 1, 0]])
+    route = schedule.Route("test_map", traj)
+
+    participant = robot_cmd.updater.get_unstable_participant()
+    routeid = participant.last_route_id
+    participant.set_itinerary([route])
+    new_routeid = participant.last_route_id
+    print(f"Previous route id: {routeid} , new route id: {new_routeid}")
+    assert routeid != new_routeid
+
+    # TODO(YL) There's an issue with during shutdown of the adapter, occurs
+    # when set_itinerary() function above is used. Similarly with a non-mock 
+    # adpater, will need to address this in near future
+    print("\n~ Shutting Down everything ~")
+
     cmd_node.destroy_node()
     observer.destroy_node()
     dispenser.destroy_node()
     ingestor.destroy_node()
 
+    robot_cmd.stop()
+    adapter.stop()
     rclpy_executor.shutdown()
     rclpy.shutdown()
 
-
 if __name__ == "__main__":
     main()

rmf_fleet_adapter_python/src/adapter.cpp

@@ -33,6 +33,7 @@ void bind_plan(py::module &);
 void bind_tests(py::module &);
 void bind_nodes(py::module &);
 void bind_battery(py::module &);
+void bind_schedule(py::module &);
 
 PYBIND11_MODULE(rmf_adapter, m) {
     bind_types(m);
@@ -43,6 +44,7 @@ PYBIND11_MODULE(rmf_adapter, m) {
     bind_tests(m);
     bind_nodes(m);
     bind_battery(m);
+    bind_schedule(m);
 
     // ROBOTCOMMAND HANDLE =====================================================
     // Abstract class
@@ -99,6 +101,10 @@ PYBIND11_MODULE(rmf_adapter, m) {
              py::arg("min_lane_length") = 1e-8,
              py::call_guard<py::scoped_ostream_redirect,
                             py::scoped_estream_redirect>())
+        .def("set_charger_waypoint", &agv::RobotUpdateHandle::set_charger_waypoint,
+             py::arg("charger_wp"),
+             py::call_guard<py::scoped_ostream_redirect,
+                            py::scoped_estream_redirect>())
         .def("update_battery_soc", &agv::RobotUpdateHandle::update_battery_soc,
              py::arg("battery_soc"),
              py::call_guard<py::scoped_ostream_redirect,
@@ -108,7 +114,16 @@ PYBIND11_MODULE(rmf_adapter, m) {
                  &agv::RobotUpdateHandle::maximum_delay, py::const_),
              [&](agv::RobotUpdateHandle& self){
                  return self.maximum_delay();
-             });
+             })
+        .def("set_infinite_delay",
+             [&](agv::RobotUpdateHandle& self){
+                self.maximum_delay(rmf_utils::nullopt);
+             })
+        .def("get_unstable_participant",
+             [&](agv::RobotUpdateHandle& self){
+                return self.unstable().get_participant();
+             },
+             "Experimental API to access the schedule participant");
 
     // FLEETUPDATE HANDLE ======================================================
     py::class_<agv::FleetUpdateHandle,
@@ -222,7 +237,12 @@ PYBIND11_MODULE(rmf_adapter, m) {
                 &agv::EasyTrafficLight::waiting_after),
             py::arg("checkpoint"),
             py::arg("location"))
-        .def("last_reached", &agv::EasyTrafficLight::last_reached);
+        .def("last_reached", &agv::EasyTrafficLight::last_reached)
+        .def("update_idle_location",
+            py::overload_cast<std::string, Eigen::Vector3d>(
+                &agv::EasyTrafficLight::update_idle_location),
+            py::arg("map_name"),
+            py::arg("position"));
 
     // prefix traffic light
     auto m_easy_traffic_light = m.def_submodule("easy_traffic_light");

rmf_fleet_adapter_python/src/geometry/shapes.cpp

@@ -52,6 +52,6 @@ void bind_shapes(py::module &m)
       [](double radius) {
         return std::make_shared<geometry::FinalConvexShape>(
             geometry::Circle(std::forward<double>(radius)).finalize_convex());
-      }),
-      py::arg("radius");
+      },
+      py::arg("radius"));
 }

rmf_fleet_adapter_python/src/graph/graph.cpp

@@ -168,7 +168,14 @@ 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_property_readonly("num_lanes", &Graph::num_lanes);
+      .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_property_readonly("num_lanes", &Graph::num_lanes)
+      .def("lanes_from_waypoint",
+          py::overload_cast<std::size_t>(&Graph::lanes_from, py::const_),
+          py::arg("wp_index"));
 
   // PARSE GRAPH ==============================================================
   // Helper function to parse a graph from a yaml file

rmf_fleet_adapter_python/src/nodes/nodes.cpp

@@ -59,11 +59,13 @@ void bind_nodes(py::module &m)
   py::class_<Dispatcher, std::shared_ptr<Dispatcher>>(
       m_nodes, "DispatcherNode")
       .def_static("make_node", &Dispatcher::make_node,
-                  py::arg("dispatcher_node_name"),
+                  // py::arg("dispatcher_node_name"), TODO Remove in next version
                   py::call_guard<py::scoped_ostream_redirect,
                                  py::scoped_estream_redirect>())
-      .def("submit_task", &Dispatcher::submit_task,
-           py::arg("task_description"))
+      // .def("submit_task",
+      //      py::overload_cast<const Dispatcher::TaskDescription&>(
+      //       &Dispatcher::submit_task),
+      //      py::arg("task_description"))
       .def("cancel_task", &Dispatcher::cancel_task,
            py::arg("task_id"))
       .def("spin", &Dispatcher::spin,

rmf_fleet_adapter_python/src/schedule/schedule.cpp

@@ -0,0 +1,86 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/eigen.h>
+#include <pybind11/chrono.h>
+#include <pybind11/stl.h>
+
+#include <rmf_traffic/schedule/Participant.hpp>
+#include <rmf_traffic/agv/Interpolate.hpp>
+#include <rmf_traffic/Route.hpp>
+
+namespace py = pybind11;
+namespace schedule = rmf_traffic::schedule;
+
+using SystemTimePoint = std::chrono::time_point<std::chrono::system_clock,
+                                                std::chrono::nanoseconds>;
+
+// TODO(YL): create a module to manage Time and Duration input and output
+rmf_traffic::Time to_rmf_time(SystemTimePoint time)
+{
+  return rmf_traffic::Time(time.time_since_epoch());
+}
+
+//==============================================================================
+void bind_schedule(py::module &m)
+{
+  auto m_schedule = m.def_submodule("schedule");
+
+  /// PARTICIPANT ==============================================================
+  py::class_<schedule::Participant,
+             std::shared_ptr<schedule::Participant>>(
+      m_schedule, "Participant")
+      .def_property_readonly("id", &schedule::Participant::id)
+      .def_property_readonly("version", &schedule::Participant::id)
+      .def_property_readonly("last_route_id", &schedule::Participant::last_route_id)
+      .def_property("delay",
+                    py::overload_cast<>(
+                        &schedule::Participant::delay, py::const_),
+                    py::overload_cast<
+                        rmf_traffic::Duration>(&schedule::Participant::delay))
+      .def("erase", &schedule::Participant::erase, py::arg("routes"))
+      .def("clear", &schedule::Participant::clear)
+      .def("get_itinerary", &schedule::Participant::itinerary)
+      .def("set_itinerary", &schedule::Participant::set, py::arg("itinerary"));
+
+  /// Writer::Item =============================================================
+  py::class_<schedule::Writer::Item>(m_schedule, "Item")
+      .def_readwrite("id", &schedule::Writer::Item::id)
+      .def_readwrite("route", &schedule::Writer::Item::route);
+
+  /// ROUTE ====================================================================
+  /// TODO(YL) The current organization of modules differ slighty with the
+  /// rmf_core directory structure. e.g. class Route should go directly
+  /// rmf_traffic. Will need to adfdress all these in near future, a fix
+  /// will be to create a generic python bindings for rmf_core itself.
+  py::class_<rmf_traffic::Route,
+             std::shared_ptr<rmf_traffic::Route>>(m_schedule, "Route")
+      .def(py::init<std::string &, rmf_traffic::Trajectory>(),
+           py::arg("map"),
+           py::arg("trajectory"))
+      .def_property("map",
+                    py::overload_cast<>(
+                        &rmf_traffic::Route::map, py::const_),
+                    py::overload_cast<
+                        std::string>(&rmf_traffic::Route::map))
+      .def_property("trajectory",
+                    py::overload_cast<>(
+                        &rmf_traffic::Route::trajectory, py::const_),
+                    py::overload_cast<
+                        rmf_traffic::Trajectory>(&rmf_traffic::Route::trajectory));
+
+  // MAKE TRAJECTORY FUNCTION ==================================================
+  m_schedule.def(
+      "make_trajectory",
+      [](const rmf_traffic::agv::VehicleTraits &traits,
+         SystemTimePoint start_time,
+         const std::vector<Eigen::Vector3d> &input_positions) {
+        return rmf_traffic::agv::Interpolate::positions(
+            traits, to_rmf_time(start_time), input_positions);
+      },
+      py::arg("traits"),
+      py::arg("start_time"),
+      py::arg("input_positions"));
+
+  // TRAJECTORY ================================================================
+  py::class_<rmf_traffic::Trajectory,
+             std::shared_ptr<rmf_traffic::Trajectory>>(m_schedule, "Trajectory");
+}