Add proto-reservation node as core part of RMF (#212)

Signed-off-by: Arjo Chakravarty <[email protected]>

rmf_demos/config/office/tinyRobot_config.yaml

@@ -30,13 +30,13 @@ rmf_fleet:
     loop: True
     delivery: True
   actions: ["teleop"]
-  finishing_request: "park" # [park, charge, nothing]
-  responsive_wait: True # Should responsive wait be on/off for the whole fleet by default? False if not specified.
+  finishing_request: "charge" # [park, charge, nothing]
+  responsive_wait: False # Should responsive wait be on/off for the whole fleet by default? False if not specified.
+  use_parking_reservations: True
   reassign_task_interval: 120 # seconds, specify how often a task reassignment should be triggered in the fleet
   robots:
     tinyRobot1:
         charger: "tinyRobot1_charger"
-        responsive_wait: False # Should responsive wait be on/off for this specific robot? Overrides the fleet-wide setting.
     tinyRobot2:
         charger: "tinyRobot2_charger"
         # No mention of responsive_wait means the fleet-wide setting will be used

rmf_demos/launch/common.launch.xml

@@ -10,6 +10,7 @@
   <arg name="bidding_time_window" description="Time window in seconds for task bidding process" default="2.0"/>
   <arg name="use_unique_hex_string_with_task_id" default="true" description="Appends a unique hex string to the task ID"/>
   <arg name="server_uri" default="" description="The URI of the api server to transmit state and task information."/>
+  <arg name="use_reservation_node" default="false" description="Enable the reservation node for parking spot de-confliction"/>
 
   <!-- Traffic Schedule  -->
   <node pkg="rmf_traffic_ros2" exec="rmf_traffic_schedule" output="both" name="rmf_traffic_schedule_primary">
@@ -61,4 +62,10 @@
       <param name="server_uri" value="$(var server_uri)"/>
     </node>
   </group>
+
+  <!-- Proto-Reservation system-->
+  <group  if="$(var use_reservation_node)">
+    <node pkg="rmf_reservation_node" exec="queue_manager"></node>
+  </group>
+
 </launch>

rmf_demos/launch/office.launch.xml

@@ -8,6 +8,7 @@
     <arg name="use_sim_time" value="$(var use_sim_time)"/>
     <arg name="viz_config_file" value ="$(find-pkg-share rmf_demos)/include/office/office.rviz"/>
     <arg name="config_file" value="$(find-pkg-share rmf_demos_maps)/office/office.building.yaml"/>
+    <arg name="use_reservation_node" value="true"/>
   </include>
 
   <!-- TinyRobot fleet adapter -->

rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py

@@ -14,6 +14,7 @@
 
 import argparse
 import asyncio
+import faulthandler
 import math
 import sys
 import threading
@@ -46,6 +47,7 @@
 # Main
 # ------------------------------------------------------------------------------
 def main(argv=sys.argv):
+    faulthandler.enable()
     # Init rclpy and adapter
     rclpy.init(args=argv)
     rmf_adapter.init_rclcpp()

rmf_demos_gz/launch/office_reservation_node.launch.xml

@@ -0,0 +1,20 @@
+<?xml version='1.0' ?>
+
+<launch>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="gazebo_version" default='8'/>
+  <arg name="use_traffic_light" default="false"/>
+
+  <!-- Common launch -->
+  <include file="$(find-pkg-share rmf_demos)/office_reservation_node.launch.xml">
+    <arg name="use_traffic_light" value="$(var use_traffic_light)"/>
+    <arg name="use_sim_time" value="$(var use_sim_time)"/>
+  </include>
+
+  <!-- Simulation launch -->
+  <include file="$(find-pkg-share rmf_demos_gz)/simulation.launch.xml">
+    <arg name="map_name" value="office" />
+    <arg name="gazebo_version" value="$(var gazebo_version)" />
+  </include>
+
+</launch>

rmf_demos_tasks/README.md

@@ -105,6 +105,12 @@ The new task system allows users to construct and submit their own tasks in a mo
   ros2 run rmf_demos_tasks  dispatch_go_to_place -p lounge -o 105 --use_sim_time
   ```
 
+- **wait_for_task_complete**
+  This is a tool meant only for creating test scenarios. Essentially it blocks until a specified robot is free of any tasks. This allows us to do integration tests. Example in office world:
+  ```
+  ros2 run rmf_demos_tasks wait_for_task_complete -F tinyRobot -R tinyRobot1
+  ```
+
 ## Quality Declaration
 
 This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](./QUALITY_DECLARATION.md) for more details.

rmf_demos_tasks/rmf_demos_tasks/__init__.py

@@ -0,0 +1,14 @@
+# Copyright 2024 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.
+"""Scripts for issuing tasks to Open-RMF from the command line."""

rmf_demos_tasks/rmf_demos_tasks/api_request.py

@@ -13,6 +13,7 @@
 # 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.
+"""Send a generic API request."""
 
 import argparse
 import asyncio
@@ -32,8 +33,10 @@
 
 ###############################################################################
 class ApiRequester(Node):
+    """API requester."""
 
     def __init__(self, argv=sys.argv):
+        """Initialize an API requester."""
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()
         parser.add_argument(
@@ -89,14 +92,15 @@ def receive_response(response_msg: ApiResponse):
 
 
 def main(argv=sys.argv):
+    """Send a generic API request."""
     rclpy.init(args=sys.argv)
     args_without_ros = rclpy.utilities.remove_ros_args(sys.argv)
 
     task_requester = ApiRequester(args_without_ros)
     rclpy.spin_until_future_complete(
         task_requester, task_requester.response, timeout_sec=5.0)
     if task_requester.response.done():
-        print(f'Got response:\n{task_requester.response.result()}')
+        print(f'Got response: \n{task_requester.response.result()}')
     else:
         print('Did not get a response')
     rclpy.shutdown()

rmf_demos_tasks/rmf_demos_tasks/cancel_task.py

@@ -13,6 +13,7 @@
 # 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.
+"""Cancel a task."""
 
 import argparse
 import json
@@ -25,14 +26,17 @@
 from rclpy.qos import QoSHistoryPolicy as History
 from rclpy.qos import QoSProfile
 from rclpy.qos import QoSReliabilityPolicy as Reliability
+
 from rmf_task_msgs.msg import ApiRequest
 
 ###############################################################################
 
 
 class TaskRequester(Node):
+    """Task requester."""
 
     def __init__(self, argv=sys.argv):
+        """Initialize a task requester."""
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()
         parser.add_argument(
@@ -73,6 +77,7 @@ def __init__(self, argv=sys.argv):
 
 
 def main(argv=sys.argv):
+    """Cancel a task."""
     rclpy.init(args=sys.argv)
     args_without_ros = rclpy.utilities.remove_ros_args(sys.argv)
 

rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py

@@ -13,6 +13,7 @@
 # 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.
+"""Dispatch an action."""
 
 import argparse
 import asyncio
@@ -27,15 +28,18 @@
 from rclpy.qos import QoSHistoryPolicy as History
 from rclpy.qos import QoSProfile
 from rclpy.qos import QoSReliabilityPolicy as Reliability
+
 from rmf_task_msgs.msg import ApiRequest
 from rmf_task_msgs.msg import ApiResponse
 
 ###############################################################################
 
 
 class TaskRequester(Node):
+    """Task requester."""
 
     def __init__(self, argv=sys.argv):
+        """Initialize a task requester."""
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()
         parser.add_argument(
@@ -210,6 +214,7 @@ def receive_response(response_msg: ApiResponse):
 
 
 def main(argv=sys.argv):
+    """Dispatch an action."""
     rclpy.init(args=sys.argv)
     args_without_ros = rclpy.utilities.remove_ros_args(sys.argv)
 
@@ -218,7 +223,7 @@ def main(argv=sys.argv):
         task_requester, task_requester.response, timeout_sec=5.0
     )
     if task_requester.response.done():
-        print(f'Got response:\n{task_requester.response.result()}')
+        print(f'Got response: \n{task_requester.response.result()}')
     else:
         print('Did not get a response')
     rclpy.shutdown()

rmf_demos_tasks/rmf_demos_tasks/dispatch_cart_delivery.py

@@ -13,6 +13,7 @@
 # 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.
+"""Dispatch a cart delivery."""
 
 import argparse
 import asyncio
@@ -34,8 +35,10 @@
 ###############################################################################
 
 class TaskRequester(Node):
+    """Task requester."""
 
     def __init__(self, argv=sys.argv):
+        """Initialize a task requester."""
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()
         parser.add_argument('-p', '--pickup', required=True,
@@ -159,14 +162,15 @@ def receive_response(response_msg: ApiResponse):
 
 
 def main(argv=sys.argv):
+    """Dispatch a cart delivery."""
     rclpy.init(args=sys.argv)
     args_without_ros = rclpy.utilities.remove_ros_args(sys.argv)
 
     task_requester = TaskRequester(args_without_ros)
     rclpy.spin_until_future_complete(
         task_requester, task_requester.response, timeout_sec=5.0)
     if task_requester.response.done():
-        print(f'Got response:\n{task_requester.response.result()}')
+        print(f'Got response: \n{task_requester.response.result()}')
     else:
         print('Did not get a response')
     rclpy.shutdown()

rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py

@@ -13,6 +13,7 @@
 # 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.
+"""Dispatch a cleaning."""
 
 import argparse
 import asyncio
@@ -27,15 +28,18 @@
 from rclpy.qos import QoSHistoryPolicy as History
 from rclpy.qos import QoSProfile
 from rclpy.qos import QoSReliabilityPolicy as Reliability
+
 from rmf_task_msgs.msg import ApiRequest
 from rmf_task_msgs.msg import ApiResponse
 
 ###############################################################################
 
 
 class TaskRequester(Node):
+    """Task requester."""
 
     def __init__(self, argv=sys.argv):
+        """Initialize a task requester."""
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()
         parser.add_argument(
@@ -185,6 +189,7 @@ def receive_response(response_msg: ApiResponse):
 
 
 def main(argv=sys.argv):
+    """Dispatch a cleaning."""
     rclpy.init(args=sys.argv)
     args_without_ros = rclpy.utilities.remove_ros_args(sys.argv)
 
@@ -193,7 +198,7 @@ def main(argv=sys.argv):
         task_requester, task_requester.response, timeout_sec=5.0
     )
     if task_requester.response.done():
-        print(f'Got response:\n{task_requester.response.result()}')
+        print(f'Got response: \n{task_requester.response.result()}')
     else:
         print('Did not get a response')
     rclpy.shutdown()

rmf_demos_tasks/rmf_demos_tasks/dispatch_delivery.py

@@ -13,6 +13,7 @@
 # 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.
+"""Dispatch a delivery."""
 
 import argparse
 import asyncio
@@ -27,15 +28,18 @@
 from rclpy.qos import QoSHistoryPolicy as History
 from rclpy.qos import QoSProfile
 from rclpy.qos import QoSReliabilityPolicy as Reliability
+
 from rmf_task_msgs.msg import ApiRequest
 from rmf_task_msgs.msg import ApiResponse
 
 ###############################################################################
 
 
 class TaskRequester(Node):
+    """Task requester."""
 
     def __init__(self, argv=sys.argv):
+        """Initialize a task requester."""
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()
         parser.add_argument(
@@ -278,6 +282,7 @@ def receive_response(response_msg: ApiResponse):
 
 
 def main(argv=sys.argv):
+    """Dispatch a delivery."""
     rclpy.init(args=sys.argv)
     args_without_ros = rclpy.utilities.remove_ros_args(sys.argv)
 
@@ -286,7 +291,7 @@ def main(argv=sys.argv):
         task_requester, task_requester.response, timeout_sec=5.0
     )
     if task_requester.response.done():
-        print(f'Got response:\n{task_requester.response.result()}')
+        print(f'Got response: \n{task_requester.response.result()}')
     else:
         print('Did not get a response')
     rclpy.shutdown()

rmf_demos_tasks/rmf_demos_tasks/dispatch_go_to_place.py

@@ -13,6 +13,7 @@
 # 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.
+"""Go to place."""
 
 import argparse
 import asyncio
@@ -28,15 +29,18 @@
 from rclpy.qos import QoSHistoryPolicy as History
 from rclpy.qos import QoSProfile
 from rclpy.qos import QoSReliabilityPolicy as Reliability
+
 from rmf_task_msgs.msg import ApiRequest
 from rmf_task_msgs.msg import ApiResponse
 
 ###############################################################################
 
 
 class TaskRequester(Node):
+    """Task requester."""
 
     def __init__(self, argv=sys.argv):
+        """Initialize a task requester."""
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()
         parser.add_argument('-F', '--fleet', type=str, help='Fleet name')
@@ -189,6 +193,7 @@ def receive_response(response_msg: ApiResponse):
 
 
 def main(argv=sys.argv):
+    """Go to place."""
     rclpy.init(args=sys.argv)
     args_without_ros = rclpy.utilities.remove_ros_args(sys.argv)
 
@@ -197,7 +202,7 @@ def main(argv=sys.argv):
         task_requester, task_requester.response, timeout_sec=5.0
     )
     if task_requester.response.done():
-        print(f'Got response:\n{task_requester.response.result()}')
+        print(f'Got response: \n{task_requester.response.result()}')
     else:
         print('Did not get a response')
     rclpy.shutdown()