Automatic retreat for battery recharging

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -24,6 +24,8 @@
 
 #include <rmf_traffic_ros2/Time.hpp>
 
+#include <rmf_traffic/agv/Planner.hpp>
+
 #include "tasks/Clean.hpp"
 #include "tasks/ChargeBattery.hpp"
 #include "tasks/Delivery.hpp"
@@ -52,7 +54,7 @@ TaskManagerPtr TaskManager::make(agv::RobotContextPtr context)
       }
     });
 
-  mgr->_timer = mgr->context()->node()->create_wall_timer(
+  mgr->_task_timer = mgr->context()->node()->create_wall_timer(
     std::chrono::seconds(1),
     [w = mgr->weak_from_this()]()
     {
@@ -61,6 +63,16 @@ TaskManagerPtr TaskManager::make(agv::RobotContextPtr context)
         mgr->_begin_next_task();
       }
     });
+
+  mgr->_retreat_timer = mgr->context()->node()->create_wall_timer(
+    std::chrono::seconds(10),
+    [w = mgr->weak_from_this()]()
+    {
+      if (auto mgr = w.lock())
+      {
+        mgr->retreat_to_charger();
+      }
+    });
   return mgr;
 }
 
@@ -359,4 +371,95 @@ void TaskManager::clear_queue()
   _queue.clear();
 }
 
+//==============================================================================
+void TaskManager::retreat_to_charger()
+{
+  if (_active_task || !_queue.empty())
+    return;
+
+  const auto task_planner = _context->task_planner();
+  if (!task_planner)
+    return;
+
+  const auto current_state = expected_finish_state();
+  if (current_state.waypoint() == current_state.charging_waypoint())
+    return;
+
+  const double threshold_soc = _context->state_config().threshold_soc();
+  const double retreat_threshold = 1.2 * threshold_soc;
+  const double current_battery_soc = _context->current_battery_soc();
+
+  const auto task_planner_config = task_planner->config();
+  const auto estimate_cache = task_planner->estimate_cache();
+
+  double retreat_battery_drain = 0.0;
+  const auto endpoints = std::make_pair(current_state.waypoint(),
+    current_state.charging_waypoint());
+  const auto& cache_result = estimate_cache->get(endpoints);
+
+  if (cache_result)
+  {
+    retreat_battery_drain = cache_result->dsoc;
+  }
+  else
+  {
+    const rmf_traffic::agv::Planner::Goal retreat_goal{current_state.charging_waypoint()};
+    const auto result_to_charger = task_planner_config->planner()->plan(
+      current_state.location(), retreat_goal);
+
+    // We assume we can always compute a plan
+    const auto& trajectory =
+        result_to_charger->get_itinerary().back().trajectory();
+    const auto& finish_time = *trajectory.finish_time();
+    const rmf_traffic::Duration retreat_duration =
+      finish_time - current_state.finish_time();
+
+    const double dSOC_motion =
+      task_planner_config->motion_sink()->compute_change_in_charge(trajectory);
+    const double dSOC_device =
+      task_planner_config->ambient_sink()->compute_change_in_charge(
+        rmf_traffic::time::to_seconds(retreat_duration));
+    retreat_battery_drain = dSOC_motion + dSOC_device;
+
+    // TODO(YV) Protect this call with a mutex
+    estimate_cache->set(endpoints, retreat_duration,
+      retreat_battery_drain);
+  }
+
+  const double battery_soc_after_retreat =
+    current_battery_soc - retreat_battery_drain;
+
+  if ((battery_soc_after_retreat < retreat_threshold) &&
+    (battery_soc_after_retreat > threshold_soc))
+  {
+    // Add a new charging task to the task queue
+    auto charging_request = rmf_task::requests::ChargeBattery::make(
+      task_planner_config->battery_system(),
+      task_planner_config->motion_sink(),
+      task_planner_config->ambient_sink(),
+      task_planner_config->planner(),
+      current_state.finish_time());
+
+    const auto finish = charging_request->estimate_finish(
+      current_state,
+      _context->state_config(),
+      estimate_cache);
+
+    if (!finish)
+      return;
+
+    rmf_task::agv::TaskPlanner::Assignment charging_assignment(
+      charging_request,
+      finish.value().finish_state(),
+      current_state.finish_time());
+
+    set_queue({charging_assignment});
+
+    RCLCPP_INFO(
+      _context->node()->get_logger(),
+      "Initiating automatic retreat to charger for robot [%s]",
+      _context->name().c_str());
+  }
+}
+
 } // namespace rmf_fleet_adapter

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp

