[Bug]: Robot not able to complete delivery task with obstacle present

[Jerrybaoyilei]

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

Binaries

Other Open-RMF installation methods

No response

Open-RMF version or commit hash

2.1.2

ROS distribution

Iron

ROS installation type

Binaries

Other ROS installation methods

No response

Package or library, if applicable

No response

Description of the bug

rmf_obstacle_detector_laserscan and rmf_obstacle_ros2 are working fine, since obstacles are published to /rmf_obstacles and lane close requests are sent by the lane blocker node. I can also see the green marker in Rviz for the obstacle.

However, when I ran the rmf demo office, placed a box at the same location as shown in the gif in the readme for this repo, and sent a delivery task, the delivery task is never completed.

Also, the green future path for robots performing delivery are not present when I am running the laserscan detector node and the lane blocker nodes (the green lines indicating which paths the robot will take)

In Rviz, I don't see lane closing either. In terminal where I run ros2 run rmf_demos_gz office.launch.xml, I see strange errors like [fleet_adapter-16] Other error for tinyRobot1 in get_data: 'robot_name'.

I have also attached some of the output that may be related to this behavior:

[ign-17] [GUI] [Wrn] [Model.hh:98] Unable to deserialize sdf::Model
[fleet_adapter-16] Starting fleet adapter...
[fleet_adapter-16] Other error for tinyRobot1 in get_data: 'robot_name'
[fleet_adapter-16] Other error for tinyRobot2 in get_data: 'robot_name'
[fleet_adapter-16] Other error for tinyRobot1 in get_data: 'robot_name'
[fleet_adapter-16] Other error for tinyRobot2 in get_data: 'robot_name'
[fleet_adapter-16] Other error for tinyRobot1 in get_data: 'robot_name'
[fleet_adapter-16] Other error for tinyRobot2 in get_data: 'robot_name'
[fleet_adapter-16] Other error for tinyRobot1 in get_data: 'robot_name'
[fleet_adapter-16] Other error for tinyRobot2 in get_data: 'robot_name'
[fleet_adapter-16] Other error for tinyRobot1 in get_data: 'robot_name'
[fleet_adapter-16] Other error for tinyRobot2 in get_data: 'robot_name'
[fleet_adapter-16] Other error for tinyRobot2 in get_data: 'robot_name'
[fleet_adapter-16] Other error for tinyRobot1 in get_data: 'robot_name'
[fleet_adapter-16] Traceback (most recent call last):
[fleet_adapter-16]   File "/home/jbao/iron_migration_ws/install/rmf_demos_fleet_adapter/lib/rmf_demos_fleet_adapter/fleet_adapter", line 33, in <module>
[fleet_adapter-16]     sys.exit(load_entry_point('rmf-demos-fleet-adapter==2.1.2', 'console_scripts', 'fleet_adapter')())
[fleet_adapter-16]   File "/home/jbao/iron_migration_ws/install/rmf_demos_fleet_adapter/lib/python3.10/site-packages/rmf_demos_fleet_adapter/fleet_adapter.py", line 155, in main
[fleet_adapter-16]     rclpy_executor.spin()
[fleet_adapter-16]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 295, in spin
[fleet_adapter-16]     self.spin_once()
[fleet_adapter-16]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 748, in spin_once
[fleet_adapter-16]     self._spin_once_impl(timeout_sec)
[fleet_adapter-16]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 745, in _spin_once_impl
[fleet_adapter-16]     raise handler.exception()
[fleet_adapter-16]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/task.py", line 239, in __call__
[fleet_adapter-16]     self._handler.send(None)
[fleet_adapter-16]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 443, in handler
[fleet_adapter-16]     await call_coroutine(entity, *arg)
[fleet_adapter-16]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 367, in _execute_subscription
[fleet_adapter-16]     await await_or_execute(sub.callback, *args)
[fleet_adapter-16]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 108, in await_or_execute
[fleet_adapter-16]     return callback(*args)
[fleet_adapter-16]   File "/home/jbao/iron_migration_ws/install/rmf_demos_fleet_adapter/lib/python3.10/site-packages/rmf_demos_fleet_adapter/fleet_adapter.py", line 434, in lane_request_cb
[fleet_adapter-16]     fleet_handle.open_lanes(msg.open_lanes)
[fleet_adapter-16] AttributeError: 'rmf_adapter.EasyFullControl' object has no attribute 'open_lanes'

