Checks idle behavior before executing emergency charge

[aaronchongth]

Bug fix

Fixed bug

Fixes https://discourse.openrobotics.org/t/auto-recharge-task-dispatched-to-decommissioned-robot-with-idle-behavior-false/53190, depends on expected behavior.

Requires open-rmf/rmf_task#134

Fix applied

• checks if idle behavior is charging, if so, also skip emergency charging task when decommissions without idle behaviors allowed

How to test

Make these changes to slotcar_common.hpp, to start with lower battery and drain faster

diff --git a/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp b/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp
index 59977c4..58f6a0d 100644
--- a/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp
+++ b/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp
@@ -221,7 +221,7 @@ private:
   const std::string _enable_instant_charge_str = "_enable_instant_charge";
   const std::string _enable_drain_str = "_enable_drain";
   const double _soc_max = 1.0;
-  double _soc = _soc_max;
+  double _soc = 0.61;
   std::vector<Eigen::Vector3d> _charger_positions;
   // Straight line distance to charging waypoint within which charging can occur
   static constexpr double _charger_dist_thres = 0.3;
diff --git a/rmf_robot_sim_common/src/slotcar_common.cpp b/rmf_robot_sim_common/src/slotcar_common.cpp
index 487c25a..e5d3990 100644
--- a/rmf_robot_sim_common/src/slotcar_common.cpp
+++ b/rmf_robot_sim_common/src/slotcar_common.cpp
@@ -1089,7 +1089,7 @@ double SlotcarCommon::compute_discharge(
   // The depleted state of charge as a fraction in range [0,1]
   double dSOC = dQ / (_params.nominal_capacity * 3600.0);
 
-  return dSOC;
+  return dSOC * 10;
 }

Make this change to tinyRobot_config.yaml, to increase recharge threshold

diff --git a/rmf_demos/config/office/tinyRobot_config.yaml b/rmf_demos/config/office/tinyRobot_config.yaml
index 3596249..9e9e23c 100644
--- a/rmf_demos/config/office/tinyRobot_config.yaml
+++ b/rmf_demos/config/office/tinyRobot_config.yaml
@@ -22,7 +22,7 @@ rmf_fleet:
     power: 20.0 # W
   tool_system:
     power: 0.0 # W
-  recharge_threshold: 0.10 # Battery level below which robots in this fleet will not operate
+  recharge_threshold: 0.50 # Battery level below which robots in this fleet will not operate
   recharge_soc: 1.0 # Battery level to which robots in this fleet should be charged up to during recharging tasks
   publish_fleet_state: 10.0 # Publish frequency for fleet state, ensure that it is same as robot_state_update_frequency
   account_for_battery_drain: True

After rebuilding and starting the sim,

• change the RTF to 5x
• start a patrol task between pantry and patrol_b for 20 loops
• when the battery goes to around 55%, decommission the robot without idle behaviors
• cancel the current task
• the robot should stay at the same spot (before this PR, it will head over to charge)

GenAI Use

We follow OSRA's policy on GenAI tools

• I used a GenAI tool in this PR.
• I did not use GenAI

Generated-by:

[xiyuoh]

Thanks for adding this in! This approach seems reasonable to me, also tested it out with/without the fix and the robot is now completely decommissioned without being allocated emergency charge task!

[Akio-Ki]

Hi @aaronchongth, thanks for this fix! I have a question about the refinement in 8ec63c6.

When finishing_request is not configured (i.e., _idle_task is null), the label check in 8ec63c6 is entirely skipped because it's inside if (_idle_task), and the emergency charge still gets dispatched to decommissioned robots.

The simpler approach in 8e9355c (if (!is_performing_idle_behavior()) return false;) would have caught this case. Was the refinement in 8ec63c6 intentional to preserve emergency charging for non-charge idle behaviors (e.g., park)?

[aaronchongth]

hey @Akio-Ki! Yes that is intended, because emergency-retreat-to-charger is a separate feature from idle behaviors.
With this current change, only if the configured idle behavior is charging, and the robot is decommissioned to not perform idle behaviors, will the emergency-retreat-to-charger be ignored.