@@ -64,12 +64,16 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   /// Get the non-charging requests among pending tasks
   const std::vector<rmf_task::ConstRequestPtr> requests() const;
 
-  // Callback for timer which begins next task if its deployment time has passed
+  /// Callback for task timer which begins next task if its deployment time has passed
   void _begin_next_task();
 
-  // The state of the robot.
+  /// The state of the robot.
   State expected_finish_state() const;
 
+  /// Callback for the retreat timer. Appends a charging task to the task queue
+  /// when robot is idle and battery level drops below a retreat threshold.
+  void retreat_to_charger();
+
 private:
 
   TaskManager(agv::RobotContextPtr context);
@@ -82,7 +86,8 @@ class TaskManager : public std::enable_shared_from_this<TaskManager>
   rxcpp::subscription _emergency_sub;
 
   std::mutex _mutex;
-  rclcpp::TimerBase::SharedPtr _timer;
+  rclcpp::TimerBase::SharedPtr _task_timer;
+  rclcpp::TimerBase::SharedPtr _retreat_timer;
 
   void clear_queue();
 };

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -633,7 +633,8 @@ void FleetUpdateHandle::add_robot(
             fleet->_pimpl->worker,
             fleet->_pimpl->default_maximum_delay,
             state,
-            state_config
+            state_config,
+            fleet->_pimpl->task_planner
           });
 
     // We schedule the following operations on the worker to make sure we do not

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp

@@ -253,6 +253,12 @@ const rxcpp::observable<double>& RobotContext::observe_battery_soc() const
   return _battery_soc_obs;
 }
 
+//==============================================================================
+const std::shared_ptr<const rmf_task::agv::TaskPlanner>& 
+RobotContext::task_planner() const
+{
+  return _task_planner;
+}
 
 //==============================================================================
 void RobotContext::respond(
@@ -284,7 +290,8 @@ RobotContext::RobotContext(
   const rxcpp::schedulers::worker& worker,
   rmf_utils::optional<rmf_traffic::Duration> maximum_delay,
   rmf_task::agv::State state,
-  rmf_task::agv::StateConfig state_config)
+  rmf_task::agv::StateConfig state_config,
+  std::shared_ptr<const rmf_task::agv::TaskPlanner> task_planner)
   : _command_handle(std::move(command_handle)),
     _location(std::move(_initial_location)),
     _itinerary(std::move(itinerary)),
@@ -296,7 +303,8 @@ RobotContext::RobotContext(
     _requester_id(
       _itinerary.description().owner() + "/" + _itinerary.description().name()),
     _state(state),
-    _state_config(state_config)
+    _state_config(state_config),
+    _task_planner(std::move(task_planner))
 {
   _profile = std::make_shared<rmf_traffic::Profile>(
         _itinerary.description().profile());

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp

@@ -11,6 +11,7 @@
 
 #include <rmf_task/agv/State.hpp>
 #include <rmf_task/agv/StateConfig.hpp>
+#include <rmf_task/agv/TaskPlanner.hpp>
 
 #include <rclcpp/node.hpp>
 
@@ -134,6 +135,10 @@ class RobotContext
   // Get a reference to the battery soc observer of this robot.
   const rxcpp::observable<double>& observe_battery_soc() const;
 
+  /// Get a mutable reference to the task planner for this robot
+  const std::shared_ptr<const rmf_task::agv::TaskPlanner>& task_planner() const;
+
+
 private:
   friend class FleetUpdateHandle;
   friend class RobotUpdateHandle;
@@ -148,7 +153,8 @@ class RobotContext
     const rxcpp::schedulers::worker& worker,
     rmf_utils::optional<rmf_traffic::Duration> maximum_delay,
     rmf_task::agv::State state,
-    rmf_task::agv::StateConfig state_config);
+    rmf_task::agv::StateConfig state_config,
+    std::shared_ptr<const rmf_task::agv::TaskPlanner> task_planner);
 
   std::weak_ptr<RobotCommandHandle> _command_handle;
   std::vector<rmf_traffic::agv::Plan::Start> _location;
