Introducing ResponsiveWait task phase (#308)

rmf_fleet_adapter/CHANGELOG.rst

@@ -10,6 +10,10 @@ Changelog for package rmf_fleet_adapter
     * The `rmf_fleet_adapter::agv` component interacts with a dispatcher node over topics with `rmf_task` prefix as specified in `rmf_fleet_adapter/StandardNames.hpp`
     * Support for executing tasks at specified timepoints
     * Support for `Loop`, `Delivery`, `Clean` and `ChargeBattery` tasks
+* Introduce ResponsiveWait: [#308](https://github.com/osrf/rmf_core/pull/308)
+    * The new ResponsiveWait task phase can be used to have idle/waiting robots respond to schedule conflicts
+    * Idle robots (robots that do not have an assigned task) will automatically enter ResponsiveWait mode
+
 
 1.2.0 (2021-01-05)
 ------------------

rmf_fleet_adapter/src/full_control/main.cpp

@@ -238,6 +238,23 @@ class FleetDriverRobotCommandHandle
   void update_state(const rmf_fleet_msgs::msg::RobotState& state)
   {
     auto lock = _lock();
+
+    // Update battery soc
+    const double battery_soc = state.battery_percent / 100.0;
+    if (battery_soc >= 0.0 && battery_soc <= 1.0)
+    {
+      _travel_info.updater->update_battery_soc(battery_soc);
+    }
+    else
+    {
+      RCLCPP_ERROR(
+        _node->get_logger(),
+        "Battery percentage reported by the robot is outside of the valid "
+        "range [0,100] and hence the battery soc will not be updated. It is "
+        "critical to update the battery soc with a valid battery percentage "
+        "for task allocation planning.");
+    }
+
     if (_travel_info.path_finished_callback)
     {
       // If we have a path_finished_callback, then the robot should be
@@ -353,18 +370,6 @@ class FleetDriverRobotCommandHandle
       // command
       estimate_state(_node, state.location, _travel_info);
     }
-
-    // Update battery soc
-    const double battery_soc = state.battery_percent / 100.0;
-    if (battery_soc >= 0.0 && battery_soc <= 1.0)
-      _travel_info.updater->update_battery_soc(battery_soc);
-    else
-      RCLCPP_ERROR(
-        _node->get_logger(),
-        "Battery percentage reported by the robot is outside of the valid "
-        "range [0,100] and hence the battery soc will not be updated. It is "
-        "critical to update the battery soc with a valid battery percentage "
-        "for task allocation planning.");
   }
 
   void set_updater(rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater)

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -31,6 +31,8 @@
 #include "tasks/Delivery.hpp"
 #include "tasks/Loop.hpp"
 
+#include "phases/ResponsiveWait.hpp"
+
 namespace rmf_fleet_adapter {
 
 //==============================================================================
@@ -76,9 +78,9 @@ TaskManagerPtr TaskManager::make(agv::RobotContextPtr context)
 
 //==============================================================================
 TaskManager::TaskManager(agv::RobotContextPtr context)
-  : _context(std::move(context))
+: _context(std::move(context))
 {
-  // Do nothing
+  _begin_waiting();
 }
 
 //==============================================================================
@@ -131,10 +133,10 @@ auto TaskManager::expected_finish_state() const -> State
   auto location = finish_state.location();
   location.time(rmf_traffic_ros2::convert(_context->node()->now()));
   finish_state.location(location);
-  
+
   const double current_battery_soc = _context->current_battery_soc();
   finish_state.battery_soc(current_battery_soc);
-  
+
   return finish_state;
 }
 
@@ -187,7 +189,7 @@ void TaskManager::set_queue(
           start,
           a.deployment_time(),
           a.state());
-        
+
         _queue.push_back(task);
       }
 
@@ -292,14 +294,15 @@ void TaskManager::_begin_next_task()
 
   if (_queue.empty())
   {
-    // _task_sub.unsubscribe();
-    // _expected_finish_location = rmf_utils::nullopt;
+    if (!_waiting)
+      _begin_waiting();
 
-    // RCLCPP_INFO(
-    //       _context->node()->get_logger(),
-    //       "Finished all remaining tasks for [%s]",
-    //       _context->requester_id().c_str());
+    return;
+  }
 
+  if (_waiting)
+  {
+    _waiting->cancel();
     return;
   }
 
@@ -378,6 +381,70 @@ void TaskManager::_begin_next_task()
     _active_task->begin();
     _register_executed_task(_active_task->id());
   }
