Support apis for task dispatcher (#10)

* support apis for task dispatcher

* append rmf_adapter.battery

* fix def_property for battery

* add dispatcher node to expand test scrips

* create a simple example for dispatcher node

* make loop and delivery test scripts work, compatible to task dispatcher

* fix test_adpater link in setup.py

* fix link on examples, update bindings according to feedbacks

* update rmf_battery bindings

* restructure pkg and clean setup.py

* fix typo

* update graph wp bind: is charger

rmf_fleet_adapter_python/CMakeLists.txt

@@ -20,6 +20,9 @@ if(NOT CMAKE_BUILD_TYPE)
 endif()
 
 find_package(rmf_fleet_adapter REQUIRED)
+find_package(rmf_task REQUIRED)
+find_package(rmf_task_ros2 REQUIRED)
+find_package(rmf_battery REQUIRED)
 find_package(rmf_task_msgs REQUIRED)
 
 include_directories(
@@ -41,15 +44,21 @@ pybind11_add_module(rmf_adapter
   src/planner/planner.cpp
   src/nodes/nodes.cpp
   src/vehicletraits/vehicletraits.cpp
+  src/battery/battery.cpp
 )
 target_link_libraries(rmf_adapter PRIVATE
-  rmf_fleet_adapter::rmf_fleet_adapter)
+  rmf_fleet_adapter::rmf_fleet_adapter
+  rmf_task_ros2::rmf_task_ros2)
 
 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_ros2_INCLUDE_DIRS}
+    ${rmf_battery_INCLUDE_DIRS}
+    ${rmf_task_INCLUDE_DIRS}
+    ${rmf_task_ros2_INCLUDE_DIRS}
     ${rmf_fleet_msgs_INCLUDE_DIRS}
     ${rclcpp_INCLUDE_DIRS}
   PRIVATE

rmf_fleet_adapter_python/package.xml

@@ -11,6 +11,7 @@
 
   <depend>pybind_ament</depend>
   <depend>rmf_fleet_adapter</depend>
+  <depend>rmf_task_ros2</depend>
 
   <export>
     <build_type>ament_python</build_type>

rmf_fleet_adapter_python/scripts/dispatcher_node.py

@@ -0,0 +1,67 @@
+# Copyright 2021 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import time
+import rmf_adapter as adpt
+import rmf_adapter.type as Type
+import rmf_adapter.nodes as Nodes
+
+from threading import Thread
+
+
+def submit_task_thread(mod):
+    time.sleep(2)
+    task_desc = Type.CPPTaskDescriptionMsg()
+    task_desc.delivery = adpt.type.CPPDeliveryMsg()
+    print("Sumbiting 2 sample delivery tasks")
+    id1 = mod.submit_task(task_desc)
+    id2 = mod.submit_task(task_desc)
+    print(f" active list >>  {mod.get_active_task_ids()}")
+
+    time.sleep(3)
+    print("---- Assertion ----")
+    state1 = mod.get_task_state(id1)
+    state2 = mod.get_task_state(id2)
+    state3 = mod.get_task_state("null_id")
+    print(f" {id1}: {state1}   \n {id2}: {state2} \n null_id: {state3}")
+
+    assert state1 == Nodes.TaskState.Failed, "Fail due to absence of a bid"
+    assert state2 == Nodes.TaskState.Pending, "state should be pending"
+    assert state3 == None, "state should be none"
+    assert mod.cancel_task(id2), "failed to cancel task"
+
+    # check if task is canceled
+    state2 = mod.get_task_state(id2)
+    print(f" Canceled:: {id2}: {state2} \n")
+    assert state2 == Nodes.TaskState.Canceled, "task should be canceled"
+    print("Done Check")
+
+
+def main():
+    print("Starting Simple Dispatcher Node")
+    adpt.init_rclcpp()
+    dispatcher = Nodes.DispatcherNode.make_node("sample_dispatcher_node")
+
+    th1 = Thread(target=submit_task_thread, args=(dispatcher,))
+    th1.start()
+
+    while True:
+        adpt.spin_some_rclcpp(dispatcher.node())
+        time.sleep(0.2)
+
+    print("Exiting")
+
+
+if __name__ == "__main__":
+    main()

rmf_fleet_adapter_python/scripts/test_delivery.py

@@ -1,21 +1,23 @@
 import rclpy
