Lane closure

[mxgrey]

New feature implementation

Implemented feature

Allow fleet adapters to close navigation graph lanes.

Implementation description

The rmf_fleet_adapter::agv::FleetUpdateHandle API can now be told to close or open navigation graph lanes during runtime.

For the legacy fleet adapter (aka the full_control adapter) you can send a LaneRequest message to open or close lanes at any time. If the full_control adapter detects that the robot is currently on a lane that was just closed, then it will have the robot retreat back to the start of the lane and find a new plan from there. NOTE: We do not offer very strong guarantees about the effectiveness or reliability of this behavior for the legacy fleet adapter, because the rmf_fleet_msgs API does not always convey as much information as we need to make the behavior reliable.

Dependencies

• Lane closure messages rmf_internal_msgs#5
• Lane closure rmf_traffic#11

[mxgrey]

I've added some CLI tools for opening and closing lanes.

Using open-rmf/rmf_demos#13 it's easy to test the lane closure feature with the following commands (using a separate terminal for each):

$ ros2 launch rmf_demos office.launch.xml
$ ros2 run rmf_demos_tasks dispatch_loop -s tinyRobot1_charger -f pantry -n 20 --use_sim_time

Once that is running, you can use

$ ros2 run rmf_fleet_adapter close_lanes tinyRobot <lanes>

to close <lanes> (replace with the indices of the lanes you want to close) and then use

$ ros2 run rmf_fleet_adapter open_lanes tinyRobot <lanes>

to reopen <lanes>.

For this demo, you can use the illustration below to pick which lanes to open and close and watch the result. Note that each "lane" is unidirectional, so closing lane 30 will block the robot from going east while closing lane 31 will block the robot from going west.

If you close the lane that the robot is currently on, the fleet adapter should instruct it to return to the start of the lane and find a new path from there.

[Yadunund]

Tested and works well! I know this is expected behaviour but when a robot is moving towards a goal and all lanes to that goal are closed, the Planner will fail to find a solution and full_control will crash:
[full_control-15] [SearchForPath] CRITICAL ERROR: Impossible plan requested! Participant [tinyRobot_1] owned by [tinyRobot] Requested path (4) --> (2)

Just wondering if the fleet adapter should do some kind of check to see if the requested lane closure will lead to this type of error and hence not close the lane?

I was also planning to open a ticket in rmf_demos to add scripts to open/close lanes but I see that they have been added here directly 👍🏼

[mxgrey]

Just wondering if the fleet adapter should do some kind of check to see if the requested lane closure will lead to this type of error and hence not close the lane?

I think the fleet adapter needs to respect lane closing commands, because it may be the case that sending the robot down a closed lane would be harmful to the robot or others.

Instead we need to improve the error handling pipeline for critical task failures. I've added a ticket #16 for this.

[Yadunund]

Agreed, that is a better approach.

Also, maybe in a future PR we can have FleetUpdateHandle publish to /closed_lanes (akin to how it publishes to /fleet_states)? Assuming we have fleets integrated with the new fleet adapter APIs, this information will not be broadcasted.

[mxgrey]

Also, maybe in a future PR we can have FleetUpdateHandle publish to /closed_lanes (akin to how it publishes to /fleet_states)? Assuming we have fleets integrated with the new fleet adapter APIs, this information will not be broadcasted.

Yeah, I wanted to save that for later to give us time to decide for sure how we want to approach that before committing to that particular topic/message long-term.