Make tb4 sim work with namespaced simulations

[luca-della-vedova]

Hello there!

I was trying to get a namespaced simulation to work for interoperability with a MoveIt2 arm that I was getting on and I noticed that the namespacing doesn't work super well.

This PR helps in that direction, note that rviz will still not quite work since it relies on a config file that is not namespaced, but at least the simulation can run.

Test it!

On latest main / rolling with nav2 and this package built from source:

ros2 launch nav2_bringup tb4_simulation_launch.py use_namespace:=true namespace:=/tb4 headless:=0

Without the PR you will see bunch of errors because the namespacing is not applied to the topic that this launch file passes to the create node and you will get a continuous stream of this message:

[create-5] [INFO] [1726828557.189342324] [ros_gz_sim]: Waiting messages on topic [robot_description].

Also ros_gz_sim create does not seem to have a robot_namespace parameter, and -entity is actually -name:

lucadv@noble:~/ionic_ws$ ros2 run ros_gz_sim create --help
create: Usage: create -world [arg] [-file FILE] [-param PARAM] [-topic TOPIC]
                       [-string STRING] [-name NAME] [-allow_renaming RENAMING] [-x X] [-y Y] [-z Z]
                       [-R ROLL] [-P PITCH] [-Y YAW]

[...]

  Flags from /usr/local/google/home/lucadv/ionic_ws/src/ros2/ros_ign/ros_gz_sim/src/create.cpp:
    -P (Pitch component of initial orientation, in radians.) type: double
      default: 0
    -R (Roll component of initial orientation, in radians.) type: double
      default: 0
    -Y (Yaw component of initial orientation, in radians.) type: double
      default: 0
    -allow_renaming (Rename entity if name already used.) type: bool
      default: false
    -file (Load XML from a file.) type: string default: ""
    -name (Name for spawned entity.) type: string default: ""
    -param (Load XML from a ROS param.) type: string default: ""
    -string (Load XML from a string.) type: string default: ""
    -topic (Load XML from a ROS string publisher.) type: string default: ""
    -world (World name.) type: string default: ""
    -x (X component of initial position, in meters.) type: double default: 0
    -y (Y component of initial position, in meters.) type: double default: 0
    -z (Z component of initial position, in meters.) type: double default: 0

With this PR, you should see that the demo works and you can set a start pose and get the robot to navigate, now this still generates a lot of errors, the costmaps are not displayed, but it's a step forward

[SteveMacenski]

I think the use_simulator needs to be placed back in

[SteveMacenski]

@luca-della-vedova for multirobot namespacing, you may want to look at the TB3 work in this same repository. We have multi-robot launch files that use those that work fine. The TB4 wasn't (I don't think?) adjusted to have those changes also make w.r.t. it since that was a 'last migration feature' for Nav2 to have the original TB3 launch files included (not including additional TB4 ones).

You may find it fruitful to look there and happy to merge in changes needed to make that possible :-)

[luca-della-vedova]

Ok added back the if condition! I admit having a bit of trouble setting up a namespaced tb4, I hope to be able to share my findings (and changes I had to do to make it work) but in a bit of a time crunch!

[SteveMacenski]

Just remove the line if its not needed

[luca-della-vedova]

The rest of its usage is under commented code that imho should be addressed at some point (spawning the robot through SDF the same way it's done for TB3 i.e. here. I commented it as consistency with the rest of the TODO and to make it easier to address it later, otherwise might as well remove all the related commented code.

[SteveMacenski]

I agree, I think we should remove all commented out code

[luca-della-vedova]

Done cdacf92

[SteveMacenski]

My understanding is this is not making namespaced work - it is an incremental improvement on the way there? If so, would it be wiser to leave this as a draft PR to build on until it works rather than merging in something half functional

now this still generates a lot of errors, the costmaps are not displayed, but it's a step forward

Or I guess maybe some of these errors are different and non-simulator related? We don't have a TB4 multirobot launch file currently in Nav2 that we support (yet!!), so I suppose you could be referring to that

[luca-della-vedova]

My understanding is this is not making namespaced work - it is an incremental improvement on the way there? If so, would it be wiser to leave this as a draft PR to build on until it works rather than merging in something half functional

now this still generates a lot of errors, the costmaps are not displayed, but it's a step forward

Or I guess maybe some of these errors are different and non-simulator related? We don't have a TB4 multirobot launch file currently in Nav2 that we support (yet!!), so I suppose you could be referring to that

I'd argue that it "makes it work" at the same level that tb3 namespaced simulation works, if that was the standard we are aiming for then it should be fairly similar. Specifically, as I mentioned the simulation runs but rviz doesn't work 100% since it doesn't display the cost maps, specifically, on current main of navigation2 and with this PR:

ros2 launch nav2_bringup tb4_simulation_launch.py headless:=0

ros2 launch nav2_bringup tb4_simulation_launch.py use_namespace:=true namespace:=/tb4 headless:=0, note how the links are displayed (and actually navigation works) but the costmaps are not visualized.

However, if I do the same for tb3:

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=0 works well (pardon my misalignment in initial pose!)

ros2 launch nav2_bringup tb3_simulation_launch.py use_namespace:=true namespace:=/tb4 headless:=0, note how, similar to tb4, visualization is missing some elements like costmaps

I believe the root cause for the remaining issue is not really in this repo but in how namespaces in rviz are handled, so I would look into navigation2 if this use case is meant to be working.

[SteveMacenski]

ros2 launch nav2_bringup tb4_simulation_launch.py use_namespace:=true namespace:=/tb4 headless:=0, note how the links are displayed (and actually navigation works) but the costmaps are not visualized.

Ah. Does the map display type for costmap use that namespace in the rviz displays panel?

For the multirobot launch file, we can see the costmaps for the TB3, but we have a namespaced templated rviz config that it launches and uses. If you just launch Nav2 with a namespace, the rviz config doesn't consider it. If you can just simply show that selecting the right topic makes it appear, I'm happy with that.

The issue then is just that the default rviz configs don't handle namespaces, which is another issue (or non-issue) altogether.

[luca-della-vedova]

ros2 launch nav2_bringup tb4_simulation_launch.py use_namespace:=true namespace:=/tb4 headless:=0, note how the links are displayed (and actually navigation works) but the costmaps are not visualized.

Ah. Does the map display type for costmap use that namespace in the rviz displays panel?

For the multirobot launch file, we can see the costmaps for the TB3, but we have a namespaced templated rviz config that it launches and uses. If you just launch Nav2 with a namespace, the rviz config doesn't consider it. If you can just simply show that selecting the right topic makes it appear, I'm happy with that.

The issue then is just that the default rviz configs don't handle namespaces, which is another issue (or non-issue) altogether.

Yap selecting the right topic works fine, I opened a proposal PR on the main nav2 repo with my proposal to remove all the manual string substitutions, multiple rviz config files / nav2 params and make all of this just work out of the box. Without that PR the state will be what is shown above (rviz not quite behaving as it is expected).

I admit being at the first peak of Dunning Kruger effect (worked on this a few days and feel likely too confident that it's correct) so I might have missed some of the background behind the decisions but it seems to work for me and makes the bringup simpler (no need to do string substitutions), more reliable (rviz for single namespaced robots works) and less boiler plate-y (don't need to maintain multiple sets of config files).

[SteveMacenski]

Thanks for this - merged. I don't want to block you for your ROSCon work