[ign-17] [Dbg] [UserCommands.cc:1303] Created entity [351] named [box]
[fleet_adapter-16] [INFO] [1713303125.529852546] [tinyRobot_fleet_adapter]: Requesting new schedule update because update timed out
[fleet_adapter-16] [INFO] [1713303125.530973609] [tinyRobot_fleet_adapter]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since version [24]
[fleet_adapter-16] [WARN] [1713303133.760234842] [tinyRobot_fleet_adapter]: Failed to update using patch for DB version 25 (mirror version: none, patch base: 24); requesting new update
[fleet_adapter-16] [INFO] [1713303133.760648699] [tinyRobot_fleet_adapter]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since beginning of recorded history
[rmf_traffic_schedule-1] [INFO] [1713303133.768550268] [rmf_traffic_schedule_primary]: [ScheduleNode::update_mirrors] Sending remedial update starting from the beginning going to 25 for query 1
[fleet_adapter-16] [INFO] [1713303153.805818564] [tinyRobot_fleet_adapter]: Requesting new schedule update because update timed out
[fleet_adapter-16] [INFO] [1713303153.806690673] [tinyRobot_fleet_adapter]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since version [25]
[fleet_adapter-16] [WARN] [1713303165.523218220] [tinyRobot_fleet_adapter]: Requesting replan for [tinyRobot/tinyRobot2] because its command handle seems to be unresponsive
[fleet_adapter-16] [INFO] [1713303165.524095765] [tinyRobot_fleet_adapter]: Replanning requested for [tinyRobot/tinyRobot2]
[fleet_adapter-16] [INFO] [1713303165.527971424] [tinyRobot_fleet_adapter]: Planning for [tinyRobot/tinyRobot2] to [pantry] from one of these locations:
[fleet_adapter-16]  -- lane 57: { L1 <20.4237 -5.3121> [tinyRobot2_charger] } -> { L1 <  18.729 -3.89598> [patrol_A2] } | location < 19.6362 -4.65402> | orientation -39.8831
[fleet_adapter-16]  -- lane 56: { L1 <  18.729 -3.89598> [patrol_A2] } -> { L1 <20.4237 -5.3121> [tinyRobot2_charger] } | location < 19.6362 -4.65402> | orientation -39.8831
[fleet_adapter-16] [INFO] [1713303165.531617181] [tinyRobot_fleet_adapter]: Executing go_to_place [pantry] for robot [tinyRobot/tinyRobot2]
[fleet_adapter-16] [INFO] [1713303165.547402547] [tinyRobot_command_handle]: Commanding [tinyRobot2] to navigate to [18.72901862 -3.8959816  -1.56726858] on map [L1]: cmd_id 2
[fleet_adapter-16] [WARN] [1713303165.558485292] [tinyRobot_fleet_adapter]: Failed to update using patch for DB version 26 (mirror version: none, patch base: 25); requesting new update
[fleet_adapter-16] [INFO] [1713303165.558679281] [tinyRobot_fleet_adapter]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since beginning of recorded history
[rmf_traffic_schedule-1] [INFO] [1713303165.569608871] [rmf_traffic_schedule_primary]: [ScheduleNode::update_mirrors] Sending remedial update starting from the beginning going to 26 for query 1
[ign-17] [INFO] [1713303165.576954955] [slotcar_tinyRobot2]: [tinyRobot2] already received request [1] -- continuing as normal
[ign-17] [Dbg] [UserCommands.cc:1353] Requesting removal of entity [351]
[fleet_adapter-16] [INFO] [1713303185.613632209] [tinyRobot_fleet_adapter]: Requesting new schedule update because update timed out
[fleet_adapter-16] [INFO] [1713303185.613786758] [tinyRobot_fleet_adapter]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since version [26]
[fleet_adapter-16] [WARN] [1713303193.758850516] [tinyRobot_fleet_adapter]: Failed to update using patch for DB version 27 (mirror version: none, patch base: 26); requesting new update
[fleet_adapter-16] [INFO] [1713303193.759098299] [tinyRobot_fleet_adapter]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since beginning of recorded history
[rmf_traffic_schedule-1] [INFO] [1713303193.767582543] [rmf_traffic_schedule_primary]: [ScheduleNode::update_mirrors] Sending remedial update starting from the beginning going to 27 for query 1

Below is output from lane blocker node, and you can see that lane closing requests are sent:

================================================
Checking collision between: 
Lane: {18.7343,-5.38498,1.56727} [2.97802,0.5]
obs: {18.7205,-5.45436,-1.5708} [0.928293,-0.9283]
Obs rot matrix: 
-3.67321e-06            1
          -1 -3.67321e-06
