From 0e5380a825ec5312996338170bde1d28b6b684c3 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Thu, 20 Feb 2025 21:37:18 +0800
Subject: [PATCH] Fix potential issues around tasks starting

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 .../src/rmf_fleet_adapter/TaskManager.cpp          | 14 ++++++++++++++
 .../rmf_fleet_adapter/events/WaitForTraffic.cpp    |  2 ++
 .../src/rmf_fleet_adapter/phases/WaitForCharge.cpp |  2 +-
 3 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
index d26717c3..337d3328 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp
@@ -1293,6 +1293,15 @@ nlohmann::json TaskManager::submit_direct_request(
     request_id.c_str(),
     robot.c_str());
 
+  _context->worker().schedule([w = weak_from_this()](const auto&)
+    {
+      if (const auto self = w.lock())
+      {
+        // Schedule this manager to check if it should run this task.
+        self->_begin_next_task();
+      }
+    });
+
   // Publish api response
   nlohmann::json response_json;
   response_json["success"] = true;
@@ -1499,6 +1508,7 @@ void TaskManager::_begin_next_task()
   const rmf_traffic::Time now = rmf_traffic_ros2::convert(
     _context->node()->now());
 
+
   if (now >= deployment_time)
   {
     if (_waiting)
@@ -1550,7 +1560,9 @@ void TaskManager::_begin_next_task()
         [w = weak_from_this()](const auto&)
         {
           if (const auto self = w.lock())
+          {
             self->_begin_next_task();
+          }
         });
 
       return;
@@ -2557,7 +2569,9 @@ std::function<void()> TaskManager::_task_finished(std::string id)
         [w = self->weak_from_this()](const auto&)
         {
           if (const auto self = w.lock())
+          {
             self->_begin_next_task();
+          }
         });
     };
 }
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp
index 9376a4d7..574ed7f6 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp
@@ -282,7 +282,9 @@ void WaitForTraffic::Active::_consider_going()
   for (const auto& dep : _dependencies)
   {
     if (!dep.reached() && !dep.deprecated())
+    {
       all_dependencies_reached = false;
+    }
   }
 
   if (all_dependencies_reached)
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp
index 9139d20c..8dc1fe24 100644
--- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp
+++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp
@@ -150,7 +150,7 @@ std::shared_ptr<LegacyTask::ActivePhase> WaitForCharge::Pending::begin()
     "Robot [%s] has begun waiting for its battery to charge to %.1f%%. "
     "Please ensure that the robot is charging.",
     _context->name().c_str(),
-    _charge_to_soc.value_or(1.0) * 100.0);
+    _charge_to_soc.value_or(0.98) * 100.0);
 
   active->_battery_soc_subscription = _context->observe_battery_soc()
     .observe_on(rxcpp::identity_same_worker(_context->worker()))