+import time
 from rclpy.executors import SingleThreadedExecutor
 from rclpy.node import Node
 
 import rmf_adapter as adpt
 import rmf_adapter.vehicletraits as traits
 import rmf_adapter.geometry as geometry
 import rmf_adapter.graph as graph
+import rmf_adapter.battery as battery
 import rmf_adapter.plan as plan
+import rmf_adapter.type as Type
 
 from rmf_fleet_adapter_python.test_utils import MockRobotCommand
 from rmf_fleet_adapter_python.test_utils import MockDispenser, MockIngestor
 from rmf_fleet_adapter_python.test_utils import TaskSummaryObserver
 
 from functools import partial
 
-
-test_name = 'test_delivery'
+test_name = 'test_delivery'  # aka task_id
 map_name = "test_map"
 fleet_name = "test_fleet"
 
@@ -113,7 +115,32 @@ def main():
     # Manages delivery or loop requests
     adapter = adpt.MockAdapter("TestDeliveryAdapter")
     fleet = adapter.add_fleet(fleet_name, robot_traits, test_graph)
-    fleet.accept_delivery_requests(lambda x: True)
+
+    # Set up task request callback function
+    # we will only accept delivery task here
+    def task_request_cb(task_profile):
+        from rmf_task_msgs.msg import TaskType	
+        if(task_profile.description.task_type == TaskType.TYPE_DELIVERY):
+            return True
+        else:
+            return False
+
+    fleet.accept_task_requests(task_request_cb)
+
+    # Set fleet battery profile
+    battery_sys = battery.BatterySystem.make(24.0, 40.0, 8.8)
+    mech_sys = battery.MechanicalSystem.make(70.0, 40.0, 0.22)
+    motion_sink = battery.SimpleMotionPowerSink(battery_sys, mech_sys)
+    ambient_power_sys = battery.PowerSystem.make(20.0)
+    ambient_sink = battery.SimpleDevicePowerSink(battery_sys, ambient_power_sys)
+    tool_power_sys = battery.PowerSystem.make(10.0)
+    tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys)
+
+    fleet.set_recharge_threshold(0.2)
+    b_success = fleet.set_task_planner_params(
+        battery_sys, motion_sink, ambient_sink, tool_sink)
+
+    assert b_success, "set battery param failed"
 
     cmd_node = Node("RobotCommandHandle")
 
@@ -132,6 +159,7 @@ def main():
 
     # Lambda to insert an adapter
     def updater_inserter(handle_obj, updater):
+        updater.update_battery_soc(1.0)
         handle_obj.updater = updater
 
     # Manages and executes robot commands
@@ -148,6 +176,7 @@ def updater_inserter(handle_obj, updater):
     ingestor = MockIngestor(ingestor_name)
 
     # INIT TASK SUMMARY OBSERVER ==============================================
+    # Note: this is used for assertation check on TaskSummary.Msg
     observer = TaskSummaryObserver()
 
     # FINAL PREP ==============================================================
@@ -163,16 +192,24 @@ def updater_inserter(handle_obj, updater):
     print("\n")
     print("# SENDING SINGLE DELIVERY REQUEST ################################")
     print(test_graph_vis)
-    request = adpt.type.CPPDeliveryMsg(test_name,
-                                       pickup_name,
-                                       dispenser_name,
-                                       dropoff_name,
-                                       ingestor_name)
+
     dispenser.reset()
     ingestor.reset()
     observer.reset()
     observer.add_task(test_name)
-    adapter.request_delivery(request)
+
+    task_desc = Type.CPPTaskDescriptionMsg()
+    # this is the time when the robot reaches the pick up point
+    task_desc.start_time_sec = int(time.time()) + 50
+    task_desc.delivery = Type.CPPDeliveryMsg(pickup_name,
+                                             dispenser_name,
+                                             dropoff_name,
+                                             ingestor_name)
+    task_profile = Type.CPPTaskProfileMsg()
+    task_profile.description = task_desc
+    task_profile.task_id = test_name
+    adapter.dispatch_task(task_profile)
+
     rclpy_executor.spin_once(1)
 
     for i in range(1000):

rmf_fleet_adapter_python/scripts/test_loop.py

@@ -1,12 +1,15 @@
 import rclpy
+import time
 from rclpy.executors import SingleThreadedExecutor
 from rclpy.node import Node
 
 import rmf_adapter as adpt
 import rmf_adapter.vehicletraits as traits
 import rmf_adapter.geometry as geometry
 import rmf_adapter.graph as graph
