[Bug]: Dock method is called twice when planned from one lane away on bi-directional lane

[cwrx777]

Before proceeding, is there an existing issue or discussion for this?

• I have done a search for similar issues and discussions.

OS and version

Ubuntu 22.04

Open-RMF installation type

Source build

Other Open-RMF installation methods

No response

Open-RMF version or commit hash

5dc339c

ROS distribution

Humble

ROS installation type

Source build

Other ROS installation methods

No response

Package or library, if applicable

No response

Description of the bug

Dock method is called twice when planned from one lane away on bi-directional lane.
Could it be due to this open-rmf#111?

Steps to reproduce the bug

rmf_ros2, rmf_traffic: main branch
rmf_demos: humble

1. Edit source to log planned path:

  const auto& graph = _context->navigation_graph();
  std::stringstream ss;
  auto waypoints = plan.get_waypoints();
  const auto t0 = waypoints.front().time();
  for (const auto& wp : waypoints)
  {
    ss << "\n -- " << agv::print_plan_waypoint(wp, graph, t0);
  }

  RCLCPP_INFO(
    _context->node()->get_logger(),
    "[%s]: "
    "Details:\n%s",
    _context->requester_id().c_str(),
    ss.str().c_str());

2. Edit office map and add dock_name to tinyRobot1_charger

3. Create mock docker config file, e.g.

   tinyRobot:
     tinyRobot1_charger:
       level_name: "L1"
         path: 
           - [10.0824,-6.9756, 1.50 ]
           - [10.4295,-5.5703, 1.50 ]
       finish_waypoint: "tinyRobot1_charger"

4. Edit office.launch.xml in rmf_demos package and add launch configuration for mock_docker

    <!-- Mock Docker Node, to provide Fleet Adapter paths for docking -->
    <group>
       <let name="docking_config_file" 
   value="/path/to/office_docker_config.yaml"/>
      <node pkg="rmf_demos_tasks" exec="mock_docker" args="-c $(var docking_config_file)">
        <param name="use_sim_time" value="$(var use_sim_time)"/>
      </node>
    </group>

5. Launch office sim.

6. Send task to 'patrol_A1' and 'tinyRobot1_charger':

   ros2 run rmf_demos_tasks dispatch_patrol -F tinyRobot -R tinyRobot1 -p patrol_A1 tinyRobot1_charger --use_sim_time

   output:

   [fleet_adapter-17] [tinyRobot_fleet_adapter]: [tinyRobot/tinyRobot1]: Details:
   [fleet_adapter-17] 
   [fleet_adapter-17]  -- t=0 < 10.1136 -6.86811  1.32862> yaw=76.1245 approach lanes:
   [fleet_adapter-17]  -- t=5 < 10.1136 -6.86811  1.32862> yaw=76.1245 event [Dock tinyRobot1_charger] approach lanes:
   [fleet_adapter-17]  -- t=10.536 < 10.1136 -6.86811 -1.81297> yaw=-103.876 event [Dock tinyRobot1_charger] approach lanes:
   [fleet_adapter-17]  -- t=13.8664 #16 < 10.4331  -5.5751 -1.81297> yaw=-103.876 approach lanes: 26
   

7. As a comparison, send another task, to 'patrol_D2' and 'tinyRobot1_charger':

   ros2 run rmf_demos_tasks dispatch_patrol -F tinyRobot -R tinyRobot1 -p patrol_D2 tinyRobot1_charger --use_sim_time

   output:

   [fleet_adapter-17] [tinyRobot_fleet_adapter]: [tinyRobot/tinyRobot1]: Details:
   [fleet_adapter-17] 
   [fleet_adapter-17]  -- t=0 < 10.2185 -3.16058  1.16615> yaw=66.8156 approach lanes:
   [fleet_adapter-17]  -- t=0 < 10.2185 -3.16058  1.16254> yaw=66.6088 approach lanes:
   [fleet_adapter-17]  -- t=7.2492 #6 < 8.91186 -6.18135  1.16254> yaw=66.6088 approach lanes: 8
   [fleet_adapter-17]  -- t=9.85274 #6 < 8.91186 -6.18135  2.54467> yaw=145.799 approach lanes:
   [fleet_adapter-17]  -- t=13.3591 #7 < 10.0861 -6.97944  2.54467> yaw=145.799 approach lanes: 10
   [fleet_adapter-17]  -- t=15.6858 #7 < 10.0861 -6.97944  1.32862> yaw=76.1243 event [Dock tinyRobot1_charger] approach lanes:
   [fleet_adapter-17]  -- t=24.2456 #16 <10.4331 -5.5751 1.32862> yaw=76.1243 approach lanes: 26
   

