Automatically begin or cancel idle behavior when commission changes

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -1356,6 +1356,15 @@ bool TaskManager::kill_task(
   return false;
 }
 
+//==============================================================================
+void TaskManager::_cancel_idle_behavior(std::vector<std::string> labels)
+{
+  if (_waiting)
+  {
+    _waiting.cancel(std::move(labels), _context->now());
+  }
+}
+
 //==============================================================================
 void TaskManager::_begin_next_task()
 {
@@ -1373,7 +1382,6 @@ void TaskManager::_begin_next_task()
 
   if (_queue.empty() && _direct_queue.empty())
   {
-
     if (!_waiting && !_finished_waiting)
     {
       _begin_waiting();
@@ -1470,7 +1478,9 @@ void TaskManager::_begin_next_task()
   else
   {
     if (!_waiting && !_finished_waiting)
+    {
       _begin_waiting();
+    }
   }
 
   _context->worker().schedule(
@@ -1730,7 +1740,16 @@ std::function<void()> TaskManager::_make_resume_from_waiting()
           if (!self)
             return;
 
-          self->_finished_waiting = true;
+          // This condition deals with an awkward edge case where idle behavior
+          // would not restart when toggling the idle behavior commission back
+          // on. We fix this by keeping the _finished_waiting flag clean if
+          // idle behavior commissioning is off, so there's nothing to block
+          // idle behavior from beginning again if it gets toggled back on.
+          if (self->_context->commission().is_performing_idle_behavior())
+          {
+            self->_finished_waiting = true;
+          }
+
           self->_waiting = ActiveTask();
           self->_begin_next_task();
 

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp

@@ -233,6 +233,16 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
     const std::string& task_id,
     std::vector<std::string> labels);
 
+  /// This should only be triggered by RobotContext::set_commission(~), and only
+  /// in scenarios where the idle behavior commission has been toggled off.
+  void _cancel_idle_behavior(std::vector<std::string> labels);
+
+  /// Begin the next task for this robot if there is a new task ready to perform
+  /// and the robot is not already performing a task or caught in an emergency or
+  /// interruption. If no task is being performed and no new task is ready, the
+  /// idle behavior will be triggered.
+  void _begin_next_task();
+
 private:
 
   TaskManager(
@@ -408,9 +418,6 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   // Map task_id to task_log.json for all tasks managed by this TaskManager
   std::unordered_map<std::string, nlohmann::json> _task_logs = {};
 
-  /// Callback for task timer which begins next task if its deployment time has passed
-  void _begin_next_task();
-
   /// Begin performing an emergency pullover. This should only be called when an
   /// emergency is active.
   void _begin_pullover();

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp

@@ -16,6 +16,7 @@
 */
 
 #include "internal_RobotUpdateHandle.hpp"
+#include "../TaskManager.hpp"
 
 #include <rmf_traffic_ros2/Time.hpp>
 
@@ -963,8 +964,25 @@ std::shared_ptr<TaskManager> RobotContext::task_manager()
 //==============================================================================
 void RobotContext::set_commission(RobotUpdateHandle::Commission value)
 {
-  std::lock_guard<std::mutex> lock(*_commission_mutex);
-  _commission = std::move(value);
+  {
+    std::lock_guard<std::mutex> lock(*_commission_mutex);
+    _commission = std::move(value);
+  }
+
+  if (const auto mgr = _task_manager.lock())
+  {
+    if (!_commission.is_performing_idle_behavior())
+    {
+      mgr->_cancel_idle_behavior({"decommissioned"});
+    }
+    else
+    {
+      // We trigger this in case the robot needs to begin its idle behavior.
+      // If it shouldn't perform idle behavior for any reason (e.g. already
+      // performing a task), then this will have no effect.
+      mgr->_begin_next_task();
+    }
+  }
 }
 
 //==============================================================================