From 34c800297359381a8942f38ad57c60e27aaa6cc0 Mon Sep 17 00:00:00 2001
From: carlychen <carly.chen@analog.com>
Date: Mon, 1 Apr 2024 16:55:39 -0400
Subject: [PATCH] added methods in fleet adapter and RobotCommandHandle to read
 and use parking spot for each robot (assuming spawn place is dedicated
 parking spot)

---
 .../rmf_demos_fleet_adapter/RobotCommandHandle.py        | 6 ++++++
 .../rmf_demos_fleet_adapter/fleet_adapter.py             | 9 +++++++++
 2 files changed, 15 insertions(+)

diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py
index 1dde2f43..507ff18c 100644
--- 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
index 5c1bdaa3..8df1a129 100644
--- 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,