added methods in fleet adapter and RobotCommandHandle to read and use…

… parking spot for each robot (assuming spawn place is dedicated parking spot)
open-rmf · Apr 1, 2024 · 34c8002 · 34c8002
1 parent 34d251e
commit 34c8002
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 0 deletions.
diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py
@@ -69,6 +69,7 @@ def __init__(self,
                  start,
                  position,
                  charger_waypoint,
+                 parking_waypoint,
                  update_frequency,
                  lane_merge_distance,
                  adapter,
@@ -87,6 +88,11 @@ def __init__(self,
         assert waypoint, f"Charger waypoint {charger_waypoint} \
           does not exist in the navigation graph"
         self.charger_waypoint_index = waypoint.index
+        # Get the index of the parking spot waypoint
+        waypoint = self.graph.find_waypoint(parking_waypoint)
+        assert waypoint, f"Parking waypoint {parking_waypoint} \
+          does not exist in the navigation graph"
+        self.parking_waypoint_index = waypoint.index
         self.update_frequency = update_frequency
         self.lane_merge_distance = lane_merge_distance
         self.update_handle = None  # RobotUpdateHandle

diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py
@@ -195,6 +195,14 @@ def _action_executor(category: str,
             cmd_handle.node.get_logger().warn(
                 "Invalid waypoint supplied for charger. "
                 "Using default nearest charger in the map")
+        if (cmd_handle.parking_waypoint_index <
+                cmd_handle.graph.num_waypoints):
+            cmd_handle.update_handle.set_parking_waypoint(
+                cmd_handle.parking_waypoint_index)
+        else:
+            cmd_handle.node.get_logger().warn(
+                "Invalid waypoint supplied for parking spot. "
+                "Using default nearest parking spot in the map")
 
     # Initialize robot API for this fleet
     prefix = 'http://' + fleet_config['fleet_manager']['ip'] + \
@@ -277,6 +285,7 @@ def _add_fleet_robots():
                         start=starts[0],
                         position=position,
                         charger_waypoint=rmf_config['charger']['waypoint'],
+                        parking_waypoint=rmf_config['start']['waypoint'],
                         update_frequency=rmf_config.get(
                             'robot_state_update_frequency', 1),
                         lane_merge_distance=lane_merge_distance,