Add API for enabling/disabling Responsive Wait behavior (#209)

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
Signed-off-by: Luca Della Vedova <luca@openrobotics.org>
Co-authored-by: Luca Della Vedova <luca@openrobotics.org>

rmf_fleet_adapter/CMakeLists.txt

@@ -409,7 +409,7 @@ target_link_libraries(test_read_only_adapter
 # -----------------------------------------------------------------------------
 
 add_executable(dump_fleet_states
-  test/dump_fleet_states
+  test/dump_fleet_states.cpp
 )
 
 target_link_libraries(dump_fleet_states

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp

@@ -316,6 +316,18 @@ class RobotUpdateHandle
   /// Add a log entry with Error severity
   void log_error(std::string text);
 
+  /// Toggle the responsive wait behavior for this robot. When responsive wait
+  /// is active, the robot will remain in the traffic schedule when it is idle
+  /// and will negotiate its position with other traffic participants to
+  /// potentially move out of their way.
+  ///
+  /// Disabling this behavior may be helpful to reduce CPU load or prevent
+  /// parked robots from moving or being seen as conflicts when they are not
+  /// actually at risk of creating traffic conflicts.
+  ///
+  /// By default this behavior is enabled.
+  void enable_responsive_wait(bool value);
+
   class Implementation;
 
   /// This API is experimental and will not be supported in the future. Users

rmf_fleet_adapter/launch/fleet_adapter.launch.xml

@@ -39,6 +39,7 @@
   <arg name="recharge_threshold" default="0.2" description="The fraction of total battery capacity below which the robot must return to its charger"/>
   <arg name="recharge_soc" default="1.0" description="The fraction of total battery capacity to which the robot should be charged"/>
   <arg name="experimental_lift_watchdog_service" default="" description="(Experimental) The name of a service to check whether a robot can enter a lift"/>
+  <arg name="enable_responsive_wait" default="true" description="Should the robot wait responsively"/>
   <!-- The uri is set to the TrajectoryServer uri for debugging purposes. The default port needs to be updated before merging -->
   <arg name="server_uri" default="" description="The URI of the api server to trasnmit state and task infromation. If empty, information will not be transmitted"/>
 
@@ -85,6 +86,7 @@
     <param name="recharge_soc" value="$(var recharge_soc)"/>
 
     <param name="experimental_lift_watchdog_service" value="$(var experimental_lift_watchdog_service)"/>
+    <param name="enable_responsive_wait" value="$(var enable_responsive_wait)"/>
 
     <param name="server_uri" value="$(var server_uri)"/>
 

rmf_fleet_adapter/src/full_control/main.cpp

@@ -508,6 +508,12 @@ class FleetDriverRobotCommandHandle
   void set_updater(rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater)
   {
     _travel_info.updater = std::move(updater);
+
+    bool enable_responsive_wait = true;
+    _node->get_parameter_or(
+      "enable_responsive_wait", enable_responsive_wait, true);
+    _travel_info.updater->enable_responsive_wait(enable_responsive_wait);
+
     // Set the action_executor for the robot
     const auto teleop_executioner =
       [w = weak_from_this()](
@@ -526,6 +532,11 @@ class FleetDriverRobotCommandHandle
     _travel_info.updater->set_action_executor(teleop_executioner);
   }
 
+  rmf_fleet_adapter::agv::RobotUpdateHandlePtr get_updater()
+  {
+    return _travel_info.updater;
+  }
+
   void newly_closed_lanes(const std::unordered_set<std::size_t>& closed_lanes)
   {
     bool need_to_replan = false;
@@ -720,6 +731,10 @@ using FleetDriverRobotCommandHandlePtr =
 /// This is an RAII class that keeps the connections to the fleet driver alive.
 struct Connections : public std::enable_shared_from_this<Connections>
 {
+  /// Parameter change listener
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
+    on_set_param;
+
   /// The API for adding new robots to the adapter
   rmf_fleet_adapter::agv::FleetUpdateHandlePtr fleet;
 
@@ -911,8 +926,37 @@ std::shared_ptr<Connections> make_fleet(
   const rmf_fleet_adapter::agv::AdapterPtr& adapter)
 {
   const auto& node = adapter->node();
+  node->declare_parameter("enable_responsive_wait", true);
+
   std::shared_ptr<Connections> connections = std::make_shared<Connections>();
   connections->adapter = adapter;
+  connections->on_set_param =
+    node->get_node_parameters_interface()->add_on_set_parameters_callback(
+    [w = connections->weak_from_this()](
+      const std::vector<rclcpp::Parameter>& params)
+    -> rcl_interfaces::msg::SetParametersResult
+    {
+      const auto self = w.lock();
+      if (!self)
+        return rcl_interfaces::msg::SetParametersResult();
+
+      for (const auto& p : params)
+      {
+        if (p.get_name() == "enable_responsive_wait")
+        {
+          const auto value = p.as_bool();
+          for (auto& [_, cmd] : self->robots)
+          {
+            if (const auto updater = cmd->get_updater())
+              updater->enable_responsive_wait(value);
+          }
+        }
+      }
+
+      rcl_interfaces::msg::SetParametersResult r;
+      r.successful = true;
+      return r;
+    });
 
   const std::string fleet_name_param_name = "fleet_name";
   const std::string fleet_name = node->declare_parameter(

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -868,6 +868,29 @@ const
   return _broadcast_client;
 }
 
+//==============================================================================
+void TaskManager::enable_responsive_wait(bool value)
+{
+  if (_responsive_wait_enabled == value)
+    return;
+
+  _responsive_wait_enabled = value;
+  if (!_responsive_wait_enabled && static_cast<bool>(_waiting))
+  {
+    _waiting.cancel({"Responsive Wait Disabled"}, _context->now());
+    return;
+  }
+
+  if (_responsive_wait_enabled)
+  {
+    std::lock_guard<std::mutex> guard(_mutex);
+    if (!_active_task && _queue.empty() && _direct_queue.empty() && !_waiting)
+    {
+      _begin_waiting();
+    }
+  }
+}
+
 //==============================================================================
 void TaskManager::set_queue(
   const std::vector<TaskManager::Assignment>& assignments)
@@ -1388,6 +1411,9 @@ std::function<void()> TaskManager::_robot_interruption_callback()
 //==============================================================================
 void TaskManager::_begin_waiting()
 {
+  if (!_responsive_wait_enabled)
+    return;
+
   // Determine the waypoint closest to the robot
   std::size_t waiting_point = _context->location().front().waypoint();
   double min_dist = std::numeric_limits<double>::max();

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp

@@ -109,6 +109,8 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   std::optional<std::weak_ptr<rmf_websocket::BroadcastClient>> broadcast_client()
   const;
 
+  void enable_responsive_wait(bool value);
+
   /// Set the queue for this task manager with assignments generated from the
   /// task planner
   void set_queue(const std::vector<Assignment>& assignments);
@@ -306,6 +308,7 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   std::weak_ptr<agv::FleetUpdateHandle> _fleet_handle;
   rmf_task::ConstActivatorPtr _task_activator;
   ActiveTask _active_task;
+  bool _responsive_wait_enabled = true;
   bool _emergency_active = false;
   std::optional<std::string> _emergency_pullover_interrupt_token;
   ActiveTask _emergency_pullover;

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -1383,6 +1383,19 @@ void FleetUpdateHandle::add_robot(
             context->name().c_str(),
             context->itinerary().id());
 
+          std::optional<std::weak_ptr<rmf_websocket::BroadcastClient>>
+          broadcast_client = std::nullopt;
+
+          if (fleet->_pimpl->broadcast_client)
+            broadcast_client = fleet->_pimpl->broadcast_client;
+
+          fleet->_pimpl->task_managers.insert({context,
+            TaskManager::make(
+              context,
+              broadcast_client,
+              std::weak_ptr<FleetUpdateHandle>(fleet))});
+
+          // -- Calling the handle_cb should always happen last --
           if (handle_cb)
           {
             handle_cb(RobotUpdateHandle::Implementation::make(std::move(context)));
@@ -1395,20 +1408,7 @@ void FleetUpdateHandle::add_robot(
               "receive the RobotUpdateHandle of the new robot. This means you will "
               "not be able to update the state of the new robot. This is likely to "
               "be a fleet adapter development error.");
-            return;
           }
-
-          std::optional<std::weak_ptr<rmf_websocket::BroadcastClient>>
-          broadcast_client = std::nullopt;
-
-          if (fleet->_pimpl->broadcast_client)
-            broadcast_client = fleet->_pimpl->broadcast_client;
-
-          fleet->_pimpl->task_managers.insert({context,
-            TaskManager::make(
-              context,
-              broadcast_client,
-              std::weak_ptr<FleetUpdateHandle>(fleet))});
         });
     });
 }

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp

@@ -607,6 +607,20 @@ void RobotUpdateHandle::log_error(std::string text)
   report.log().error(std::move(text));
 }
 
+//==============================================================================
+void RobotUpdateHandle::enable_responsive_wait(bool value)
+{
+  const auto context = _pimpl->get_context();
+  if (!context)
+    return;
+
+  context->worker().schedule(
+    [mgr = context->task_manager(), value](const auto&)
+    {
+      mgr->enable_responsive_wait(value);
+    });
+}
+
 //==============================================================================
 RobotUpdateHandle::RobotUpdateHandle()
 {

rmf_fleet_adapter_python/src/adapter.cpp

@@ -212,7 +212,10 @@ PYBIND11_MODULE(rmf_adapter, m) {
     py::arg("text"))
   .def("log_error",
     &agv::RobotUpdateHandle::log_error,
-    py::arg("text"));
+    py::arg("text"))
+  .def("enable_responsive_wait",
+    &agv::RobotUpdateHandle::enable_responsive_wait,
+    py::arg("value"));
 
   // ACTION EXECUTOR   =======================================================
   auto m_robot_update_handle = m.def_submodule("robot_update_handle");