Disable automatic retreat #330

xiyuoh · 2024-03-12T04:34:01Z

This PR aims to

Make retreat_to_charger configurable from the fleet config, and
Prevents retreat_to_charger from being triggered if the robot's finishing task is already a Charge task

We ran into scenarios where the robot is on its way back to the charger (as part of the finishing task) while its battery level is nearing the recharge threshold. This sometimes triggers a retreat to charger that overrides the finishing task. As a result, the robot is unable to perform other tasks queued later on until its current battery SOC reaches the recharge SOC.

In the retreat_to_charger callback, we now check the fleet's configured behavior before initiating automatic retreat.

This depends on open-rmf/rmf_task#114

Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>

Yadunund

Thanks for working on the fix but I feel the implementation should be as follows:

FleetUpdateHandle::set_task_planner_params() should accept an std::optional<rmf_traffic::Duration> param.

Add a TaskManager::retreat_to_charger(std::optional<rmf_traffic::Duration> duration) method. If a value is provided this method will execute

rmf_ros2/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

Lines 184 to 192 in a2269ab

    
           mgr->_retreat_timer = mgr->context()->node()->try_create_wall_timer( 
        
             std::chrono::seconds(10), 
        
             [w = mgr->weak_from_this()]() 
        
             { 
        
               if (auto mgr = w.lock()) 
        
               { 
        
                 mgr->retreat_to_charger(); 
        
               } 
        
             });

. If std::nullopt, it will call mgr->_retreat_timer.reset() to disable the timer.

Setting the task planner params or using the new setter in FleetUpdateHandle will invoke the method in 2. This would allow for more efficient processing as if the check is not needed, the timer callback will not execute thereby saving some cycles for the ROS 2 executor.

Yadunund · 2024-03-12T06:27:34Z

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp

+  /// Get whether or not to trigger automatic retreat to charger.
+  bool retreat_to_charger() const;
+
+  /// Set whether or not to trigger automatic retreat to charger.
+  void set_retreat_to_charger(bool value);
+


I don't think these APIs should be added here. Instead they should be present in FleetUpdateHandle instead. Users who rely on EasyFullControl API can get a FleetUpdateHandler handler using the EasyFullControl::more() API and set this config.

See comment below on setting/getting this value as an std::optional<rmf_traffic::Duration>.

I see, what is stopping us from allowing this to be parsed in automatically via FleetConfiguration? This API is used within set_task_planner_params to retrieve the configured values. Without it, users will have to modify the default value from the fleet adapter implementation itself.

Yadunund · 2024-03-12T06:35:36Z

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -1628,6 +1628,10 @@ void TaskManager::retreat_to_charger()

  if (!task_planner->configuration().constraints().drain_battery())
    return;
+  if (!task_planner->configuration().constraints().retreat_to_charger())
+    return;
+  if (_idle_task && _idle_task->request_type() == "Charge")


We shouldn't have comparison to magic stings like this. The rmf_task::Request class should have a virtual static member function that should return the request type. Then you can call _idle_task && _idle_task->request_type() == ParkRobotIndefinitely::request_type()

With the proposed refactor, we no longer rely on checking request types.

Yadunund · 2024-03-12T06:42:26Z

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp

@@ -308,7 +313,8 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
    double recharge_threshold,
    double recharge_soc,
    bool account_for_battery_drain,
-    rmf_task::ConstRequestFactoryPtr finishing_request = nullptr);
+    rmf_task::ConstRequestFactoryPtr finishing_request = nullptr,
+    bool retreat_to_charger = true);


I don't think this should be a bool. Instead it should be the timer period. If this is set to std::nullopt, we reset the timer thereby disabling the automatic retreat to charger behavior. This way we not only expose the ability to enable/disable this feature but also allow users to configure the rate at which the check happens. Else we will need to add another parameter for that.

The docstring above should also be updated and it's worth mentioning the timer there so users know how this check is performed.

Suggested change

bool retreat_to_charger = true);

std::optional<rmf_traffic::Duration> check_retreat_to_charger_interval = rmf_traffic::time::from_seconds(10));

Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>

xiyuoh · 2024-04-15T07:32:31Z

