Performing docking using jazzy/efc rmf_demos_fleet_adapter (#631)

Open-RMF Open-RMF General
1

Posted by @cwrx777:

Hi,
I’m trying to test the docking behaviour of the easy full control rmf_demos_fleet_adapter and I have setup the following:
in office.buiding.yaml:

in tinyRobot_config.yaml, add/modify as follows:

rmf_fleet:
  name: "tinyRobot"
  ...
  finishing_request: "nothing" # [park, charge, nothing]
  robots:
    tinyRobot1:
        charger: "tinyRobot1_charger"
        finishing_request: 
          type: "nothing" 
          # waypoint_name: "supplies" 
...
fleet_manager:
  ...
  action_paths:
    dock:
      supplies_dock_name:
        map_name: "L1"
        path: 
          - [7.02, -2.11, -1.7]
          - [6.52, -3.24, -1.7] # the coordinate of `supplies` waypoint
        finish_waypoint: "supplies"

Then I started the task by using:

ros2 run rmf_demos_tasks dispatch_patrol -F tinyRobot -R tinyRobot1 -p supplies --use_sim_time

Log file

As it can be seen in the log file, after the robot moved to the last position in the path specified in the supplies_dock_name dock action_paths, this code executed.
Do I miss anything in my configuration?

[1740984524.609440952][tinyRobot_fleet_adapter]: Executing final go_to_place [supplies] for robot [tinyRobot/tinyRobot1]
[1740984524.610371180][tinyRobot_fleet_adapter]: follow_new_path for robot [tinyRobot/tinyRobot1] with PlanId [0]
...
[1740984564.172617074][tinyRobot_fleet_adapter]: Received a request to dock robot [tinyRobot/tinyRobot1] at [supplies_dock_name]...
...
...
[1740984575.156743768][tinyRobot_fleet_adapter]: Requesting replan for [tinyRobot/tinyRobot1] after finishing a schedule override
...
[1740984575.158092242][tinyRobot_fleet_adapter]: Executing final go_to_place [supplies] for robot [tinyRobot/tinyRobot1]
[1740984575.158518328][tinyRobot_fleet_adapter]: follow_new_path for robot [tinyRobot/tinyRobot1] with PlanId [2]
...
[1740984575.665998576][tinyRobot_fleet_adapter]: Received a request to dock robot [tinyRobot/tinyRobot1] at [supplies_dock_name]...
...
[1740984586.108384706][tinyRobot_fleet_adapter]: Executing final go_to_place [supplies] for robot [tinyRobot/tinyRobot1]
[1740984586.113477337][tinyRobot_fleet_adapter]: follow_new_path for robot [tinyRobot/tinyRobot1] with PlanId [6]
Chosen answer

Answer chosen by @cwrx777 at 2025-03-07T05:07:04Z.
Answered by @xiyuoh:

Hi @cwrx777 ! Thanks for pointing this out, I’m able to reproduce what you’re seeing. I suspect after docking has completed the first time, during the requested replan RMF mistakenly assumed that the robot hasn’t performed docking at all based on its current position, and issued the same navigation command again, leading to a cyclic behavior.

I’ll look into the code to identify if there are any bugs, but a quick way to correct this is to increase the max_merge_waypoint_distance value in the fleet config:

rmf_fleet:
  ...
  responsive_wait: False # Should responsive wait be on/off for the whole fleet by default? False if not specified.
  max_merge_waypoint_distance: 0.02
  use_parking_reservations: True
  ...

I’ve tested out and the value 0.02 seems to work, providing a little more buffer between the dock coordinates and waypoints. The default max_merge_waypoint_distance without configuration is 0.001.

2

Posted by @xiyuoh:

Hi @cwrx777 ! Thanks for pointing this out, I’m able to reproduce what you’re seeing. I suspect after docking has completed the first time, during the requested replan RMF mistakenly assumed that the robot hasn’t performed docking at all based on its current position, and issued the same navigation command again, leading to a cyclic behavior.

I’ll look into the code to identify if there are any bugs, but a quick way to correct this is to increase the max_merge_waypoint_distance value in the fleet config:

rmf_fleet:
  ...
  responsive_wait: False # Should responsive wait be on/off for the whole fleet by default? False if not specified.
  max_merge_waypoint_distance: 0.02
  use_parking_reservations: True
  ...

I’ve tested out and the value 0.02 seems to work, providing a little more buffer between the dock coordinates and waypoints. The default max_merge_waypoint_distance without configuration is 0.001.

This is the chosen answer.

3

Posted by @mxgrey:

What’s really happening is that whenever a schedule override happens, we automatically do a replan when the command is finished because the EasyFullControl API can’t be certain if the overridden path brings the robot to where it was supposed to go.

When that replan is triggered, the EasyFullControl API believes the robot is still in the docking lane rather than arrived at the waypoint because the waypoint merge radius is too small to automatically declare the robot as being on the waypoint. Since the robot is believed to be on the docking lane and not at the waypoint, doing the replan will prompt the docking sequence to run again.

There are two options that I would recommend for resolving this: