Update Gazebo #51

elsayedelsheikh · 2024-12-09T15:16:13Z

Closes (#44), Tested with ros2-rolling with a few tasks left:

Update block spawner
Add set_init_amcl_pose (/or) use nav2_params.yaml to initialize robot pose.

We are gonna need to update /cmd_vel publishers as nav2 has migrated to TwistStamped message for /cmd_vel topics a few days ago.

Should go for the latest update with TwistStamped messages and update docker file as well ?

sea-bass · 2024-12-09T16:23:19Z

Amazing!

Is the TwistStamped update only for Rolling, or will it be backported/possible in Humble/Jazzy as well?

I think I would maybe bias towards what is going to be possible in Jazzy. For Humble, the current version of the repo is fine if needed.

Edit: I see it's going in for Jazzy (https://discourse.ros.org/t/nav2-support-for-twiststamped-available-rolling-jazzy/35995). In that case, yes let's update.

tb3_worlds/launch/tb3_world.launch.py

dependencies.repos

elsayedelsheikh · 2024-12-09T17:32:11Z

Amazing!

Is the TwistStamped update only for Rolling, or will it be backported/possible in Humble/Jazzy as well?

I think I would maybe bias towards what is going to be possible in Jazzy. For Humble, the current version of the repo is fine if needed.

Edit: I see it's going in for Jazzy (https://discourse.ros.org/t/nav2-support-for-twiststamped-available-rolling-jazzy/35995). In that case, yes let's update.

Great! Moving forward with TwistStamped. I believe there's no need to update the Docker files for now. With this, the repository now supports ROS2 Humble and the latest distribution.

elsayedelsheikh · 2024-12-09T17:38:41Z

So the world file and examples still have a TurtleBot 3, but this now is bring in a TurtleBot4 repo?

I'm okay with either robot, but we should be consistent. Probably prefer the 4 since it's newer.

We can have both turtlebot3 and turtlebot 4 models, I'll add an extra argument to specify which model to use in tb3_demo_world.launch.yaml

launch:
- arg:
    name: 'use_turtlebot3'
    default: 'False' # False for Turtlebot4
...
- include:
    file: '$(find-pkg-share nav2_bringup)/launch/tb3_simulation_launch.py'
    if: '$(eval "$(var use_turtlebot3) == True")'
...
- include:
    file: '$(find-pkg-share nav2_bringup)/launch/tb4_simulation_launch.py'
    if: '$(eval "$(var use_turtlebot3) == False")'
...

tb3_worlds/launch/tb3_demo_world_old.launch.py

sea-bass

Just did some testing.

At this point, I think it makes sense to default to using Jazzy, for which I have a PR you can stack on top of: #52

Also, notice that the formatting check is failing. You can run it locally with:

pip3 install pre-commit
pre-commit run -a

It seems that the sim service no longer works since we're not installing the turtlebot3_gazebo package any longer. Might want to remove.

  # Runs basic TurtleBot3 simulation
  sim:
    extends: overlay
    command: ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

Also, docker compose run demo-world also did not succeed for 2 reasons:

The launch.py needs to change to launch.yaml -- on second though, should we just keep this as a Python file for consistency?
There is no package.xml dependency on the nav2_bringup package. This must be added

Lastly, I tried running this with Jazzy and the robot did not move. This happened in both the TB3 and TB4 case. Even using the RViz widget to follow a path, Nav2 outputs a path to follow, but the robot never follows it. Could it be that the twist command topics are not properly connected?

On a smaller note, it also seems like the AMCL initial guess never was updated, as it looks off.

dependencies.repos

tb3_worlds/launch/block_spawner.launch.py

sea-bass · 2024-12-11T00:36:32Z

tb3_worlds/launch/tb3_demo_world.launch.yaml

+- arg:
+    name: 'use_turtlebot3' ## Default: False, Use Turtlebot4
+    default: 'True'


This is fine, but also note that there is a top-level .env file that defines certain args like TURTLEBOT3_MODEL.

Maybe this argument can now be switched to something like

TURTLEBOT_VERSION=3 # or 4

and piped through to the launch files as shown in the docker-compose.yaml file:

demo-behavior-py: extends: overlay command: > ros2 launch tb3_autonomy tb3_demo_behavior_py.launch.py tree_type:=${BT_TYPE:?} enable_vision:=${ENABLE_VISION:?} target_color:=${TARGET_COLOR:?}

tb3_worlds/launch/tb3_world.launch.py

tb3_worlds/scripts/set_init_amcl_pose.py

tb3_autonomy/src/vision_behaviors.cpp

sea-bass · 2024-12-11T01:20:12Z

Oh... Actually my Jazzy PR cannot go in until this PR does, because there is no gazebo_ros package (i.e., no Gazebo classic support). Fun :)