@@ -175,6 +181,7 @@ class RobotContext
   rxcpp::observable<double> _battery_soc_obs;
   rmf_task::agv::State _state;
   rmf_task::agv::StateConfig _state_config;
+  std::shared_ptr<const rmf_task::agv::TaskPlanner> _task_planner;
 };
 
 using RobotContextPtr = std::shared_ptr<RobotContext>;

rmf_task/include/rmf_task/agv/TaskPlanner.hpp

@@ -151,6 +151,9 @@ class TaskPlanner
   /// The configuration for the planner
   TaskPlanner(std::shared_ptr<Configuration> config);
 
+  /// Get a shared pointer to the configuration of this task planner
+  const std::shared_ptr<Configuration> config() const;
+
   /// Get the greedy planner based assignments for a set of initial states and 
   /// requests
   Assignments greedy_plan(

rmf_task/src/rmf_task/agv/TaskPlanner.cpp

@@ -679,9 +679,11 @@ bool Filter::ignore(const Node& node)
   return !new_node;
 }
 
+// ============================================================================
 const rmf_traffic::Duration segmentation_threshold =
     rmf_traffic::time::from_seconds(1.0);
 
+// ============================================================================
 inline double compute_g_assignment(const TaskPlanner::Assignment& assignment)
 {
   if (std::dynamic_pointer_cast<const rmf_task::requests::ChargeBattery>(
@@ -696,6 +698,7 @@ inline double compute_g_assignment(const TaskPlanner::Assignment& assignment)
 
 } // anonymous namespace
 
+// ============================================================================
 class TaskPlanner::Implementation
 {
 public:
@@ -1345,6 +1348,7 @@ class TaskPlanner::Implementation
 
 };
 
+// ============================================================================
 TaskPlanner::TaskPlanner(std::shared_ptr<Configuration> config)
 : _pimpl(rmf_utils::make_impl<Implementation>(
       Implementation{
@@ -1355,6 +1359,7 @@ TaskPlanner::TaskPlanner(std::shared_ptr<Configuration> config)
   // Do nothing
 }
 
+// ============================================================================
 auto TaskPlanner::greedy_plan(
   rmf_traffic::Time time_now,
   std::vector<State> initial_states,
@@ -1370,6 +1375,7 @@ auto TaskPlanner::greedy_plan(
     true);
 }
 
+// ============================================================================
 auto TaskPlanner::optimal_plan(
   rmf_traffic::Time time_now,
   std::vector<State> initial_states,
@@ -1386,16 +1392,23 @@ auto TaskPlanner::optimal_plan(
     false);
 }
 
+// ============================================================================
 auto TaskPlanner::compute_cost(const Assignments& assignments) -> double
 {
   return _pimpl->compute_g(assignments);
 }
 
+// ============================================================================
 const std::shared_ptr<EstimateCache> TaskPlanner::estimate_cache() const
 {
   return _pimpl->estimate_cache;
 }
 
+// ============================================================================
+const std::shared_ptr<TaskPlanner::Configuration> TaskPlanner::config() const
+{
+  return _pimpl->config;
+}
 
 } // namespace agv
 } // namespace rmf_task