Fixing topic sync lagging and delay issues, more examples (depthai, zed) (#1206)

* Fixed odometry latency * Fixed node ID=0 issues as msgs may not have seq. (#1202) (cherry picked from commit 097cab0667) * Odom: removed long processing from ros callbacks (decreasing delay, also making delay independent of the message filters topic_queue_size and sync_queue_size parameters) * Changed a log from info->debug * Changed a log from info->debug * Updated default topic and sync queue_size inside nodes. Exposing topic and sync queue size params in warning when cannot synchronize. Added zed and depthai examples. Odom: Fixed imu callback group, added multi-threaded executors for all odometry nodes. * fixed merge * Include everything needed in example launch files for simple launch. Small fixes. --------- Co-authored-by: Borong Yuan <[email protected]>
2026-10-04 08:47:45 +08:00 · 2024-09-04 00:46:44 -07:00
parent 268609b0b7
commit c956e3780f
27 changed files with 465 additions and 180 deletions
@@ -0,0 +1,76 @@
+# Requirements:
+#   A ZED camera
+#   Install zed ros2 wrapper package (https://github.com/stereolabs/zed-ros2-wrapper)
+# Example:
+#   $ ros2 launch rtabmap_examples zed.launch.py camera_model:=zed2i
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+import tempfile
+
+def generate_launch_description():
+
+    # Hack to override grab_resolution parameter without changing any files
+    with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file:
+        zed_override_file.write("---\n"+
+                  "/**:\n"+
+                  "    ros__parameters:\n"+
+                  "        general:\n"+
+                  "            grab_resolution: 'VGA'")
+
+    camera_model = LaunchConfiguration('camera_model'),
+
+    parameters=[{'frame_id':'zed_camera_link',
+                 'subscribe_rgbd':True,
+                 'subscribe_odom_info':True,
+                 'approx_sync':False,
+                 'wait_imu_to_init':True}]
+
+    remappings=[('imu', '/zed/zed_node/imu/data')]
+
+    return LaunchDescription([
+
+        # Launch camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('zed_wrapper'), 'launch'),
+                '/zed_camera.launch.py']),
+                launch_arguments={'camera_model': camera_model,
+                                  'ros_params_override_path': zed_override_file.name}.items(),
+        ),
+
+        # Sync right/depth/camera_info together
+        Node(   
+            package='rtabmap_sync', executable='rgbd_sync', output='screen',
+            parameters=parameters,
+            remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'),
+                        ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'),
+                        ('depth/image', '/zed/zed_node/depth/depth_registered')]),
+
+        # Visual odometry
+        Node(
+            package='rtabmap_odom', executable='rgbd_odometry', output='screen',
+            parameters=parameters,
+            remappings=remappings),
+
+        # VSLAM
+        Node(
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=parameters,
+            remappings=remappings,
+            arguments=['-d']),
+
+        # Visualization
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            parameters=parameters,
+            remappings=remappings)
+    ])