Expected behavior

There should only be one dock event

Actual behavior

There are two dock events.

Additional information or screenshots

No response

[kjchee]

i found that this issue occurs when the robot's startset is on a lane with entry event,
eg.

[fleet_adapter-18] [INFO] [1760952150.358930918] [tinyRobot_fleet_adapter]: Planning for [tinyRobot/tinyRobot1] to [tinyRobot1_charger] from one of these locations:                    
[fleet_adapter-18]  -- lane 0: { L1 < 10.0861 -6.97944> [patrol_A1] event Dock tinyRobot1_charger } -> { L1 <10.4331 -5.5751> [tinyRobot1_charger] } | location < 10.0892 -6.96699> | or
ientation 76.1467               

And the root cause is that the entry_event is applied twice,

1. at the start node, at expand_start() here.

2. at subsequent node while expanding traversal (lane patrol_A1--tinyRobot1_charger with entry event Dock) at expand_traversal() here.

I don’t think this is a bug per se — it seems to be intentional design? eg: When a robot start on a lane, it will always perform the entry_event (eg, LiftDorrOpem, LiftSessionBegin, DoorOpen, LiftMove, Dock).

But Dock is a special case, executing it twice can lead to funny behavior (unlike door open triggering twice no problem).

To optimize it, do we perform filtering at

