How can I implement Nav2 on turtlebot lime on Isaac SIM?

[Misoka_An]

Hi,

I’m trying to implement Custom Nav2 on a Custom robot, but I’m having some trouble with it.

odom

map

current usd
draft_lime.zip (22.5 KB)

Custom robot code (Turtlerobot Lime)
github

• 2D Pose Estimate not work
• Message Filter dropping message: frame ‘lidar_scan’ at time 12.418 for reason ‘discarding message because the queue is full’
• Isaac Sim error?

2024-09-12 11:51:37 [17,141,726ms] [Error] [omni.isaac.dynamic_control.plugin] DcGetRigidBodyLocalLinearVelocity: Invalid or expired body handle
2024-09-12 11:51:37 [17,141,727ms] [Error] [omni.isaac.dynamic_control.plugin] DcGetRigidBodyAngularVelocity: Invalid or expired body handle
2024-09-12 11:51:37 [17,141,743ms] [Error] [omni.isaac.dynamic_control.plugin] DcGetRigidBodyPose: Invalid or expired body handle
2024-09-12 11:51:37 [17,141,743ms] [Error] [omni.isaac.dynamic_control.plugin] DcGetRigidBodyLocalLinearVelocity: Invalid or expired body handle
2024-09-12 11:51:37 [17,141,743ms] [Error] [omni.isaac.dynamic_control.plugin] DcGetRigidBodyAngularVelocity: Invalid or expired body handle
^C2024-09-12 11:51:37 [17,141,760ms] [Error] [omni.isaac.dynamic_control.plugin] DcGetRigidBodyPose: Invalid or expired body handle
2024-09-12 11:51:37 [17,141,760ms] [Error] [omni.isaac.dynamic_control.plugin] DcGetRigidBodyLocalLinearVelocity: Invalid or expired body handle
2024-09-12 11:51:37 [17,141,760ms] [Error] [omni.isaac.dynamic_control.plugin] DcGetRigidBodyAngularVelocity: Invalid or expired body handle

[zhengwang]

Hey @Misoka_An ! Thanks for your information.

I am trying to replicate your issue with the files you provided.
I opened up the usd file you shared in Isaac Sim 4.1.0. But I don’t see the turtle3_lime robot under World. Are you sharing the correct file or am I missing something?

So I tried to generate the urdf file and imported it (I named it turtlebot3_lime_01 because somehow I cannot name it to turtlebot3_lime). I created a rotating lidar under base_scan.

I found one issue:
For the Isaac Compute Odometry Node, the chassis prim is not a valid rigid body or articulation root. Maybe change it to /World/turtlebot3_lime/base_link?

After I made that change, the error of Invalid or expired body handle is gone.
Here is the usd file with my change (including both the scene and the robot usd files)
draft_lime_zw.zip (12.0 MB)

To help you with pose estimation issue and lidar_scan issue, could you please also share the map.yaml and png/pgm file for this scene? Thanks!

[Misoka_An]

Hi @zhengwang,
Thank you for taking the time to consider this !

I uploaded map.png and map.yaml to this git repository.

(I also reuploaded the USD file containing the TurtleBot3_Lime model.)

I’ll check the USD changes you made now.

edit:
Thanks to the change you made, Invalid or expired body handle error is gone.

However when i try to do SLAM or NAV2, i still get below error.

ros2 launch turtlebot3_lime_cartographer cartographer.launch.py

