full-control traffic participants negotiation not started

[txlei]

Hi, would like to explain a test scenario where a robot remain in conflicted state indefinitely.

The rmf_demo office world is modified to have an additional waypoint "block" in the middle of the lane. It also include 2 fleets with one robot each. The purpose was to test the response of the current traffic management system when an idle robot (tinyRobot1) at the block waypoint, is in the way of another robot (mobot1). mobot1 has a loop request between station_1 and cubicle_1.

My predicament was that tinyRobot1 would still participate in negotiation, although its idle/ has no other tasks, and move to another waypoint eg. tinyRobot1_charger, out of the way of mobot1’s path and continue staying idle. Alternatively, even if tinyRobot1 does not want to negotiate, mobot1 could reroute. Instead of turning left, it would turn right and go the longer way to station_1 and then cubicle_1.

However, none of the two possible solution was found by the traffic system. In the log below, it seemed that there were no active negotiation from the rmf_traffic_schedule as well. Though mobot is aware there was an "interruption" and tried to replan, the green path from the visualizer says otherwise. Does mobot knows that there is a possible "conflict"? Could you explain about the behavior exhibited here?

[rmf_traffic_schedule-1] [INFO] [1614236648.231527437] [rmf_traffic_schedule_node]: Registered participant [0] named [tinyRobot1] owned by [tinyRobot]
[full_control-15] [INFO] [1614236648.231980961] [tinyRobot_fleet_adapter]: Added a robot named [tinyRobot1] with participant ID [0]
[gzclient-14] [INFO] [1614236648.241777278] [gazebo_ros_node]: ROS was initialized without arguments.
[rviz2-4] [INFO] [1614236648.293838101] [rviz_node]: Received map "building" containing 1 level(s)
[rviz2-4] [INFO] [1614236648.296479866] [rviz_node]: Level cache updated
[rviz2-4] [INFO] [1614236648.297706628] [rviz_node]: Loading floorplan Image: officepng
[rviz2-7] [INFO] [1614236648.506422150] [rviz2]: Trying to create a map of size 3047 x 1717 using 1 swatches
[rviz2-7] [ERROR] [1614236648.528289712] [rviz2]: Vertex Program:rviz/glsl120/indexed_8bit_image.vert Fragment Program:rviz/glsl120/indexed_8bit_image.frag GLSL link result : 
[rviz2-7] active samplers with a different type refer to the same texture image unit
[rmf_traffic_schedule-1] [INFO] [1614236648.736354016] [rmf_traffic_schedule_node]: Registered participant [1] named [mobot1] owned by [mobot]
[full_control-17] [INFO] [1614236648.736789780] [mobot_fleet_adapter]: Added a robot named [mobot1] with participant ID [1]
[rviz2-7] [INFO] [1614236649.319288177] [door_panel_requester_node]: New door [coe_door] found, refreshing...
[rviz2-7] [INFO] [1614236649.573057121] [door_panel_requester_node]: New door [hardware_door] found, refreshing...
[rviz2-7] [INFO] [1614236649.609952950] [door_panel_requester_node]: New door [main_door] found, refreshing...
[rmf_task_dispatcher-10] [INFO] [1614236675.539381431] [rmf_dispatcher_node]: Received Task Submission [Loop0]
[rmf_task_dispatcher-10] [INFO] [1614236675.539446353] [rmf_dispatcher_node]: Add Task [Loop0] to a bidding queue
[api_server-11] [INFO] [1614236675.585978826] [dispatcher_client]: New Dispatch task_id Loop0
[full_control-17] [INFO] [1614236675.642768175] [mobot_fleet_adapter]: Fleet [mobot] does not have a named waypoint [station_1_1] configured in its nav graph. Rejecting BidNotice with task_id:[Loop0]
[full_control-15] [INFO] [1614236675.644437692] [tinyRobot_fleet_adapter]: Generated Loop request for task_id:[Loop0]
[full_control-15] [INFO] [1614236675.644543679] [tinyRobot_fleet_adapter]: Planning for [1] robot(s) and [1] request(s)
[full_control-15] [INFO] [1614236675.646114282] [tinyRobot_fleet_adapter]: Submitted BidProposal to accommodate task [Loop0] by robot [tinyRobot1] with new cost [39.633343]
[rmf_task_dispatcher-10] [INFO] [1614236677.842412347] [rmf_dispatcher_node]: Determined winning Fleet Adapter: [tinyRobot], from 1 submissions
[rmf_task_dispatcher-10] [INFO] [1614236677.842537491] [rmf_dispatcher_node]: Dispatcher Bidding Result: task [Loop0] is accepted by fleet adapter [tinyRobot]
[full_control-15] [INFO] [1614236677.843011373] [tinyRobot_fleet_adapter]: Bid for task_id:[Loop0] awarded to fleet [tinyRobot]. Processing request...
[full_control-15] [INFO] [1614236677.854439271] [tinyRobot_fleet_adapter]: Beginning new task [Loop0] for [tinyRobot/tinyRobot1]. Remaining queue size: 0
[full_control-15] [INFO] [1614236677.856058074] [tinyRobot_fleet_adapter]: Assignments updated for robots in fleet [tinyRobot] to accommodate task_id:[Loop0]
[rmf_task_dispatcher-10] [INFO] [1614236677.856608538] [rmf_dispatcher_node]: Received dispatch ack from fleet [tinyRobot] that task [Loop0] is queued
[gzserver-13] [INFO] [1614236677.869674184] [slotcar_tinyRobot1]: tinyRobot1 received a path request with 6 waypoints
[gzserver-13] [INFO] [1614236689.052915284] [slotcar_tinyRobot1]: tinyRobot1 reached waypoint 1/5
[gzserver-13] [INFO] [1614236689.767410313] [slotcar_tinyRobot1]: tinyRobot1 reached waypoint 2/5
[gzserver-13] [INFO] [1614236697.265099580] [slotcar_tinyRobot1]: tinyRobot1 reached waypoint 3/5
[gzserver-13] [INFO] [1614236697.266217497] [slotcar_tinyRobot1]: tinyRobot1 reached waypoint 4/5
[gzserver-13] [INFO] [1614236705.108575469] [slotcar_tinyRobot1]: tinyRobot1 reached waypoint 5/5
[gzserver-13] [INFO] [1614236705.108621865] [slotcar_tinyRobot1]: tinyRobot1 reached goal -- rotating to face target
[gzserver-13] [INFO] [1614236705.221746699] [slotcar_tinyRobot1]: tinyRobot1 received a path request with 6 waypoints
[gzserver-13] [INFO] [1614236710.785633831] [slotcar_tinyRobot1]: tinyRobot1 reached waypoint 1/5
[gzserver-13] [INFO] [1614236711.255306926] [slotcar_tinyRobot1]: tinyRobot1 reached waypoint 2/5
[gzserver-13] [INFO] [1614236718.710670831] [slotcar_tinyRobot1]: tinyRobot1 reached waypoint 3/5
[gzserver-13] [INFO] [1614236718.711596287] [slotcar_tinyRobot1]: tinyRobot1 reached waypoint 4/5
[gzserver-13] [INFO] [1614236724.642626418] [slotcar_tinyRobot1]: tinyRobot1 reached waypoint 5/5
[gzserver-13] [INFO] [1614236724.642667890] [slotcar_tinyRobot1]: tinyRobot1 reached goal -- rotating to face target
[rmf_task_dispatcher-10] [INFO] [1614236724.878906536] [rmf_dispatcher_node]: Received status from fleet [tinyRobot], task [Loop0] is now terminated
[rmf_task_dispatcher-10] [INFO] [1614236763.019082717] [rmf_dispatcher_node]: Received Task Submission [Loop1]
[rmf_task_dispatcher-10] [INFO] [1614236763.019137774] [rmf_dispatcher_node]: Add Task [Loop1] to a bidding queue
[api_server-11] [INFO] [1614236763.020780748] [dispatcher_client]: New Dispatch task_id Loop1
[full_control-15] [INFO] [1614236763.045106564] [tinyRobot_fleet_adapter]: Fleet [tinyRobot] does not have a named waypoint [station_1_2] configured in its nav graph. Rejecting BidNotice with task_id:[Loop1]
[full_control-17] [INFO] [1614236763.046383588] [mobot_fleet_adapter]: Generated Loop request for task_id:[Loop1]
[full_control-17] [INFO] [1614236763.046464146] [mobot_fleet_adapter]: Planning for [1] robot(s) and [1] request(s)
[full_control-17] [INFO] [1614236763.049515180] [mobot_fleet_adapter]: Submitted BidProposal to accommodate task [Loop1] by robot [mobot1] with new cost [328.095974]
[rmf_task_dispatcher-10] [INFO] [1614236765.044897418] [rmf_dispatcher_node]: Determined winning Fleet Adapter: [mobot], from 1 submissions
[rmf_task_dispatcher-10] [INFO] [1614236765.044985532] [rmf_dispatcher_node]: Dispatcher Bidding Result: task [Loop1] is accepted by fleet adapter [mobot]
[full_control-17] [INFO] [1614236765.045256750] [mobot_fleet_adapter]: Bid for task_id:[Loop1] awarded to fleet [mobot]. Processing request...
[full_control-17] [INFO] [1614236765.048339422] [mobot_fleet_adapter]: Beginning new task [Loop1] for [mobot/mobot1]. Remaining queue size: 0
[full_control-17] [INFO] [1614236765.049082454] [mobot_fleet_adapter]: Assignments updated for robots in fleet [mobot] to accommodate task_id:[Loop1]
[rmf_task_dispatcher-10] [INFO] [1614236765.049246964] [rmf_dispatcher_node]: Received dispatch ack from fleet [mobot] that task [Loop1] is queued
[gzserver-13] [INFO] [1614236765.055028677] [slotcar_mobot1]: mobot1 received a path request with 10 waypoints
[gzserver-13] [INFO] [1614236771.529236034] [slotcar_mobot1]: mobot1 reached waypoint 1/9
[gzserver-13] [INFO] [1614236771.530196779] [slotcar_mobot1]: mobot1 reached waypoint 2/9
[gzserver-13] [INFO] [1614236778.438207696] [slotcar_mobot1]: mobot1 reached waypoint 3/9
[gzserver-13] [INFO] [1614236778.439132660] [slotcar_mobot1]: mobot1 reached waypoint 4/9
[gzserver-13] [INFO] [1614236783.006887839] [slotcar_mobot1]: Stopping [mobot1] to avoid a collision
[full_control-17] [INFO] [1614236794.874088229] [mobot_fleet_adapter]: Replanning for [mobot/mobot1] because of an interruption
[gzserver-13] [INFO] [1614236794.876509965] [slotcar_mobot1]: mobot1 received a path request with 6 waypoints
[full_control-17] [INFO] [1614236808.976787181] [mobot_fleet_adapter]: Replanning for [mobot/mobot1] because of an interruption
[gzserver-13] [INFO] [1614236808.981328950] [slotcar_mobot1]: mobot1 received a path request with 8 waypoints
[gzserver-13] [INFO] [1614236808.982244855] [slotcar_mobot1]: No more obstacles; resuming course for [mobot1]
[gzserver-13] [INFO] [1614236812.876535551] [slotcar_mobot1]: mobot1 reached waypoint 1/7
[gzserver-13] [INFO] [1614236812.877538643] [slotcar_mobot1]: mobot1 reached waypoint 2/7
[gzserver-13] [INFO] [1614236815.006815182] [slotcar_mobot1]: Stopping [mobot1] to avoid a collision
[full_control-17] [INFO] [1614236830.083403929] [mobot_fleet_adapter]: Replanning for [mobot/mobot1] because of an interruption
[gzserver-13] [INFO] [1614236830.085584596] [slotcar_mobot1]: mobot1 received a path request with 6 waypoints
[full_control-17] [INFO] [1614236840.139083370] [mobot_fleet_adapter]: Replanning for [mobot/mobot1] because of an interruption
[gzserver-13] [INFO] [1614236840.141604759] [slotcar_mobot1]: mobot1 received a path request with 7 waypoints
[gzserver-13] [INFO] [1614236840.142458815] [slotcar_mobot1]: No more obstacles; resuming course for [mobot1]
[gzserver-13] [INFO] [1614236844.005926681] [slotcar_mobot1]: mobot1 reached waypoint 1/6
[gzserver-13] [INFO] [1614236846.143904199] [slotcar_mobot1]: Stopping [mobot1] to avoid a collision

