search

open-rmf / rmf_obstacle

Packages that infer the presence of obstacles from sensor inputs.
Apache License 2.0
5 stars 10 forks source link

[Bug]: `rmf_obstacle_detector_laserscan` and `rmf_obstacle_ros2` unable to work with multiple scanners in the simulation (have a temporary fix) #34

Open Jerrybaoyilei opened 4 months ago

Jerrybaoyilei commented 4 months ago

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

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

rmf_obstacle_detector_laserscan, rmf_obstacle_ros2

Description of the bug

Hi @Yadunund , currently, with my setup (ROS 2 Iron binary installation and Open-RMF Iron binary on Ubuntu 22.04), if there are multiple scanners in the simulation, the two packages rmf_obstacle_detector_laserscan and rmf_obstacle_ros2 no longer functions.

(But I have worked out a fix. If you want, I can create a pull request)

Specifically, there will be ghost obstacles showing up. Also, if I add 2 obstacles, each one detectable by only one lidar scanner, then the 2 lanes that should both remain closed will be closed alternatively.

I have written a temporary fix by adding two converter nodes and making adjustments to LaserscanDetector.cpp and LaneBlocker.cpp:

  1. One node subscribes to /lidar/scan and publish to each /lidar/scan/<lidar_scanner_name>.
  2. The other node subscribes to each /rmf_obstacles/<lidar_scanner_name> and publish to /rmf_obstacles.
  3. In LaserscanDetector.cpp, I changed the subscription _scan_sub to subscribe to /lidar/scan/<lidar_scanner_name> instead of /lidar/scan. Also, the publisher _obs_pub publishes to /rmf_obstacles/<lidar_scanner_name> instead of rmf_obstacles.
  4. In LaneBlocker.cpp, I changed the subscription _obstacle_sub from subscribing to rmf_obstacles to subscribing to rmf_obstacles/<lidar_scanner_name>.

With these changes, and run only 1 instance of the two nodes, and for each scanner, have 1 laserscan_detector node and 1 lane_blocker_node running. Then the simulation can handle multiple scanners. The only requirement will be that each scanner has a unique name.

Steps to reproduce the bug

Run the simulation as usual, but have more than 1 lidar scanner in the simulation. This will already be problematic. Then also add more obstacles, some for one scanner and some for the other scanner, and there will be more problems.

Expected behavior

Doesn't matter how many scanners there are in the simulation, the obstacle detection and lane blocking should function normally. When an obstacle is placed in the simulation, it should be detected, and intersecting lanes should be closed. When obstacles are removed, they should disappear from Rviz, and intersecting lanes should be re-opened.

Actual behavior

When the simulation with multiple scanners starts, ghost obstacles will be detected (obstacles show up in Rviz, even though user has added no obstacle at all). When this problem is fixed, and add multiple obstacles to these scanners, lanes won't remain closed.

Additional information or screenshots

No response

Yadunund commented 4 months ago

Thanks for the ticket @Jerrybaoyilei.

I was hoping we could refactor the codebase here as described in https://github.com/open-rmf/rmf_obstacle/issues/22. Specifically implement a new rmf_obstacle_reporter which can load obstacl detectors via pluginlib. This way we can actually just pass multiple Laserscan detectors to the same rmf_obstacle_reporter node. Each detector would report obstacles wrt a unique header frame. That should allow the same behavior with a single lane_blocker_node. Let me know what you think and if you're keen on working on this.

Jerrybaoyilei commented 4 months ago

Hi @Yadunund , thanks for the reply. I have a couple of clarifying questions:

  1. Are there any performance improvements by having a single rmf_obstacle_reporter node and several rmf_obstacle_detector_laserscan plugins attached to this node, over having multiple rmf_obstacle_detector_laserscan nodes? Right now I am taking the latter approach, and I do notice that many times, when I send a delivery task in the demo office simulation, I got "no response" back.
  2. I don't know if you are also thinking about making the LaneBlocker node as a plugin? Because I found that having only one LaneBlocker node creates the problem of lanes closing alternatively instead of simultaneously (which is the expected behavior).

If I understand your approach correctly, I feel like my current approach is like a less refined version of yours, with multiple nodes running instead of single node with multiple plugins. Plus 2 "converter" nodes to convert laserscan topics and obstacle topics to solve the multi scanner problems.