Add API to update speed limits for lanes and publish LaneStates message

rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp

@@ -60,6 +60,8 @@ const std::string DockSummaryTopicName = "dock_summary";
 const std::string NavGraphTopicName = "nav_graphs";
 const std::string LaneClosureRequestTopicName = "lane_closure_requests";
 const std::string ClosedLaneTopicName = "closed_lanes";
+const std::string SpeedLimitRequestTopicName = "speed_limit_requests";
+const std::string LaneStatesTopicName = "lane_states";
 
 const std::string InterruptRequestTopicName = "robot_interrupt_request";
 

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp

@@ -206,6 +206,39 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   /// Specify a set of lanes that should be open.
   void open_lanes(std::vector<std::size_t> lane_indices);
 
+  /// A class used to describe speed limit imposed on lanes.
+  class SpeedLimitRequest
+  {
+  public:
+    /// Constructor
+    ///
+    /// \param[in] lane_index
+    ///   The index of the lane to impose a speed limit upon.
+    ///
+    /// \param[in] speed_limit
+    ///   The speed limit to be imposed for this lane.
+    SpeedLimitRequest(
+      std::size_t lane_index,
+      double speed_limit);
+
+    /// Get the lane_index
+    std::size_t lane_index() const;
+
+    /// Get the speed_limit
+    double speed_limit() const;
+
+    class Implementation;
+
+  private:
+    rmf_utils::impl_ptr<Implementation> _pimpl;
+  };
+
+  /// Impose speed limits on specified lanes.
+  void limit_lane_speeds(std::vector<SpeedLimitRequest> requests);
+
+  /// Remove speed limits from specified lanes.
+  void remove_speed_limits(std::vector<std::size_t> requests);
+
   /// Set the parameters required for task planning. Without calling this
   /// function, this fleet will not bid for and accept tasks.
   ///

rmf_fleet_adapter/src/full_control/main.cpp

@@ -33,6 +33,7 @@
 #include <rmf_fleet_msgs/srv/lift_clearance.hpp>
 #include <rmf_fleet_msgs/msg/lane_request.hpp>
 #include <rmf_fleet_msgs/msg/closed_lanes.hpp>
+#include <rmf_fleet_msgs/msg/speed_limit_request.hpp>
 #include <rmf_fleet_msgs/msg/interrupt_request.hpp>
 
 // RMF Task messages
@@ -775,6 +776,10 @@ struct Connections : public std::enable_shared_from_this<Connections>
   rclcpp::Publisher<rmf_fleet_msgs::msg::ClosedLanes>::SharedPtr
     closed_lanes_pub;
 
+  /// The topic subscription for listening for speed limit requests
+  rclcpp::Subscription<rmf_fleet_msgs::msg::SpeedLimitRequest>::SharedPtr
+    speed_limit_request_sub;
+
   /// Container for remembering which lanes are currently closed
   std::unordered_set<std::size_t> closed_lanes;
 
@@ -1030,8 +1035,8 @@ std::shared_ptr<Connections> make_fleet(
       if (!connections)
         return;
 
-      if (request_msg->fleet_name != fleet_name &&
-      !request_msg->fleet_name.empty())
+      if (request_msg->fleet_name != fleet_name ||
+      request_msg->fleet_name.empty())
         return;
 
       connections->fleet->open_lanes(request_msg->open_lanes);
@@ -1062,6 +1067,35 @@ std::shared_ptr<Connections> make_fleet(
       connections->closed_lanes_pub->publish(state_msg);
     });
 
+  connections->speed_limit_request_sub =
+    adapter->node()->create_subscription<
+    rmf_fleet_msgs::msg::SpeedLimitRequest>(
+    rmf_fleet_adapter::SpeedLimitRequestTopicName,
+    rclcpp::SystemDefaultsQoS(),
+    [w = connections->weak_from_this(), fleet_name](
+      rmf_fleet_msgs::msg::SpeedLimitRequest::ConstSharedPtr request_msg)
+    {
+      const auto connections = w.lock();
+      if (!connections)
+        return;
+
+      if (request_msg->fleet_name != fleet_name ||
+      request_msg->fleet_name.empty())
+        return;
+
+      std::vector<rmf_fleet_adapter::agv::FleetUpdateHandle::SpeedLimitRequest>
+      requests;
+      for (const auto& limit : request_msg->speed_limits)
+      {
+        auto request =
+        rmf_fleet_adapter::agv::FleetUpdateHandle::SpeedLimitRequest(
+          limit.lane_index, limit.speed_limit);
+        requests.push_back(std::move(request));
+      }
+      connections->fleet->limit_lane_speeds(requests);
+      connections->fleet->remove_speed_limits(request_msg->remove_limits);
+    });
+
   connections->interrupt_request_sub =
     adapter->node()->create_subscription<rmf_fleet_msgs::msg::InterruptRequest>(
     rmf_fleet_adapter::InterruptRequestTopicName,

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -18,6 +18,7 @@
 #include <rmf_fleet_msgs/msg/robot_state.hpp>
 #include <rmf_fleet_msgs/msg/robot_mode.hpp>
 #include <rmf_fleet_msgs/msg/location.hpp>
+#include <rmf_fleet_msgs/msg/speed_limited_lane.hpp>
 
 #include <rmf_traffic_ros2/Time.hpp>
 #include <rmf_traffic_ros2/agv/Graph.hpp>
@@ -1539,13 +1540,18 @@ void FleetUpdateHandle::close_lanes(std::vector<std::size_t> lane_indices)
       auto new_config = (*self->_pimpl->planner)->get_configuration();
       auto& new_lane_closures = new_config.lane_closures();
       for (const auto& lane : lane_indices)
+      {
         new_lane_closures.close(lane);
+        // Bookkeeping
+        self->_pimpl->closed_lanes.insert(lane);
+      }
 
       *self->_pimpl->planner =
       std::make_shared<const rmf_traffic::agv::Planner>(
         new_config, rmf_traffic::agv::Planner::Options(nullptr));
 
       self->_pimpl->task_parameters->planner(*self->_pimpl->planner);
+      self->_pimpl->publish_lane_states();
     });
 }
 
@@ -1581,16 +1587,157 @@ void FleetUpdateHandle::open_lanes(std::vector<std::size_t> lane_indices)
       auto new_config = (*self->_pimpl->planner)->get_configuration();
       auto& new_lane_closures = new_config.lane_closures();
       for (const auto& lane : lane_indices)
+      {
         new_lane_closures.open(lane);
+        // Bookkeeping
+        self->_pimpl->closed_lanes.erase(lane);
+      }
 
       *self->_pimpl->planner =
       std::make_shared<const rmf_traffic::agv::Planner>(
         new_config, rmf_traffic::agv::Planner::Options(nullptr));
 
       self->_pimpl->task_parameters->planner(*self->_pimpl->planner);
+      self->_pimpl->publish_lane_states();
     });
 }
 