Obs vertices before trans: {18.2564,-4.99021} {19.1847,-4.99021} {18.2564,-5.9185} {19.1847,-5.9185} 
Lane rot inv matrix: 
0.00352774   0.999994
 -0.999994 0.00352774
Obs vertices after trans: {0.393087,0.479288} {0.396358,-0.449006} {-0.535201,0.476016} {-0.531929,-0.452278} 
Lane box_min: -1.48901,-0.25
Lane box_max: 1.48901,0.25
obs transformed box_min: -0.535201,-0.452278
obs transformed box_max: 0.396358,0.479288
================================================
Checking collision between: 
Lane: {18.7343,-5.38498,1.56727} [2.97802,0.5]
obs: {18.7205,-5.45436,-1.5708} [0.928293,-0.9283]
Obs rot matrix: 
-3.67321e-06            1
          -1 -3.67321e-06
Obs vertices before trans: {18.2564,-4.99021} {19.1847,-4.99021} {18.2564,-5.9185} {19.1847,-5.9185} 
Lane rot inv matrix: 
0.00352774   0.999994
 -0.999994 0.00352774
Obs vertices after trans: {0.393087,0.479288} {0.396358,-0.449006} {-0.535201,0.476016} {-0.531929,-0.452278} 
Lane box_min: -1.48901,-0.25
Lane box_max: 1.48901,0.25
obs transformed box_min: -0.535201,-0.452278
obs transformed box_max: 0.396358,0.479288
Obstacle 0 search in graph for fleet tinyRobot took 27.2187 ms
[INFO] [1713303105.762357271] [lane_blocker_node]: Search concluded with 4 lanes in the vicinity of obstacle rmf_obstacle_detector_laserscan_0
[INFO] [1713303105.763445123] [lane_blocker_node]: There are 4 lanes with changes to the number of obstacles in their vicinity
[INFO] [1713303105.765076838] [lane_blocker_node]: Requested 4 lanes to close for fleet tinyRobot
[INFO] [1713303105.765984774] [lane_blocker_node]: Requested 0 lanes to open for fleet tinyRobot
[INFO] [1713303106.733052440] [lane_blocker_node]: Obstacle rmf_obstacle_detector_laserscan_0 was previously in the vicinity of 4 lanes
================================================
Checking collision between: 
Lane: {19.5764,-4.60404,0.696091} [2.20846,0.5]
obs: {18.7205,-5.45436,-1.5708} [0.928293,-0.9283]
Obs rot matrix: 
-3.67321e-06            1
          -1 -3.67321e-06
Obs vertices before trans: {18.2564,-4.99021} {19.1847,-4.99021} {18.2564,-5.9185} {19.1847,-5.9185} 
Lane rot inv matrix: 
 0.767355  0.641223
-0.641223  0.767355
Obs vertices after trans: {-1.26051,0.550076} {-0.54818,-0.0451742} {-1.85576,-0.162252} {-1.14343,-0.757502} 
Lane box_min: -1.10423,-0.25
Lane box_max: 1.10423,0.25
obs transformed box_min: -1.85576,-0.757502
obs transformed box_max: -0.54818,0.550076
[INFO] [1713303106.734845375] [lane_blocker_node]: Obstacle rmf_obstacle_detector_laserscan_0 is still in the vicinity of lane tinyRobot_56
================================================
Checking collision between: 
Lane: {19.5764,-4.60404,0.696091} [2.20846,0.5]
obs: {18.7205,-5.45436,-1.5708} [0.928293,-0.9283]
Obs rot matrix: 
-3.67321e-06            1
          -1 -3.67321e-06
Obs vertices before trans: {18.2564,-4.99021} {19.1847,-4.99021} {18.2564,-5.9185} {19.1847,-5.9185} 
Lane rot inv matrix: 
 0.767355  0.641223
-0.641223  0.767355
Obs vertices after trans: {-1.26051,0.550076} {-0.54818,-0.0451742} {-1.85576,-0.162252} {-1.14343,-0.757502} 
Lane box_min: -1.10423,-0.25
Lane box_max: 1.10423,0.25
obs transformed box_min: -1.85576,-0.757502
obs transformed box_max: -0.54818,0.550076
[INFO] [1713303106.736324360] [lane_blocker_node]: Obstacle rmf_obstacle_detector_laserscan_0 is still in the vicinity of lane tinyRobot_57
================================================
Checking collision between: 
Lane: {18.7343,-5.38498,1.56727} [2.97802,0.5]
obs: {18.7205,-5.45436,-1.5708} [0.928293,-0.9283]
Obs rot matrix: 
-3.67321e-06            1
          -1 -3.67321e-06