Updated according to the changes requested, without the check for the fleet's configured idle task before deciding whether to proceed with the automatic retreat to charger. I'll work on that in a separate PR.

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>

mxgrey

I've made some changes to prevent the API from breaking and for a few other nitpicky reasons.

I've left in-line comments as part of this review to explain the changes that are being made, but no further action is needed.

mxgrey · 2024-04-16T03:44:12Z

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp

@@ -568,6 +574,7 @@ class EasyFullControl::FleetConfiguration
    double recharge_threshold,
    double recharge_soc,
    bool account_for_battery_drain,
+    std::optional<rmf_traffic::Duration> retreat_to_charger_interval,


Adding a field to the constructor breaks the API in a way that isn't backwards compatible.

We should leave this argument out of the constructor and instead provide a set_retreat_to_charger_interval(std::optional<rmf_traffic::Duration>) member function.

We could also consider putting this as the last argument in the constructor and giving it a default value. That would probably be an ABI break, but we can probably have some flexibility on that, as opposed to breaking the API which we should not do for anything less serious than a security vulnerability.

mxgrey · 2024-04-16T03:49:18Z

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

@@ -1753,7 +1743,33 @@ std::function<void()> TaskManager::_make_resume_from_waiting()
 }

 //==============================================================================
-void TaskManager::retreat_to_charger()
+void TaskManager::retreat_to_charger(


This is somewhat of a nitpick, but I don't like that we're changing the meaning of the retreat_to_charger function. I think I'd prefer we keep the current retreat_to_charger function as it was, and introduce a new function called configure_retreat_to_charger(std::optional<rmf_traffic::Duration>).

mxgrey · 2024-04-16T04:16:27Z

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp

+  }
+  else
+  {
+    const auto retreat_to_charger_interval_sec =


This parsing logic doesn't seem very sound to me.

This assumes that if a value is given for retreat_to_charger_interval then it will definitely be a double. I believe this line would throw an exception if it's not a double. We should endeavor to return std::nullopt from this function in cases where things cannot be parsed correctly instead of throwing an exception.

Also this treats values of 0.0 as null, which is ... not an unusual convention historically speaking, but I think it's a practice that should be left behind. Instead we should support the user providing a yaml null value for cases where they want to turn off this behavior.

mxgrey · 2024-04-16T04:28:06Z

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp

@@ -45,6 +45,7 @@

 #include <rmf_task_sequence/phases/SimplePhase.hpp>

+#include <iostream>


Seems like this was put in for debugging and not needed anymore.

mxgrey · 2024-04-16T04:30:41Z

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp

@@ -308,7 +314,8 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
    double recharge_threshold,
    double recharge_soc,
    bool account_for_battery_drain,
-    rmf_task::ConstRequestFactoryPtr finishing_request = nullptr);
+    rmf_task::ConstRequestFactoryPtr finishing_request = nullptr,
+    std::optional<rmf_traffic::Duration> retreat_to_charger_interval = std::nullopt);


This default value isn't backwards compatible. An old fleet adapter that was using the API as it used to be will unintentionally turn off the retreat behavior after updating to this version of the library.

Since we have a separate function for modifying the interval, let's just keep this fully separate and remove this argument.

mxgrey · 2024-04-16T04:39:31Z

rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