+//==============================================================================
+class FleetUpdateHandle::SpeedLimitRequest::Implementation
+{
+public:
+  std::size_t lane_index;
+  double speed_limit;
+};
+
+//==============================================================================
+FleetUpdateHandle::SpeedLimitRequest::SpeedLimitRequest(
+  std::size_t lane_index,
+  double speed_limit)
+: _pimpl(rmf_utils::make_impl<Implementation>(Implementation{
+      std::move(lane_index),
+      std::move(speed_limit)
+    }))
+{
+  // Do nothing
+}
+
+//==============================================================================
+std::size_t FleetUpdateHandle::SpeedLimitRequest::lane_index() const
+{
+  return _pimpl->lane_index;
+}
+
+//==============================================================================
+double FleetUpdateHandle::SpeedLimitRequest::speed_limit() const
+{
+  return _pimpl->speed_limit;
+}
+
+//==============================================================================
+auto FleetUpdateHandle::limit_lane_speeds(
+  std::vector<SpeedLimitRequest> requests) -> void
+{
+  _pimpl->worker.schedule(
+    [w = weak_from_this(), requests = std::move(requests)](const auto&)
+    {
+      if (requests.empty())
+        return;
+      const auto self = w.lock();
+      if (!self)
+        return;
+
+      auto new_config = (*self->_pimpl->planner)->get_configuration();
+      auto& new_graph = new_config.graph();
+      for (const auto& request : requests)
+      {
+        // TODO: Check if planner supports negative speed limits.
+        if (request.lane_index() >= new_graph.num_lanes() ||
+        request.speed_limit() <= 0.0)
+        {
+          RCLCPP_WARN(
+            self->_pimpl->node->get_logger(),
+            "Ignoring speed limit request %f for lane %d in fleet %s as it is "
+            "not greater than zero. If you would like to close the lane, use "
+            "the FleetUpdateHandle::close_lanes(~) API instead.",
+            request.speed_limit(),
+            request.lane_index(),
+            self->_pimpl->name.c_str()
+          );
+          continue;
+        }
+        auto& properties = new_graph.get_lane(
+          request.lane_index()).properties();
+        properties.speed_limit(request.speed_limit());
+        // Bookkeeping
+        self->_pimpl->speed_limited_lanes[request.lane_index()] =
+        request.speed_limit();
+      }
+
+      *self->_pimpl->planner =
+      std::make_shared<const rmf_traffic::agv::Planner>(
+        new_config, rmf_traffic::agv::Planner::Options(nullptr));
+
+      self->_pimpl->task_parameters->planner(*self->_pimpl->planner);
+      self->_pimpl->publish_lane_states();
+    });
+}
+
+//==============================================================================
+void FleetUpdateHandle::remove_speed_limits(std::vector<std::size_t> requests)
+{
+  _pimpl->worker.schedule(
+    [w = weak_from_this(), requests = std::move(requests)](const auto&)
+    {
+      if (requests.empty())
+        return;
+      const auto self = w.lock();
+      if (!self)
+        return;
+
+      auto new_config = (*self->_pimpl->planner)->get_configuration();
+      auto& new_graph = new_config.graph();
+      for (const auto& request : requests)
+      {
+
+        if (auto it = self->_pimpl->speed_limited_lanes.find(request) ==
+        self->_pimpl->speed_limited_lanes.end())
+          continue;
+        auto& properties = new_graph.get_lane(
+          request).properties();
+        properties.speed_limit(std::nullopt);
+        // Bookkeeping
+        self->_pimpl->speed_limited_lanes.erase(request);
+      }
+
+      *self->_pimpl->planner =
+      std::make_shared<const rmf_traffic::agv::Planner>(
+        new_config, rmf_traffic::agv::Planner::Options(nullptr));
+
+      self->_pimpl->task_parameters->planner(*self->_pimpl->planner);
+      self->_pimpl->publish_lane_states();
+    });
+}
+
+//==============================================================================
+void FleetUpdateHandle::Implementation::publish_lane_states() const
+{
+  if (lane_states_pub == nullptr)
+    return;
+  auto msg = std::make_unique<LaneStates>();
+  msg->fleet_name = name;
+  for (const auto& index : closed_lanes)
+    msg->closed_lanes.push_back(index);
+  for (const auto& [index, limit] : speed_limited_lanes)
+  {
+    // TODO(YV): Use type_adapter in Humble to avoid this copy
+    msg->speed_limits.push_back(
+      rmf_fleet_msgs::build<rmf_fleet_msgs::msg::SpeedLimitedLane>()
+      .lane_index(index)
+      .speed_limit(limit));
+  }
+  lane_states_pub->publish(std::move(msg));
+}
 //==============================================================================
 FleetUpdateHandle& FleetUpdateHandle::accept_task_requests(
   AcceptTaskRequest check)

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp

@@ -38,6 +38,7 @@
 #include <rmf_building_map_msgs/msg/graph.hpp>
 
 #include <rmf_fleet_msgs/msg/dock_summary.hpp>
+#include <rmf_fleet_msgs/msg/lane_states.hpp>
 
 #include <rmf_fleet_adapter/agv/FleetUpdateHandle.hpp>
 #include <rmf_fleet_adapter/StandardNames.hpp>
@@ -319,6 +320,11 @@ class FleetUpdateHandle::Implementation
 
   mutable rmf_task::Log::Reader log_reader = {};
 
+  using LaneStates = rmf_fleet_msgs::msg::LaneStates;
+  rclcpp::Publisher<LaneStates>::SharedPtr lane_states_pub = nullptr;
+  std::unordered_map<std::size_t, double> speed_limited_lanes = {};
+  std::unordered_set<std::size_t> closed_lanes = {};
+
   template<typename... Args>
   static std::shared_ptr<FleetUpdateHandle> make(Args&& ... args)
   {
@@ -404,6 +410,13 @@ class FleetUpdateHandle::Implementation
           self->_pimpl->dock_summary_cb(msg);
       });
 
+    // Publish LaneStates
+    handle->_pimpl->lane_states_pub =
+      handle->_pimpl->node->create_publisher<LaneStates>(
+      LaneStatesTopicName,
+      transient_qos);
+    handle->_pimpl->publish_lane_states();
+
     // Populate charging waypoints
     const auto& graph = (*handle->_pimpl->planner)->get_configuration().graph();
     for (std::size_t i = 0; i < graph.num_waypoints(); ++i)
@@ -571,6 +584,8 @@ class FleetUpdateHandle::Implementation
 
   void publish_fleet_state_topic() const;
 
+  void publish_lane_states() const;
+
   void update_fleet() const;
 
   void update_fleet_state() const;