Obs vertices before trans: {18.2564,-4.99021} {19.1847,-4.99021} {18.2564,-5.9185} {19.1847,-5.9185} 
Lane rot inv matrix: 
0.00352774   0.999994
 -0.999994 0.00352774
Obs vertices after trans: {0.393087,0.479288} {0.396358,-0.449006} {-0.535201,0.476016} {-0.531929,-0.452278} 
Lane box_min: -1.48901,-0.25
Lane box_max: 1.48901,0.25
obs transformed box_min: -0.535201,-0.452278
obs transformed box_max: 0.396358,0.479288
[INFO] [1713303106.736845889] [lane_blocker_node]: Obstacle rmf_obstacle_detector_laserscan_0 is still in the vicinity of lane tinyRobot_58
================================================
Checking collision between: 
Lane: {18.7343,-5.38498,1.56727} [2.97802,0.5]
obs: {18.7205,-5.45436,-1.5708} [0.928293,-0.9283]
Obs rot matrix: 
-3.67321e-06            1
          -1 -3.67321e-06
Obs vertices before trans: {18.2564,-4.99021} {19.1847,-4.99021} {18.2564,-5.9185} {19.1847,-5.9185} 
Lane rot inv matrix: 
0.00352774   0.999994
 -0.999994 0.00352774
Obs vertices after trans: {0.393087,0.479288} {0.396358,-0.449006} {-0.535201,0.476016} {-0.531929,-0.452278} 
Lane box_min: -1.48901,-0.25
Lane box_max: 1.48901,0.25
obs transformed box_min: -0.535201,-0.452278
obs transformed box_max: 0.396358,0.479288
[INFO] [1713303106.737418656] [lane_blocker_node]: Obstacle rmf_obstacle_detector_laserscan_0 is still in the vicinity of lane tinyRobot_59
[INFO] [1713303106.737484564] [lane_blocker_node]: There are 0 lanes with changes to the number of obstacles in their vicinity
[INFO] [1713303107.732959356] [lane_blocker_node]: Obstacle rmf_obstacle_detector_laserscan_0 was previously in the vicinity of 4 lanes
================================================

Steps to reproduce the bug

1. Run the ign to ros bridge for lidar/scan and /clock topics
2. run the static transform publisher from fixed frame "map" to lidar model
3. set the publisher in the last step to use sim time by running ros2 param set /lidar_0_tf_publisher use_sim_time true
4. Run a static transform publisher from lidar model to obstacles ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 1 VelodyneHDL-32/top/gpu_lidar VelodyneHDL-32/top/gpu_lidar_obstacles
5. Run the demo office simulation . install/setup.bash ros2 launch rmf_demos_gz office.launch.xml
6. Run and configure the laserscan detector . install/setup.bash ros2 run rmf_obstacle_detector_laserscan laserscan_detector
7. Configure and activate the laserscan detector node ros2 lifecycle set /laserscan_obstacle_detector configure ros2 lifecycle set /laserscan_obstacle_detector activate
8. Run the lane blocker node . install/setup.bash ros2 run rmf_obstacle_ros2 lane_blocker_node
9. Run a delivery task ros2 run rmf_demos_tasks dispatch_delivery -p pantry -ph coke_dispenser -d hardware_2 -dh coke_ingestor --use_sim_time

Expected behavior

Lane blocked by obstacle is closed, and robot is able to complete the delivery task via alternative route

Actual behavior

Lane doesn't show up as closed in Rviz2. Robot stop in the middle of the delivery task and never finish the task.

Additional information or screenshots

No response

[Yadunund]

If you look closely at the logs you shared, the fleet adapter crashes with

[fleet_adapter-16] AttributeError: 'rmf_adapter.EasyFullControl' object has no attribute 'open_lanes'

This was fixed by open-rmf/rmf_demos#220. Please update your rmf_demos repo and try again. I'll close this ticket for now but feel free to reopen if you think it's a different issue.

[Jerrybaoyilei]

Hi @Yadunund , thanks for the reply. I applied the changes in that PR, and now lanes are closing when obstacle is placed in the simulation, and robots are rerouted correctly to avoid the closed lanes. However, when I remove the obstacle, the lanes are not reopened. I am looking into why this is happening