diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 326f00704..219ae05e2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -922,6 +922,29 @@ void TaskManager::set_queue( { return; } + + // Cancel pending automatic tasks that will be removed from the new + // assignments. + std::unordered_set new_automatic_task_ids; + const std::vector cancellation_labels = + {"New task assignments received."}; + for (const auto& a : assignments) + { + if (a.request()->booking()->automatic()) + { + new_automatic_task_ids.insert(a.request()->booking()->id()); + } + } + for (const auto& a : _queue) + { + if (a.request()->booking()->automatic() && + new_automatic_task_ids.find(a.request()->booking()->id()) == + new_automatic_task_ids.end()) + { + _publish_canceled_pending_task(a, cancellation_labels); + } + } + _queue = assignments; _publish_task_queue(); }