Modifications to support refactor of rmf_task

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp

@@ -74,7 +74,8 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   /// Specify a set of lanes that should be open.
   void open_lanes(std::vector<std::size_t> lane_indices);
 
-  /// Set the parameters required for task planning
+  /// Set the parameters required for task planning. Without calling this
+  /// function, this fleet will not bid for and accept tasks.
   ///
   /// \param[in] battery_system
   ///   Specify the battery system used by the vehicles in this fleet.
@@ -88,29 +89,32 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   /// \param[in] tool_sink
   ///   Specify the device sink for special tools used by the vehicles in this fleet.
   ///
+  /// \param[in] recharge_threshold
+  ///   The threshold for state of charge below which robots in this fleet
+  ///   will cease to operate and require recharging. A value between 0.0 and
+  ///   1.0 should be specified.
+  ///
+  /// \param[in] recharge_soc
+  ///   The state of charge to which robots in this fleet should be charged up
+  ///   to by automatic recharging tasks. A value between 0.0 and 1.0 should be
+  ///   specified.
+  ///
+  /// \param[in] account_for_battery_drain
+  ///   Specify whether battery drain is to be considered while allocating tasks.
+  ///   If false, battery drain will not be considered when planning for tasks.
+  ///   As a consequence, charging tasks will not be automatically assigned to
+  ///   vehicles in this fleet when battery levels fall below the
+  ///   recharge_threshold.
+  ///
   /// \return true if task planner parameters were successfully updated.
   bool set_task_planner_params(
     std::shared_ptr<rmf_battery::agv::BatterySystem> battery_system,
     std::shared_ptr<rmf_battery::MotionPowerSink> motion_sink,
     std::shared_ptr<rmf_battery::DevicePowerSink> ambient_sink,
-    std::shared_ptr<rmf_battery::DevicePowerSink> tool_sink);
-
-  /// Specify whether battery drain is to be considered while allocating tasks.
-  /// By default battery drain is considered.
-  ///
-  /// \param[in] value
-  ///   If false, battery drain will not be considered when planning for tasks.
-  ///   As a consequence, charging tasks will not be automatically assigned to
-  ///   vehicles in this fleet when battery levels fall below their thresholds.
-  bool account_for_battery_drain(bool value = true);
-
-  /// Set the threshold for state of charge below which robots in this fleet
-  /// will cease to operate and require recharging. A value between 0.0 and 1.0
-  /// should be specified. Default value is 0.2.
-  ///
-  /// \param[in] threshold
-  ///   Threshold as a fraction of the total battery capacity
-  FleetUpdateHandle& set_recharge_threshold(const double threshold);
+    std::shared_ptr<rmf_battery::DevicePowerSink> tool_sink,
+    double recharge_threshold,
+    double recharge_soc,
+    bool account_for_battery_drain);
 
   /// A callback function that evaluates whether a fleet will accept a task
   /// request

rmf_fleet_adapter/launch/fleet_adapter.launch.xml

@@ -36,6 +36,7 @@
   <arg name="tool_power_drain" description="The power rating(W) of special tools (vaccuum, cleaning systems, etc.) of the vehicles in this fleet"/>
   <arg name="drain_battery" default="false" description="Whether battery drain should be considered while assigning tasks to vechiles in this fleet"/>
   <arg name="recharge_threshold" default="0.2" description="The fraction of total battery capacity below which the robot must return to its charger"/>
+  <arg name="recharge_soc" default="1.0" description="The fraction of total battery capacity to which the robot should be charged"/>
   <arg name="experimental_lift_watchdog_service" default="" description="(Experimental) The name of a service to check whether a robot can enter a lift"/>
 
 
@@ -77,6 +78,7 @@
     <param name="tool_power_drain" value="$(var tool_power_drain)"/>
     <param name="drain_battery" value="$(var drain_battery)"/>
     <param name="recharge_threshold" value="$(var recharge_threshold)"/> 
+    <param name="recharge_soc" value="$(var recharge_soc)"/> 
 
     <param name="experimental_lift_watchdog_service" value="$(var experimental_lift_watchdog_service)"/>
 

rmf_fleet_adapter/src/full_control/main.cpp

@@ -922,16 +922,20 @@ std::shared_ptr<Connections> make_fleet(
   // Drain battery
   const bool drain_battery = rmf_fleet_adapter::get_parameter_or_default(
     *node, "drain_battery", false);
-  connections->fleet->account_for_battery_drain(drain_battery);
-
   // Recharge threshold
   const double recharge_threshold = rmf_fleet_adapter::get_parameter_or_default(
     *node, "recharge_threshold", 0.2);
-
-  connections->fleet->set_recharge_threshold(recharge_threshold);
-
+  // Recharge state of charge
+  const double recharge_soc = rmf_fleet_adapter::get_parameter_or_default(
+    *node, "recharge_soc", 1.0);
   if (!connections->fleet->set_task_planner_params(
-        battery_system, motion_sink, ambient_sink, tool_sink))
+        battery_system,
+        motion_sink,
+        ambient_sink,
+        tool_sink,
+        recharge_threshold,
+        recharge_soc,
+        drain_battery))
   {
     RCLCPP_ERROR(
       node->get_logger(),

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -180,7 +180,7 @@ void TaskManager::set_queue(
       rmf_task_msgs::msg::TaskType task_type_msg;
       const auto request = a.request();
       if (std::dynamic_pointer_cast<
-        const rmf_task::requests::CleanDescription>(request->description()) != nullptr)
+        const rmf_task::requests::Clean::Description>(request->description()) != nullptr)
       {
         task_type_msg.type = task_type_msg.TYPE_CLEAN;
         auto task = rmf_fleet_adapter::tasks::make_clean(
@@ -194,7 +194,7 @@ void TaskManager::set_queue(
       }
 
       else if (std::dynamic_pointer_cast<
-        const rmf_task::requests::ChargeBatteryDescription>(
+        const rmf_task::requests::ChargeBattery::Description>(
           request->description()) != nullptr)
       {
         task_type_msg.type = task_type_msg.TYPE_CHARGE_BATTERY;
@@ -209,7 +209,7 @@ void TaskManager::set_queue(
       }
 
       else if (std::dynamic_pointer_cast<
-        const rmf_task::requests::DeliveryDescription>(
+        const rmf_task::requests::Delivery::Description>(
           request->description()) != nullptr)
       {
         task_type_msg.type = task_type_msg.TYPE_DELIVERY;
@@ -224,7 +224,7 @@ void TaskManager::set_queue(
       }
 
       else if (std::dynamic_pointer_cast<
-        const rmf_task::requests::LoopDescription>(request->description()) != nullptr)
+        const rmf_task::requests::Loop::Description>(request->description()) != nullptr)
       {
         task_type_msg.type = task_type_msg.TYPE_LOOP;
         const auto task = tasks::make_loop(
@@ -276,7 +276,7 @@ const std::vector<rmf_task::ConstRequestPtr> TaskManager::requests() const
   for (const auto& task : _queue)
   {
     if (std::dynamic_pointer_cast<
-      const rmf_task::requests::ChargeBatteryDescription>(
+      const rmf_task::requests::ChargeBattery::Description>(
         task->request()->description()))
       continue;
     requests.push_back(task->request());
@@ -309,7 +309,15 @@ void TaskManager::_begin_next_task()
   const rmf_traffic::Time now = rmf_traffic_ros2::convert(
     _context->node()->now());
   const auto next_task = _queue.front();
-  const auto deployment_time = next_task->deployment_time();
+  // We take the minimum of the two to deal with cases where the deployment_time
+  // as computed by the task planner is greater than the earliest_start_time
+  // which is greater than now. This can happen for example if the previous task
+  // completed earlier than estimated.
+  // TODO: Reactively replan task assignments across agents in a fleet every
+  // time as task is completed.
+  const auto deployment_time = std::min(
+    next_task->deployment_time(),
+    next_task->request()->earliest_start_time());
 
   if (now >= deployment_time)
   {
@@ -460,22 +468,25 @@ void TaskManager::retreat_to_charger()
   if (!task_planner)
     return;
 
+  if (!task_planner->configuration().constraints().drain_battery())
+    return;
+
   const auto current_state = expected_finish_state();
   if (current_state.waypoint() == current_state.charging_waypoint())
     return;
 
-  const double threshold_soc =
-    _context->task_planning_constraints().threshold_soc();
-  const double retreat_threshold = 1.2 * threshold_soc;
+  const auto& constraints = task_planner->configuration().constraints();
+  const double threshold_soc = constraints.threshold_soc();
+  const double retreat_threshold = 1.2 * threshold_soc; // safety factor
   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();
+  const auto& parameters = task_planner->configuration().parameters();
+  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);
+  const auto& cache_result = estimate_cache.get(endpoints);
 
   if (cache_result)
   {
@@ -485,7 +496,7 @@ void TaskManager::retreat_to_charger()
   {
     const rmf_traffic::agv::Planner::Goal retreat_goal{
       current_state.charging_waypoint()};
-    const auto result_to_charger = task_planner_config.planner()->plan(
+    const auto result_to_charger = parameters.planner()->plan(
       current_state.location(), retreat_goal);
 
     // We assume we can always compute a plan
@@ -502,16 +513,16 @@ void TaskManager::retreat_to_charger()
         finish_time - itinerary_start_time;
 
       dSOC_motion =
-        task_planner_config.motion_sink()->compute_change_in_charge(
+        parameters.motion_sink()->compute_change_in_charge(
           trajectory);
       dSOC_device =
-        task_planner_config.ambient_sink()->compute_change_in_charge(
+        parameters.ambient_sink()->compute_change_in_charge(
           rmf_traffic::time::to_seconds(itinerary_duration));
       retreat_battery_drain += dSOC_motion + dSOC_device;
       retreat_duration +=itinerary_duration;
       itinerary_start_time = finish_time;
     }
-    estimate_cache->set(endpoints, retreat_duration,
+    estimate_cache.set(endpoints, retreat_duration,
       retreat_battery_drain);
   }
 
@@ -522,16 +533,15 @@ void TaskManager::retreat_to_charger()
     (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(),
+    const auto charging_request = rmf_task::requests::ChargeBattery::make(
       current_state.finish_time());
+    const auto model = charging_request->description()->make_model(
+      current_state.finish_time(),
+      parameters);
 
-    const auto finish = charging_request->description()->estimate_finish(
+    const auto finish = model->estimate_finish(
       current_state,
-      _context->task_planning_constraints(),
+      constraints,
       estimate_cache);
 
     if (!finish)