elsayedelsheikh · 2024-12-11T12:37:22Z

  # Runs basic TurtleBot3 simulation
  sim:
    extends: overlay
    command: ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

Let's replace this with the minimal simulation

sim:
    extends: overlay
    command: ros2 launch tb3_worlds tb3_world.launch.py

The launch.py needs to change to launch.yaml -- on second though, should we just keep this as a Python file for consistency?

Agreed! I switched to python launch file, I needed extra control over arguments anyway

There is no package.xml dependency on the nav2_bringup package. This must be added

Done!

On a smaller note, it also seems like the AMCL initial guess never was updated, as it looks off.

There's an offset by 0.5m so I've modified the initial pose

Lastly, I tried running this with Jazzy and the robot did not move. This happened in both the TB3 and TB4 case. Even using the RViz widget to follow a path, Nav2 outputs a path to follow, but the robot never follows it. Could it be that the twist command topics are not properly connected?

Yes! I tested it on Jazzy and encountered several issues. The nav2_bringup binaries haven't been updated yet, so I copied the ROS-Gazebo bridge configuration. For now, we're using Twist messages.

Also, tb3_world.launch.py was not working because I forgot to modify the headless argument in the world file, LMK if we need to run this in a headless mode with only rviz showing up.

Additionally, for TurtleBot3, I added a camera plugin. Initially, there was only a depth camera, not an RGB camera, so I modified the setup to include an RGB camera. I also updated the ROS-Gazebo bridge to publish RGB values to ROS, mapping the topic name to /camera/image_raw, as this is used in the tb3_autonomy package.

elsayedelsheikh · 2024-12-11T12:55:41Z

Summary

We had to copy almost every launch file in nav2_minimal_tb3_sim

configs/turtlebot3_waffle_bridge.yaml to Add camera bridge and use Twist message for /cmd_vel
launch/spawn_tb3.launch.py because it doesn't have an argument for the previous config file
urdf/gz_waffle.sdf.xacro to add camera plugin

Similarly, for TurtleBot4, we will need to repeat this process. However, I am considering a different approach: creating a batch file to automate these changes in Docker after cloning the nav2_minimal_turtlebot_simulation repository. This would allow us to revert to the original implementation using the YAML launch file (or use python launch) for including those in nav2_minimal_turtlebot_simulation

sea-bass

I could get the example to work now! Nav2 works perfectly.

Still a few of my previous comments left over (such as the formatting and the docker-compose.yaml updates), plus some new ones I found from being able to test the full example.

Anyways, I did this with Jazzy using my PR in #52, rebased on top of this branch. It's close!

tb3_worlds/scripts/set_init_amcl_pose.py

sea-bass · 2024-12-12T02:22:14Z

tb3_worlds/maps/sim_house_locations.yaml

+location1: [-2.0, -1.5, -2.25]
+location2: [1.75, 1.0, 0.785]
+location3: [0.5, 4.0, 1.571]
+location4: [3.2, 0.75, -1.571]


why did these object locations need to change? Isn't the map the same?

Indeed, the navigation targets in the BT example do not position the camera to face the blocks.

When I revert this change, the examples work fine.

Ok, I'll revert changes back and test again

Are these correct locations? These ones using my changes

turtlebot3_behavior_demos/tb3_worlds/launch/block_spawner.launch.py

Line 22 in ef02d85

block_spawn_offset = 0.05

Also the original value for this was 0.6 but it gets randomly spawned very far from the wall

Which should we use?

The picture looks fine to me... I wonder then why it looked different on my end?

Ah I see now -- the blocks spawn in the proper locations, but the robot navigates to the wrong poses it seems. The orientation seems wrong and the camera does not face the blocks. What would have changed?

OK for some reason, that offset of 0.6 is needed. The correct values seem to be:

location1: [-1.6, -1.1, -2.25] location2: [-0.1, 3.4, 0.785] location3: [3.4, -0.1, 1.571] location4: [2.15, 1.9, -1.571]

and block_spawn_offset = 0.6 -- this needs to be a large value because the robot needs to navigate 0.6 meters away from the blocks.