1. upstream at expand_traversal (to check if parent node and current node have same entry event then skip adding to current node), or at reconstruct_waypoints(), or
2. downstream at execute_plan,cpp (skip dock's execute() if duplicate event detected)?

[mxgrey]

Thanks for tracking down that root cause.

I think the code as-written is definitely intentional, but there are arguably two deeper problems happening at the same time:

Problem 1: The Start conditions given to the planner are not set correctly

The waypoint field is ideally the waypoint that the robot is currently on.

However if the robot is not "on" a waypoint right now (e.g. it is near the initial waypoint but not "on" it) then you can add a location offset. The robot will first move from that location to the initial waypoint, and then it will traverse the graph from there.

But if your robot is more specifically in the middle of some lane then you can specify both a location and a lane. The robot will first traverse to the initial waypoint, along the specified lane, applying any relevant lane constraints along the way, including lane entry and exit events.

If the start information is set correctly then a double-event should not happen. Consider this very simple diagram:

If the robot starts in between vertices A and B then there are two valid items for the start set:

# Start 1
waypoint: B
lane: 1
location: [x, y]

# Start 2
waypoint: A
lane: 2
location: [x, y]

With a start set that includes both of the above, there's no way to get a double-docking situation:

1. For Start 1 the expand_start will apply the docking event of lane 1, and the expansion will end at waypoint B. From there expand_traversal can only send it to waypoint A along lane 2, which has no docking event.

2. For Start 2 the expand_start will send the robot to waypoint A along lane 2 which has no docking event. Once the robot arrives at waypoint A, expand_traversal might send the robot to waypoint B along lane 1 if waypoint B is the goal. In that case the lane event is still only applied once: after the robot has finishing moving to A.

It sounds like we're giving the planner a start set that includes

# Bad Start
waypoint: A
lane: 1
location: [x, y]

In that case the lane event will be applied when "traversing" to waypoint A to begin the plan, and then it will get applied again because the planner believes we now need to traverse from waypoint A to waypoint B, and that can only be done along lane 1.

I'd suggest we figure out where this bad start condition is being created and figure out how to prevent it from being created.

Problem 2: The planner naively accepts any waypoint + lane combo given to it

Currently the planner makes no effort to check whether the initial lane setting is consistent with the initial waypoint setting when expanding from the start conditions. We could consider adding a condition to check whether the initial lane will actually lead to the initial waypoint, as the planner requires. We could consider one of the following options:

1. Just skip Start conditions where initial lane is inconsistent with initial waypoint
2. When the initial lane is set, the planner ignores the user's setting for initial waypoint and instead just moves the robot to the exit waypoint of the initial lane, no matter what they set the initial waypoint to.

I think (2) will probably give us better and less surprising results, although I worry that it will cover up mistakes that are being made by downstream users.

[kjchee]

hi @mxgrey, i follow your advise and tried to see whats wrong with the 'start'. And surprisingly, nothing wrong. The 'start' is one of the 'correct start' you mentioned above, but still we will see duplicate entry_events.

Below is the diagram illustrates waypoint index & lane index with entry event, and the 'start' passed to the planner, and during the test i sent robot from patrol_A1 to tinyRobot1_charger:

Startset:
-- lane 0: { L1 < 10.0861 -6.97944> [patrol_A1] event Dock tinyRobot1_charger } -> { L1 <10.4331 -5.5751> [tinyRobot1_charger] } | location < 10.0892 -6.96699> | orientation 76.1467

Then I tried to debug who generates these duplicate events. I look at the Nodes generated by make_start_node(), expand_start(), expand_traversal(). The diagram below is the const std::vector<NodePtr>& node_sequence passed into function reconstruct_waypoints() at here:

ok, so far all good, no issues, all the nodes are correct, no duplicate events.

Then, the duplication happened in reconstruct_waypoints() at here, when there is a change of position & orientation (from Node 1 to Node 2), then reconstruct_waypoints() will push_back new candidate which has same position with Node 1 but has orientation of Node 2, so it can rotate in place towards next node's orientation. But, also at here, Node 1's event is copied over to this new candidate.

This is the final candidates in reconstruct_waypoints():

------- [reconstruct_waypoints] candidate waypoint [0] details:
position= 10.0892 -6.96699  1.32901 time=35.7 graph_index=none approach_lanes=[] arrival=[(0, 0) ] event=none

------- [reconstruct_waypoints] candidate waypoint [1] details:
position= 10.0892 -6.96699  1.32901 time=40.7 graph_index=none approach_lanes=[] arrival=[(0, 1) ] event=has event

------- [reconstruct_waypoints] candidate waypoint [2] details:
position= 10.0892 -6.96699 -1.81297 time=40.7 graph_index=none approach_lanes=[] arrival=[] event=has event

------- [reconstruct_waypoints] candidate waypoint [3] details:
position= 10.4331  -5.5751 -1.81297 time=49.7695 graph_index=1 approach_lanes=[0 ] arrival=[(0, 7) ] event=none

candidate waypoint [2] is the one with duplicate event and it is copied from candidate waypoint [1]'s.

[mxgrey]

I see, so the real problem was in reconstruct_waypoints all along. It's still worth considering adding the original "fix" since it's still possible that could become a bug in the future, but this reconstruction_waypoints bug is obviously more important. That particular part of the code is less than a year old, which helps explain why we haven't observed this until recently.

At first I thought we could remove prev_wp.event from this line, but that will lead to a major issue: If a docking event is meant to happen then that would first issue a command to perform the docking, and then after the docking is done it would command the robot back to the start for the rotate-in-place. So this approach is no good.

I can think of two approaches that should work, with just slightly different behavior:

1. After setting prev_wp.event we nullify the event of candidates[i-1]. I find this a bit unappealing because now we risk introducing with off-by-one errors, but if we're very meticulous about it then it should work fine.
2. If prev_wp.event is not null then we skip this rotate-in-place entirely. Or we check specifically whether the event is a docking event and then skip the rotate-in-place. This would be the simpler solution, but it's a bit brute force (which isn't necessarily bad).

[kjchee]

It's still worth considering adding the original "fix" since it's still possible that could become a bug in the future

Sure, I'll work on this afterwards.

2. If prev_wp.event is not null then we skip this rotate-in-place entirely. Or we check specifically whether the event is a docking event and then skip the rotate-in-place. This would be the simpler solution, but it's a bit brute force (which isn't necessarily bad).

For this method, will there be a case where user/SI wants the robot's pre-dock orientation follows the docking lane's ["orientation_constraint"] (Constraint::Direction::Backward or Constraint::Direction::Forward) they set in the nav_graph? If yes, then the robot will not be in correct orientation while starting Dock event.