+  if (_retreat_timer)
+    _retreat_timer->reset();
+  _retreat_timer = _context->node()->try_create_wall_timer(
+    duration.value(),


We're not checking if duration <= 0, which could cause the timer to behave in ways we don't actually want. We should issue an error message in that situation.

mxgrey · 2024-04-16T04:55:14Z

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp

+
+  /// Set whether or not to trigger automatic retreat to charger with a
+  /// duration between checks for automatic retreat.
+  FleetUpdateHandle& retreat_to_charger_interval(


Overloading the function name makes it messy to produce python bindings, so let's add set_ in front of this function.

Out of date

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>

* Add retreat_to_charger param Signed-off-by: Xiyu Oh <xiyu@openrobotics.org> * If idle task is charge, do not retreat to charger Signed-off-by: Xiyu Oh <xiyu@openrobotics.org> * Use duration instead of boolean for retreat to charger Signed-off-by: Xiyu Oh <xiyu@openrobotics.org> * Undo adding request type Signed-off-by: Xiyu Oh <xiyu@openrobotics.org> * Change retreat to timer from bool to optional duration Signed-off-by: Xiyu Oh <xiyu@openrobotics.org> * Store param in FleetUpdateHandle Signed-off-by: Xiyu Oh <xiyu@openrobotics.org> * Uncrustify and remove request_type from ChargeBattery Signed-off-by: Xiyu Oh <xiyu@openrobotics.org> * Tweak some details and remove API breakages Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai> * Fix style Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai> --------- Signed-off-by: Xiyu Oh <xiyu@openrobotics.org> Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai> Co-authored-by: Michael X. Grey <mxgrey@intrinsic.ai> Co-authored-by: Grey <grey@openrobotics.org> Signed-off-by: Arjo Chakravarty <arjoc@google.com>

xiyuoh added 2 commits March 11, 2024 16:04

Add retreat_to_charger param

8dfe658

Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>

If idle task is charge, do not retreat to charger

99d0118

Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>

xiyuoh requested review from mxgrey and Yadunund March 12, 2024 04:34

xiyuoh mentioned this pull request Mar 12, 2024

Add request_type identifier and configurable retreat_to_charger open-rmf/rmf_task#114

Open

Yadunund previously requested changes Mar 12, 2024

View reviewed changes

Use duration instead of boolean for retreat to charger

76ec0ed

Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>

aaronchongth mentioned this pull request Mar 15, 2024

[Bug]: dashboard selectively displays tasks open-rmf/rmf-web#898

Closed

1 task

xiyuoh added 7 commits March 18, 2024 18:47

Merge branch 'main' into xiyu/disable_automatic_retreat

cce8cf1

Undo adding request type

90b5610

Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>

Change retreat to timer from bool to optional duration

2c25e30

Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>

Store param in FleetUpdateHandle

fc59945

Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>

Merge branch 'main' into xiyu/disable_automatic_retreat

c55361a

Merge branch 'main' into xiyu/disable_automatic_retreat

fcb9ff5

Uncrustify and remove request_type from ChargeBattery

052244d

Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>

xiyuoh requested a review from Yadunund April 15, 2024 07:32

Tweak some details and remove API breakages

c6d434e

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>

mxgrey previously approved these changes Apr 16, 2024

View reviewed changes

Fix style

e157415

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>

mxgrey dismissed their stale review via e157415 April 16, 2024 08:09

Merge branch 'main' into xiyu/disable_automatic_retreat

3614bdf

mxgrey approved these changes Apr 17, 2024

View reviewed changes

xiyuoh merged commit 6dfafa0 into main Apr 18, 2024
3 of 4 checks passed

xiyuoh deleted the xiyu/disable_automatic_retreat branch April 18, 2024 02:12

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Disable automatic retreat #330

Disable automatic retreat #330

xiyuoh commented Mar 12, 2024

Yadunund left a comment •

edited

Loading

Yadunund Mar 12, 2024

Yadunund Mar 12, 2024

xiyuoh Mar 12, 2024

Yadunund Mar 12, 2024

Yadunund Mar 12, 2024

Yadunund Mar 12, 2024

xiyuoh commented Apr 15, 2024

mxgrey left a comment

mxgrey Apr 16, 2024

mxgrey Apr 16, 2024

mxgrey Apr 16, 2024

mxgrey Apr 16, 2024

mxgrey Apr 16, 2024

mxgrey Apr 16, 2024

mxgrey Apr 16, 2024

	mgr->_retreat_timer = mgr->context()->node()->try_create_wall_timer(
	std::chrono::seconds(10),
	[w = mgr->weak_from_this()]()
	{
	if (auto mgr = w.lock())
	{
	mgr->retreat_to_charger();
	}
	});

	bool retreat_to_charger = true);
	std::optional<rmf_traffic::Duration> check_retreat_to_charger_interval = rmf_traffic::time::from_seconds(10));

		@@ -45,6 +45,7 @@

		#include <rmf_task_sequence/phases/SimplePhase.hpp>

		#include <iostream>

Disable automatic retreat #330

Disable automatic retreat #330

Conversation

xiyuoh commented Mar 12, 2024

Yadunund left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

xiyuoh commented Apr 15, 2024

mxgrey left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Yadunund left a comment •

edited

Loading