Add Speed Limit Requests (#152)

* support SpeedLimitRequests Signed-off-by: Pranay Shirodkar <[email protected]> * Rearrange imports Signed-off-by: Xiyu Oh <[email protected]> --------- Signed-off-by: Pranay Shirodkar <[email protected]> Signed-off-by: Xiyu <[email protected]> Signed-off-by: Xiyu Oh <[email protected]> Co-authored-by: Grey <[email protected]> Co-authored-by: Xiyu <[email protected]>
open-rmf · Feb 17, 2025 · 41d10f1 · 41d10f1
1 parent 989135c
commit 41d10f1
Showing 1 changed file with 21 additions and 0 deletions.
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
@@ -36,6 +36,7 @@
 from rmf_fleet_msgs.msg import LaneRequest
 from rmf_fleet_msgs.msg import ModeRequest
 from rmf_fleet_msgs.msg import RobotMode
+from rmf_fleet_msgs.msg import SpeedLimitRequest
 import yaml
 
 from .RobotClientAPI import RobotAPI
@@ -464,6 +465,18 @@ def lane_request_cb(msg):
         state_msg.closed_lanes = list(closed_lanes)
         closed_lanes_pub.publish(state_msg)
 
+    def speed_limit_request_cb(msg):
+        if msg.fleet_name is None or msg.fleet_name != fleet_name:
+            return
+
+        requests = []
+        for limit in msg.speed_limits:
+            request = rmf_adapter.fleet_update_handle.SpeedLimitRequest(
+                limit.lane_index, limit.speed_limit)
+            requests.append(request)
+        fleet_handle.more().limit_lane_speeds(requests)
+        fleet_handle.more().remove_speed_limits(msg.remove_limits)
+
     def mode_request_cb(msg):
         if (
             msg.fleet_name is None
@@ -485,6 +498,13 @@ def mode_request_cb(msg):
         qos_profile=qos_profile_system_default,
     )
 
+    speed_limit_request_sub = node.create_subscription(
+        SpeedLimitRequest,
+        'speed_limit_requests',
+        speed_limit_request_cb,
+        qos_profile=qos_profile_system_default,
+    )
+
     action_execution_notice_sub = node.create_subscription(
         ModeRequest,
         'action_execution_notice',
@@ -494,6 +514,7 @@ def mode_request_cb(msg):
 
     return [
         lane_request_sub,
+        speed_limit_request_sub,
         action_execution_notice_sub,
     ]