[INFO] [launch]: All log files can be found below /home/an/.ros/log/2024-09-19-15-24-02-084072-anpc-656582
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [cartographer_node-1]: process started with pid [656583]
[INFO] [cartographer_occupancy_grid_node-2]: process started with pid [656585]
[INFO] [rviz2-3]: process started with pid [656587]
[cartographer_node-1] [INFO] [1726727042.141125545] [cartographer logger]: I0919 15:24:02.000000 656583 configuration_file_resolver.cc:41] Found ‘/home/an/Documents/turtlebot/turtlebot3_ws/install/turtlebot3_lime_cartographer/share/turtlebot3_lime_cartographer/config/turtlebot3_2d.lua’ for ‘turtlebot3_2d.lua’.
[cartographer_node-1] [INFO] [1726727042.141294659] [cartographer logger]: I0919 15:24:02.000000 656583 configuration_file_resolver.cc:41] Found ‘/opt/ros/humble/share/cartographer/configuration_files/map_builder.lua’ for ‘map_builder.lua’.
[cartographer_node-1] [INFO] [1726727042.141305666] [cartographer logger]: I0919 15:24:02.000000 656583 configuration_file_resolver.cc:41] Found ‘/opt/ros/humble/share/cartographer/configuration_files/map_builder.lua’ for ‘map_builder.lua’.
[cartographer_node-1] [INFO] [1726727042.141326010] [cartographer logger]: I0919 15:24:02.000000 656583 configuration_file_resolver.cc:41] Found ‘/opt/ros/humble/share/cartographer/configuration_files/pose_graph.lua’ for ‘pose_graph.lua’.
[cartographer_node-1] [INFO] [1726727042.141331647] [cartographer logger]: I0919 15:24:02.000000 656583 configuration_file_resolver.cc:41] Found ‘/opt/ros/humble/share/cartographer/configuration_files/pose_graph.lua’ for ‘pose_graph.lua’.
[cartographer_node-1] [INFO] [1726727042.141385627] [cartographer logger]: I0919 15:24:02.000000 656583 configuration_file_resolver.cc:41] Found ‘/opt/ros/humble/share/cartographer/configuration_files/trajectory_builder.lua’ for ‘trajectory_builder.lua’.
[cartographer_node-1] [INFO] [1726727042.141391992] [cartographer logger]: I0919 15:24:02.000000 656583 configuration_file_resolver.cc:41] Found ‘/opt/ros/humble/share/cartographer/configuration_files/trajectory_builder.lua’ for ‘trajectory_builder.lua’.
[cartographer_node-1] [INFO] [1726727042.141406021] [cartographer logger]: I0919 15:24:02.000000 656583 configuration_file_resolver.cc:41] Found ‘/opt/ros/humble/share/cartographer/configuration_files/trajectory_builder_2d.lua’ for ‘trajectory_builder_2d.lua’.
[cartographer_node-1] [INFO] [1726727042.141412588] [cartographer logger]: I0919 15:24:02.000000 656583 configuration_file_resolver.cc:41] Found ‘/opt/ros/humble/share/cartographer/configuration_files/trajectory_builder_2d.lua’ for ‘trajectory_builder_2d.lua’.
[cartographer_node-1] [INFO] [1726727042.141460116] [cartographer logger]: I0919 15:24:02.000000 656583 configuration_file_resolver.cc:41] Found ‘/opt/ros/humble/share/cartographer/configuration_files/trajectory_builder_3d.lua’ for ‘trajectory_builder_3d.lua’.
[cartographer_node-1] [INFO] [1726727042.141465737] [cartographer logger]: I0919 15:24:02.000000 656583 configuration_file_resolver.cc:41] Found ‘/opt/ros/humble/share/cartographer/configuration_files/trajectory_builder_3d.lua’ for ‘trajectory_builder_3d.lua’.
[cartographer_node-1] [INFO] [1726727042.146980218] [cartographer logger]: I0919 15:24:02.000000 656583 map_builder_bridge.cpp:136] Added trajectory with ID ‘0’.
[rviz2-3] Gtk-Message: 15:24:02.176: Failed to load module “canberra-gtk-module”
[rviz2-3] Gtk-Message: 15:24:02.176: Failed to load module “canberra-gtk-module”
[rviz2-3] [INFO] [1726727042.655108328] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-3] [INFO] [1726727042.655179090] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-3] [INFO] [1726727042.744779757] [rviz2]: Stereo is NOT SUPPORTED
[cartographer_node-1] [WARN] [1726727043.143600129] [cartographer logger]: W0919 15:24:03.000000 656583 tf_bridge.cpp:53] Lookup would require extrapolation at time 747.416706, but only time 747.416706 is in the buffer, when looking up transform from frame [base_footprint] to frame [imu_link]
[cartographer_node-1] [WARN] [1726727043.143731611] [cartographer logger]: W0919 15:24:03.000000 656583 tf_bridge.cpp:53] Lookup would require extrapolation at time 747.416706, but only time 747.416706 is in the buffer, when looking up transform from frame [base_footprint] to frame [imu_link]
[rviz2-3] [INFO] [1726727043.144666251] [rviz]: Message Filter dropping message: frame ‘odom’ at time 747.067 for reason ‘discarding message because the queue is full’
[rviz2-3] [INFO] [1726727043.176186432] [rviz]: Message Filter dropping message: frame ‘odom’ at time 747.083 for reason ‘discarding message because the queue is full’
[rviz2-3] [INFO] [1726727043.208207157] [rviz]: Message Filter dropping message: frame ‘odom’ at time 747.100 for reason ‘discarding message because the queue is full’
[rviz2-3] [INFO] [1726727043.240000171] [rviz]: Message Filter dropping message: frame ‘odom’ at time 747.117 for reason ‘discarding message because the queue is full’

