Local and global costmaps are not published in multi-robot example

[simonchamorro]

Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• ROS2 Version:
  • Foxy
• Version or commit hash: foxy-devel
  • foxy-devel branch
• DDS implementation:
  • Fast-RTPS

Steps to reproduce issue

Run the multi-robot example

ros2 launch nav2_bringup multi_tb3_simulation_launch.py

Expected behavior

Nav2 stack works and local and global costmaps get updated just like in the single robot example.

Actual behavior

For both robots, the nav stack works, but the local and global costmaps are not being published.

Additional information

[SteveMacenski]

Did you change the topics to be the right namespaced topic with compatible QoS settings?

[simonchamorro]

I think they are already namespaced. Is this what you mean? https://github.com/ros-planning/navigation2/blob/10650836e639f6c6cdb529f78a049dd0623ee46a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml#L167

[SteveMacenski]

Did you verify that the rviz configuration is setup to visualize it? This might be something better to ask on ROS Answers.

[simonchamorro]

Well I am using the example script from nav2_bringup, so the config is from there and it should be set up correctly since it's the same config as the single robot example but namespaced. I thought I'd post here since it is the example launch file with no modifications that is not working. Also, the bots collide when I send goals that are at the same spot, so I don't think the problem is in rviz since the planners are not avoiding obstacles.

[SteveMacenski]

I'm not sure what to tell you then, you'd have to do some minimal debugging and let me know what you think is wrong. There are other users using it that haven't reported issues in Foxy.

[padhupradheep]

@SteveMacenski I will look into this issue, once #2332 is over!

I also tried it in Foxy and I'm also facing the same problem.

[SteveMacenski]

Awesome! Thanks! 😄 another thing I wouldn't have gotten to for a long time (if ever)

[simonchamorro]

The only thing I have been able to find, by inspecting the nodes with ros2 node info, is that the nodes /robotX/local_costmap/local_costmap_rclcpp_node do not subscribe to any laserscan message, while in the single robot demo /local_costmap/local_costmap_rclcpp_node subscribes to /scan. Any ideas on what part of code to look at in particular?

[SteveMacenski]

Did you change the scan topic to be the appropriate namespaced topic that the robot is actually publishing?

[simonchamorro]

Yes, they are namespaced. I also tried remapping /robot1/scan to /scan, but it didn't subscribe to that either.
This is the subscriber part of the output from the single robot example for ros2 node info /local_costmap/local_costmap_rclcpp_node.

Subscribers:
    /clock: rosgraph_msgs/msg/Clock
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /scan: sensor_msgs/msg/LaserScan

And this is the output from the multi robot for /robot1/local_costmap/local_costmap_rclcpp_node. I guess that even if the namespacing was wrong, it would at least show a subscriber trying to listen to /scan instead of not having one at all?

Subscribers:
    /clock: rosgraph_msgs/msg/Clock
    /parameter_events: rcl_interfaces/msg/ParameterEvent

[SteveMacenski]

I seem to remember when we were making this that the forward / on the topic names were important for the multirobot use-case due to the way we had to push topics and nodes into namespaces at launch and configuration. What I'm wondering now is if we did something either in how we created our internal costmap_2d nodes, how the launch system is pushing into namespaces, or something in ROS2 core itself changed such that the / is somehow messing up that remapping.

So I'd try removing that forward / and doing some testing again. If that doesn't work for both scan / namespaced scan, then I'd start looking into the node that is subscribing to that scan, where it was created, with what settings, and if that's changed within the last year or so. If that's not the issue, then its worth looking into the node graph at runtime and the launch system to see if there's something odd going wrong.

[Yousseflah]

Hi guys,
The solution is to replace the start_gazebo_cmd in the multi robot launch file with the following:

start_gazebo_cmd = ExecuteProcess(
        cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', '-s' , 'libgazebo_ros_factory.so', '-s' , 'libgazebo_ros_force_system.so', world],
        output='screen')

BUT !!! if you try this, the cotsmap will appear for only one of the robots (robot1 or robot2), one the robots will not work because as per this link the default values for groot_zmq_publisher_port and groot_zmq_server_port are 1666 and 1667, so you have to modify the nav2_params for one of the robots to use different port, for example in nav2_multirobot_params_2.yaml, you can add different ports in bt_navigator like this :

enable_groot_monitoring: True
    groot_zmq_publisher_port: 1789
    groot_zmq_server_port: 1887

So your bt_navigator will look like:

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20

    # groot ports need to be different from other robots config !!
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1789
    groot_zmq_server_port: 1887
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - ....
    - .....
    - ......

[SteveMacenski]

So sounds like we need to do 2 things:

• Change the default values of the port in the multirobot example configuration files
• Add 2 gazebo .so paths to the command

@Yousseflah can you submit a PR with those changes you've made? @simonchamorro can you confirm this resolves your concerns?

[simonchamorro]

Sorry for the late response, yes this fixes it! Thanks @Yousseflah !

[SteveMacenski]

Awesome, changes have been merged, closing issue