[mxgrey]

What you've described is actually the expected behavior for this scenario, although I understand that it's not desirable for your use case.

To elaborate on that, there is an implicit assumption in the current design of the full control fleet adapter that when a robot goes idle, it no longer needs to participate in traffic negotiation. The idea is that a robot should only be allowed to go idle when it is in a designated location that will not conflict with the traffic of other robots. However, the fleet adapter does not currently enforce that assumption because the question of how to enforce it in a generalized way is tricky, and this particular concern hasn't been a top priority yet, because we currently just rely on the system integrators to command their robots to only go idle in reasonable places.

We do have several ideas for how to solve this, which we are actively working on:

1. Before a robot goes idle, the full control fleet adapter should automatically command it to go park in designated parking spot, to keep it out of the way of other robots.
2. If a robot does need to wait at a waypoint that puts it in conflict with other traffic, that waiting should be handled as an explicit ActiveWaiting task instead of leaving the robot completely idle. The ActiveWaiting task would be responsible for keeping the robot on the desired waypoint as much as possible while still having the robot move over to allow other robots to pass.

These features are not available yet, but they are under active development. I don't have a timeline for either of them currently, but I would expect to see them within the next few months.

[mxgrey]

I've added an issue to track the new feature: #304

[mxgrey]

@txlei, with the introduction of #308 you should now get the behavior that you desired for this scenario.