These values worked for me.

tb3_worlds/launch/block_spawner.launch.py

sea-bass · 2024-12-12T02:38:06Z

tb3_worlds/launch/block_spawner.launch.py

+                    "name": mdl_name,
+                    "x": str(x),
+                    "y": str(y),
+                    "z": "0.2",


these blocks are floating in midair, 0.2 is too high a Z value.

Cubes were spawned far from wall several times and they ended up colliding with the robot so I've updated z value to be detected by the lidar and avoided, I'll set it back to 0.1

Is there maybe a way to have the blocks themselves spawn to not be visible by lidar? Or move the robot to be farther away from the blocks at its navigation poses?

Maybe add offset for each location? (Easier and more direct than using cos(theta), sin(theta) - can't change the robot target heading without changing the blocks positions)

location1: [-1.6, -1.1, -2.25] offset1: [0.5,0.3]

Sure. But note that these files get parsed by the autonomy stack as well, in putting together the behavior trees.

I would probably try this later, after this PR

elsayedelsheikh · 2024-12-12T16:39:27Z

I could get the example to work now! Nav2 works perfectly.

Still a few of my previous comments left over (such as the formatting and the docker-compose.yaml updates), plus some new ones I found from being able to test the full example.

Anyways, I did this with Jazzy using my PR in #52, rebased on top of this branch. It's close!

I have a VScode Dev container for each distro so I've been using that for testing, and I left the docker-compose.yaml updates for #52 PR

For formatting, Somehow I forgot to push the changes

elsayedelsheikh · 2024-12-12T20:13:19Z

Oops! I forgot to create a ros_gz config file for the turtlebot 4 model since the gazebo topic names are not the same for the 3 and 4 models
I will push that in the next few hours

sea-bass

Seems with the latest changes the image topics are down. I am getting nothing published on the /camera/image_raw topic.

It appears the Gazebo topic is not echoing either:

gz topic -e -t /world/default/model/turtlebot3_waffle/link/camera_link/sensor/intel_realsense_r200_depth/image

EDIT: AH -- this should be turtlebot3 instead of turtlebot3_waffle -- you need to fix that in your bridge YAML file.

elsayedelsheikh · 2024-12-13T18:48:52Z

I've used this environment variable to choose between turtlebot 3 and turtlebot 4 models

turtlebot3_behavior_demos/.env

Line 4 in 187dbf3

TURTLEBOT3_MODEL=waffle_pi

With the 4 model as the default

turtlebot3_behavior_demos/tb3_worlds/launch/turtlebot_spawner.launch.py

Lines 57 to 60 in b31a145

    
           declare_turtlebot_model_cmd = DeclareLaunchArgument( 
        
               "turtlebot_model", 
        
               default_value=EnvironmentVariable("TURTLEBOT3_MODEL", default_value="4"), 
        
           )

Tested both models a few times and we're good to go but I didn't handle other values than (3 or 4)

sea-bass · 2024-12-13T22:11:48Z

Oh, I was basing my branch on yours. I think you should merge this to main first and then I will clean up for jazzy

sea-bass · 2024-12-13T22:12:57Z

tb3_worlds/configs/turtlebot3_bridge.yaml

@@ -49,7 +49,7 @@

 # replace image_bridge
 - ros_topic_name: "/camera/image_raw"
-  gz_topic_name: "/world/default/model/turtlebot3_waffle/link/camera_link/sensor/intel_realsense_r200_depth/image"
+  gz_topic_name: "/world/default/model/turtlebot/link/camera_link/sensor/intel_realsense_r200_depth/image"


I had to make this turtlebot3, not turtlebot. Did you change anything, or is this a typo?

Anyways I will test again when I get a chance

Yes, I've updated this to use turtlebot for both models, so it works now:

turtlebot3_behavior_demos/tb3_worlds/launch/tb3_world.launch.py

Lines 62 to 64 in b31a145

declare_robot_name_cmd = DeclareLaunchArgument(

"robot_name", default_value="turtlebot", description="name of the robot"

)

It's fine to change the robot name for TurtleBot4, but not for the TurtleBot3 model. I think we shouldn't allow this as an option in the launch file because changing it would interfere with the image bridge.

elsayedelsheikh · 2024-12-13T23:42:07Z

tb3_worlds/configs/nav2_params.yaml

@@ -0,0 +1,441 @@
+/**:
+  ros__parameters:
+    enable_stamped_cmd_vel: true


TwistStamped messages were disabled by default for jazzy

https://discourse.ros.org/t/notice-nav2-migrated-to-twiststamped-to-replace-twist-for-cmd-vel-topics/40944

https://discourse.ros.org/t/nav2-support-for-twiststamped-available-rolling-jazzy/35995

sea-bass

OK -- tested again and now both the TurtleBot 3 and 4 models move! yay.

I still found inconsistencies between the block spawn and navigation poses.

Seems like now the Gazebo world and map frames are off by this 0.6, 0.6 offset, so the block spawning needs to be handled differently. See my comments.

sea-bass · 2024-12-14T00:47:59Z

tb3_worlds/launch/tb3_demo_world.launch.py

+    ## Update: Set initial pose in configs/nav2_params.yaml using AMCL initial_pose* parameters
+    ## Set /initialpose
+    # init_amcl_cmd = Node(
+    #     package="tb3_worlds",
+    #     executable="set_init_amcl_pose.py",
+    #     name="initial_pose_publisher",
+    #     parameters=[
+    #         {
+    #             "x": 0.6,
+    #             "y": 0.6,
+    #         }
+    #     ],
+    #     output="screen",
+    # )


what should we do with this part? I guess if it's replaced with the nav2_params.yaml changes, this can be removed.

sea-bass · 2024-12-14T00:49:46Z

tb3_worlds/configs/turtlebot3_bridge.yaml

+# This is a copy of https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation/blob/main/nav2_minimal_tb3_sim/configs/turtlebot3_waffle_bridge.yaml
+# Modification:
+#   - Added camera bridge


Instead of saying "this is a copy of URL" for all these files, maybe the generic statement can be:

Suggested change

# This is a copy of https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation/blob/main/nav2_minimal_tb3_sim/configs/turtlebot3_waffle_bridge.yaml

# Modification:

# - Added camera bridge

# This file has been modified from

# https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation/blob/main/nav2_minimal_tb3_sim/configs/turtlebot3_waffle_bridge.yaml

Also, might be better to use a permalink instead of the branch name, since these files are prone to change / be renamed / moved around.

Again, same for all the other instances of this note.

sea-bass · 2024-12-14T00:55:56Z

tb3_worlds/maps/sim_house_locations.yaml

+location1: [-1.6, -1.1, -2.25]
+location2: [-0.1, 3.4, 0.785]
+location3: [3.4, -0.1, 1.571]
+location4: [2.15, 1.9, -1.571]


I don't know what changed in the nav2 setup, but now it's the old poses that work again ... otherwise the robot navigates to an offset.

However, if you change the poses, while the robot navigates to the right place, the blocks now spawn some distance away.

I think now the Gazebo world and the map frame are off by 0.6 meters in X + Y, so this has to be accounted for somewhere.

So what worked for me on the latest branch was:

Revert to the old poses

Change the block spawner logic to account for this [0.6, 0.6] offset in addition to the robot offset

# Offset between Gazebo world and map frame (can we fix this somehow else?) block_spawn_offset = [-0.6, -0.6] # Distance from navigation waypoint to spawn object offset_from_robot = 0.6 # Spawn blocks in random locations model_names = ["red_block", "green_block", "blue_block"] sampled_locs = random.sample(list(locations.keys()), len(model_names)) for mdl_name, loc in zip(model_names, sampled_locs): x, y, theta = locations[loc] x += block_spawn_offset[0] + offset_from_robot * math.cos(theta) y += block_spawn_offset[1] + offset_from_robot * math.sin(theta)

sea-bass

OK -- this is all working now.

Thanks for all your work @elsayedelsheikh!

elsayedelsheikh · 2024-12-14T17:01:01Z

It was a pleasure working on this project.
Feel free to reach out if there are any additional tasks or future projects :)

elsayedelsheikh added 2 commits December 9, 2024 16:10

Update (cv_bridge.h is deprecated)

d8dc54f

Update to new Gazebo

17ccfba

sea-bass reviewed Dec 9, 2024

View reviewed changes

tb3_worlds/launch/tb3_world.launch.py Outdated Show resolved Hide resolved

dependencies.repos Outdated Show resolved Hide resolved

sea-bass reviewed Dec 10, 2024

View reviewed changes

tb3_worlds/launch/tb3_demo_world_old.launch.py Outdated Show resolved Hide resolved

elsayedelsheikh added 4 commits December 10, 2024 11:23

Use launch file for block spawner

d45c8c1

Publish initial pose for a few seconds

b0f887b

Update launch files for the latest gazebo

93eebc0

updating origin

b5bd7d1

sea-bass requested changes Dec 11, 2024

View reviewed changes

This comment was marked as duplicate.

Sign in to view

sea-bass reviewed Dec 11, 2024

View reviewed changes

tb3_autonomy/src/vision_behaviors.cpp Show resolved Hide resolved

sea-bass mentioned this pull request Dec 11, 2024

Update to ROS 2 Jazzy and make TurtleBot naming generic #52

Merged

elsayedelsheikh added 5 commits December 11, 2024 14:37

Update dependencies.repos

290e194

Update gz model path is set in the launch files

ed82048

Add nav2 dependency

01c40e1

Modify depth plugin to rgb camera only

acbfa18

Update for jazzy support

ef02d85

sea-bass reviewed Dec 12, 2024

View reviewed changes

elsayedelsheikh added 2 commits December 12, 2024 18:45

Update formatting and a couple of parameters

a5e593f

Add turtlebot4 support

2e5545e

sea-bass reviewed Dec 13, 2024

View reviewed changes

Fix gz bridge configuration

e65649d

elsayedelsheikh added 3 commits December 13, 2024 15:17

Add reference

98f2b06

Update locations

2779220

Update spawner

b31a145

elsayedelsheikh changed the base branch from main to jazzy-support December 13, 2024 18:22

sea-bass reviewed Dec 13, 2024

View reviewed changes

elsayedelsheikh changed the base branch from jazzy-support to main December 13, 2024 23:36

elsayedelsheikh added 2 commits December 14, 2024 01:37

Use TwistStamped messages for cmd_vel

d593ccd

Set AMCL initial pose using nav2 params file

88fb8aa

elsayedelsheikh commented Dec 13, 2024

View reviewed changes

elsayedelsheikh requested a review from sea-bass December 13, 2024 23:58

sea-bass reviewed Dec 14, 2024

View reviewed changes

elsayedelsheikh added 6 commits December 14, 2024 10:41

Use nav2_params for setting initial amcl pose

525920c

Fix camera topic

67cf517

Include URL references

dc7a97e

Revert old poses

5d7d105

Account for world offset

61f8883

Add pre commit

285e9fa

sea-bass approved these changes Dec 14, 2024

View reviewed changes

sea-bass merged commit bb4b026 into sea-bass:main Dec 14, 2024
1 of 2 checks passed

elsayedelsheikh deleted the update-to-gz-ionic branch December 14, 2024 16:44

	declare_robot_name_cmd = DeclareLaunchArgument(
	"robot_name", default_value="turtlebot", description="name of the robot"
	)

Update Gazebo #51

Update Gazebo #51

Conversation

elsayedelsheikh commented Dec 9, 2024 • edited Loading

sea-bass commented Dec 9, 2024 • edited Loading

elsayedelsheikh commented Dec 9, 2024

elsayedelsheikh commented Dec 9, 2024

sea-bass left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

This comment was marked as duplicate.

sea-bass commented Dec 11, 2024

elsayedelsheikh commented Dec 11, 2024

elsayedelsheikh commented Dec 11, 2024 • edited Loading

Summary

sea-bass left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sea-bass Dec 13, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

elsayedelsheikh commented Dec 12, 2024

elsayedelsheikh commented Dec 12, 2024

sea-bass left a comment • edited Loading

Choose a reason for hiding this comment

elsayedelsheikh commented Dec 13, 2024 • edited Loading

sea-bass commented Dec 13, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sea-bass left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sea-bass Dec 14, 2024 • edited Loading

Choose a reason for hiding this comment

sea-bass left a comment

Choose a reason for hiding this comment

elsayedelsheikh commented Dec 14, 2024

elsayedelsheikh commented Dec 9, 2024 •

edited

Loading

sea-bass commented Dec 9, 2024 •

edited

Loading

sea-bass left a comment •

edited

Loading

elsayedelsheikh commented Dec 11, 2024 •

edited

Loading

sea-bass Dec 13, 2024 •

edited

Loading

sea-bass left a comment •

edited

Loading

elsayedelsheikh commented Dec 13, 2024 •

edited

Loading

sea-bass Dec 14, 2024 •

edited

Loading