[zhengwang]

Thanks for the map data! Could you please also share your turtlebot3_lime usd file? I think in your scene usd file, it is referencing the turtlebot3_lime usd file. So when I open the scene, there is no robot.

According to the error message and the tf tree, it looks like something is wrong with how the links are set up so AMCL is not able to publish TF between map and odom.

I also noticed that in the lidar action graph, you set the frame ID of laser scan type data to lime_laser and the frame ID of point cloud type data to lime_pointcloud. Why do you use different frame IDs for the same data source?

You can also refer to the ROS2 navigation tutorial which is doing similar thing.

[Misoka_An]

I’m not sure why it didn’t show in the scene, but I’ve uploaded my robot USD file here:
Uploading: turtlebot3_lime_demo.usd.zip…
About the robot, instead of defining the camera for the RealSense D435i, I attached the D455 for convenience. It locate in base_footprint

According to the error message and the tf tree, it looks like something is wrong with how the links are set up so AMCL is not able to publish TF between map and odom.

That might be the case. Here’s what it looks like in Gazebo. However, I’m still having trouble creating the under TF using the Action Graph. Could you share how I should set it up?

I also noticed that in the lidar action graph, you set the frame ID of laser scan type data to lime_laser and the frame ID of point cloud type data to lime_pointcloud . Why do you use different frame IDs for the same data source?
You can also refer to the ROS2 navigation tutorial which is doing similar thing.

I was only using the /scan data, so I thought it would be better to give it a different name. As for the tutorial you mentioned, I’ve already gone through it and am now trying to figure out how to apply a similar approach to the Turtlebot Lime.

[zhengwang]

@Misoka_An

Uploading: turtlebot3_lime_demo.usd.zip…

I think the turtle3_lime_demo.usd is failed to upload… Could you please give it another try?

[Misoka_An]

turtlebot3_lime_demo.usd.zip (13.2 MB)

I think the turtle3_lime_demo.usd is failed to upload… Could you please give it another try?

Oh, my bad. Could you check it now?

[zhengwang]

Thanks for your file!

Besides the chassis prim of the Isaac Compute Odometry Node needs to be changed to /World/turtlebot3_lime/base_link, here are a few more things that probably need to be changed:

1. For node Isaac Read Simulation Time Node (one in ROS_odometry and one in Clock), check “Reset On Stop”, which makes the simulation time to start from 0 every time you starts it.
2. For the two ROS2RTX Lidar Helper Nodes, change the frameId to “base_scan” to align with the lidar link name in your robot

I am not sure how you customized your navigation launch file and param file. But here are how I customized them based on the example from carter_navigation tutorial. Please note that the parameters are not tuned for turtlebot lime.

navigation2_use_sim_time_zw.launch.py (5.6 KB)
turtlebot3_use_sim_time_zw.yaml (11.2 KB)

With the above changes, I am able to set the robot pose in rviz:

[Misoka_An]

Hi @zhengwang !
Thank you so much for your valuable feedback! After making the suggested adjustments for Turtlebot Lime, I’ve been able to successfully get Nav2 working and the robot is now moving around smoothly. I really appreciate the time and effort you put into helping me.

As I move on to integrating Moveit2 on Lime, I might run into new challenges, and I would be truly grateful for your guidance again if needed.