+  else
+  {
+    if (!_waiting)
+      _begin_waiting();
+  }
+}
+
+//==============================================================================
+void TaskManager::_begin_waiting()
+{
+  const std::size_t waiting_point = _context->location().front().waypoint();
+
+  _waiting = phases::ResponsiveWait::make_indefinite(
+        _context, waiting_point)->begin();
+
+  _task_sub = _waiting->observe()
+    .observe_on(rxcpp::identity_same_worker(_context->worker()))
+    .subscribe(
+      [this](Task::StatusMsg msg)
+      {
+        msg.task_id = _context->requester_id() + ":waiting";
+        msg.robot_name = _context->name();
+        msg.fleet_name = _context->description().owner();
+        // TODO: Fill in end_time with the beginning time of the next task if
+        // the next task is known.
+        msg.start_time = _context->node()->now();
+        msg.end_time = msg.start_time;
+
+        _context->node()->task_summary()->publish(msg);
+      },
+      [this](std::exception_ptr e)
+      {
+        rmf_task_msgs::msg::TaskSummary msg;
+        msg.state = msg.STATE_FAILED;
+
+        try {
+          std::rethrow_exception(e);
+        }
+        catch (const std::exception& e) {
+          msg.status = e.what();
+        }
+
+        msg.task_id = _context->requester_id() + ":waiting";
+        msg.robot_name = _context->name();
+        msg.fleet_name = _context->description().owner();
+        msg.start_time = rmf_traffic_ros2::convert(
+          _active_task->deployment_time());
+        msg.end_time = rmf_traffic_ros2::convert(
+          _active_task->finish_state().finish_time());
+        _context->node()->task_summary()->publish(msg);
+
+        RCLCPP_WARN(
+          _context->node()->get_logger(),
+          "Robot [%s] encountered an error while doing a ResponsiveWait: %s",
+          _context->requester_id().c_str(), msg.status.c_str());
+
+        // Go back to waiting if an error has occurred
+        _begin_waiting();
+      },
+      [this]()
+      {
+        _waiting = nullptr;
+        _begin_next_task();
+      });
 }
 
 //==============================================================================

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp

@@ -88,6 +88,11 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   rxcpp::subscription _task_sub;
   rxcpp::subscription _emergency_sub;
 
+  /// This phase will kick in automatically when no task is being executed. It
+  /// will ensure that the agent continues to respond to traffic negotiations so
+  /// it does not become a blocker for other traffic participants.
+  std::shared_ptr<Task::ActivePhase> _waiting;
+
   // TODO: Eliminate the need for a mutex by redesigning the use of the task
   // manager so that modifications of shared data only happen on designated
   // rxcpp worker
@@ -102,6 +107,9 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   /// Callback for task timer which begins next task if its deployment time has passed
   void _begin_next_task();
 
+  /// Begin responsively waiting for the next task
+  void _begin_waiting();
+
   /// Function to register the task id of a task that has begun execution
   /// The input task id will be inserted into the registry such that the max
   /// size of the registry is 100.

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp

@@ -184,7 +184,7 @@ class RobotContext
   rmf_traffic::schedule::Negotiator* _negotiator = nullptr;
 
   /// Always call the current_battery_soc() setter to set a new value
-  double _current_battery_soc;
+  double _current_battery_soc = 1.0;
   rxcpp::subjects::subject<double> _battery_soc_publisher;
   rxcpp::observable<double> _battery_soc_obs;
   rmf_task::agv::State _current_task_end_state;

rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.hpp

@@ -56,7 +56,7 @@ class GoToPlace
     void cancel() final;
 
     // Documentation inherited from ActivePhase
-    const std::string & description() const final;
+    const std::string& description() const final;
 
     // Documentation inherited from Negotiator
     void respond(