[arjo129]

New feature implementation

Implemented feature

This PR implements a demo and unit test for docking. Do note that the actual docking implementation is NOT in this PR and the unit test here is supposed to fail!

Implementation description

To run a grid-world gui version of the demo on the web run the following:

ros2 launch rmf_path_server_demo demo_docking.launch.py

You should see something like this when you open up http://localhost:8080

Clicking "Preset: Delayed Dock + Pass" should trigger a demo where robot 1 is about to dock and robot2 receives a delayed command leading to a replan. In this branch a replan will happen for all robots (you'll notice R_1 incorrectly will back outy). This is the incorrect behaviour however as this PR is part of a stack the intent is to show that the behaviour is broken.

We also add an automated test to check for replans in
path_server/rmf_path_server_test/test/test_docking_replan_scenario.py

In this PR the test should fail. I will be fixed in another PR targeting this one.

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: Gemini 4 Argon

[SamuelFoo]

I think the cell center may need a half-cell offset here.

The existing +- half_cell calculates the box edges around (cx, cy), but cx = x as f32 * resolution and cy = y as f32 * resolution refer to the cell's bottom-left corner. ROS defines the map origin at the bottom-left corner of cell (0, 0).

For example, at resolution 1.0, cell (1, 1) currently gets bounds [0.5, 1.5] on each axis instead of [1, 2]. A focused check with a radius-0.05 robot moving from (1.7, 1.75) to (1.9, 1.75) returns no blockage even though the route is inside the occupied cell. The same check passes before this change.

Should we add the offset to the center calculation?

let cx = (x as f32 + 0.5) * resolution;
let cy = (y as f32 + 0.5) * resolution;
let box_min = (cx - half_cell, cy - half_cell);
let box_max = (cx + half_cell, cy + half_cell);

This gives bounds [1, 2] for the example above.

[SamuelFoo]

If this is correct, we should also align the planner and corridor fixture with the ROS map convention.

The corridor test places the robot at (-1, 2), which is the dock cell's corner under that convention. Adding +0.5 therefore makes this test report a blockage. The planner also uses index * resolution + origin.

[SamuelFoo]

Alternatively, I think using cell centers internally would also work. We could shift the route into a cell-center frame and keep the existing obstacle-center calculation.

The issue here is that the route is transformed relative to the ROS map's corner origin, while the obstacle boxes treat that origin as a cell center.