+import rmf_adapter.battery as battery
 import rmf_adapter.plan as plan
+import rmf_adapter.type as Type
 
 from rmf_fleet_adapter_python.test_utils import MockRobotCommand
 from rmf_fleet_adapter_python.test_utils import MockDispenser, MockIngestor
@@ -15,7 +18,7 @@
 from functools import partial
 
 
-test_name = 'test_loop'
+test_name = 'test_loop'  # aka task_id
 map_name = "test_map"
 fleet_name = "test_fleet"
 
@@ -108,10 +111,35 @@ def main():
                                         angular=traits.Limits(1.0, 0.45),
                                         profile=profile)
 
-    # Manages delivery or loop requests
-    adapter = adpt.MockAdapter("TestDeliveryAdapter")
+    # Manages loop requests
+    adapter = adpt.MockAdapter("TestLoopAdapter")
     fleet = adapter.add_fleet(fleet_name, robot_traits, test_graph)
-    fleet.accept_delivery_requests(lambda x: True)
+
+    # Set up task request callback function
+    # we will only accept Loop task here
+    def task_request_cb(task_profile):
+        from rmf_task_msgs.msg import TaskType	
+        if(task_profile.description.task_type == TaskType.TYPE_LOOP):
+            return True
+        else:
+            return False
+
+    fleet.accept_task_requests(task_request_cb)
+
+    # Set fleet battery profile
+    battery_sys = battery.BatterySystem.make(24.0, 40.0, 8.8)
+    mech_sys = battery.MechanicalSystem.make(70.0, 40.0, 0.22)
+    motion_sink = battery.SimpleMotionPowerSink(battery_sys, mech_sys)
+    ambient_power_sys = battery.PowerSystem.make(20.0)
+    ambient_sink = battery.SimpleDevicePowerSink(battery_sys, ambient_power_sys)
+    tool_power_sys = battery.PowerSystem.make(10.0)
+    tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys)
+
+    fleet.set_recharge_threshold(0.2)
+    b_success = fleet.set_task_planner_params(
+        battery_sys, motion_sink, ambient_sink, tool_sink)
+
+    assert b_success, "set battery param failed"
 
     cmd_node = Node("RobotCommandHandle")
 
@@ -130,6 +158,7 @@ def main():
 
     # Lambda to insert an adapter
     def updater_inserter(handle_obj, updater):
+        updater.update_battery_soc(1.0)
         handle_obj.updater = updater
 
     # Manages and executes robot commands
@@ -142,6 +171,7 @@ def updater_inserter(handle_obj, updater):
                     partial(updater_inserter, robot_cmd))
 
     # INIT TASK SUMMARY OBSERVER ==============================================
+    # Note: this is used for assertation check on TaskSummary.Msg
     observer = TaskSummaryObserver()
 
     # FINAL PREP ==============================================================
@@ -155,14 +185,22 @@ def updater_inserter(handle_obj, updater):
     print("\n")
     print("# SENDING SINGLE LOOP REQUEST ####################################")
     print(test_graph_vis)
-    request = adpt.type.CPPLoopMsg(test_name,
-                                   fleet_name,
-                                   loop_count,
-                                   start_name,
-                                   finish_name)
+
     observer.reset()
     observer.add_task(test_name)
-    adapter.request_loop(request)
+
+    # Create a task to dispatch
+    task_desc = Type.CPPTaskDescriptionMsg()
+    # this is the time when the robot reaches the start waypoint for loop
+    task_desc.start_time_sec = int(time.time()) + 50
+    task_desc.loop = adpt.type.CPPLoopMsg(fleet_name,
+                                          loop_count,
+                                          start_name,
+                                          finish_name)
+    task_profile = Type.CPPTaskProfileMsg()
+    task_profile.description = task_desc
+    task_profile.task_id = test_name
+    adapter.dispatch_task(task_profile)
 
     for i in range(1000):
         if observer.all_tasks_complete():

rmf_fleet_adapter_python/scripts/test_utils.py

@@ -348,7 +348,7 @@ def _timer_cb(self,
             print("[RobotUpdateHandle] UPDATING ROBOT POSITION:", 
                   previous_waypoint_graph_idx)
 
-            self.updater.update_position(
+            self.updater.update_current_waypoint(
                 previous_waypoint_graph_idx,
                 previous_waypoint.position[2]
             )