Added new depthai examples (color and stereo)

2026-09-12 22:30:19 +08:00 · 2026-02-08 14:54:33 -08:00
parent 3283309e76
commit 6ed02b65e4
3 changed files with 178 additions and 1 deletions
@@ -1,8 +1,14 @@
 # Requirements:
 #   A OAK-D camera
-#   Install depthai-ros package (https://github.com/luxonis/depthai-ros)
+#   Install depthai-ros package (https://github.com/luxonis/depthai-ros). Tested on Humble branch!
 # Example:
 #   $ ros2 launch rtabmap_examples depthai.launch.py camera_model:=OAK-D
+#
+# Description: In this example, we feed IR-D images to rtabmap
+#
+# Note: The first frames may be too bright or too dark till camera exposure adjusts 
+#       to an appriopriate level. Do "Detection->Reset odometry", then 
+#       "Edit->Delete memory" if tracking is lost on start.

 import os

@@ -0,0 +1,81 @@
+# Requirements:
+#   A OAK-D camera
+#   Install depthai-ros package (https://github.com/luxonis/depthai-ros). Tested on Humble branch!
+# Example:
+#   $ ros2 launch rtabmap_examples depthai_color.launch.py camera_model:=OAK-D
+#
+# Description: In this example, we feed RGB-D images to rtabmap
+#
+# Note: The first frames may be too bright or too dark till camera exposure adjusts 
+#       to an appriopriate level. Do "Detection->Reset odometry", then 
+#       "Edit->Delete memory" if tracking is lost on start.
+
+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.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    parameters=[{'frame_id':'oak-d-base-frame',
+                 'subscribe_rgbd':True,
+                 'subscribe_odom_info':True,
+                 'approx_sync':False}]
+    
+    sync_parameters=[{'approx_sync':True,
+                      'approx_sync_max_interval':0.005}]
+    
+    remappings=[('imu', '/imu/data')]
+
+    return LaunchDescription([
+
+        # Launch camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('depthai_examples'), 'launch'),
+                '/stereo_inertial_node.launch.py']),
+                launch_arguments={'enableRviz': 'false',
+                                  'rgbResolution': '1080p',
+                                  'rgbScaleNumerator': '2', # Convert to 720p (same size than depth)
+                                  'rgbScaleDinominator': '3'}.items(),
+        ),
+
+        # Sync right/depth/camera_info together
+        Node(   
+            package='rtabmap_sync', executable='rgbd_sync', output='screen',
+            parameters=sync_parameters,
+            remappings=[('rgb/image', '/color/image'),
+                        ('rgb/camera_info', '/color/camera_info'),
+                        ('depth/image', '/stereo/depth')]),
+
+        # Compute quaternion of the IMU
+        Node(
+            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
+            parameters=[{'use_mag': False, 
+                         'world_frame':'enu', 
+                         'publish_tf':False}],
+            remappings=[('imu/data_raw', '/imu')]),
+
+        # 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)
+    ])
@@ -0,0 +1,90 @@
+# Requirements:
+#   A OAK-D camera
+#   Install depthai-ros package (https://github.com/luxonis/depthai-ros). Tested on Humble branch!
+#
+#   Issue: To have compatible stereo camera info with rtabmap (P[0,3] should be positive on left camera info or negative in right camera info), 
+#          we should add the following line here (https://github.com/luxonis/depthai-ros/blob/887248d72cc6b9515793f828645346408b9cad47/depthai_examples/src/stereo_inertial_publisher.cpp#L593)
+#          so that left camera info has a positive P[0,3] instead of negative to correctly compute the baseline:
+#
+#   <line 593> leftCameraInfo.p[3] *=-1;
+# 
+# Example:
+#   $ ros2 launch rtabmap_examples depthai_stereo.launch.py camera_model:=OAK-D
+#
+# Description: In this example, we feed stereo IR images to rtabmap
+#
+# Note: The first frames may be too bright or too dark till camera exposure adjusts 
+#       to an appriopriate level. Do "Detection->Reset odometry", then 
+#       "Edit->Delete memory" if tracking is lost on start.
+
+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.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    parameters={'frame_id':'oak-d-base-frame',
+                 'subscribe_rgbd':True,
+                 'subscribe_odom_info':True,
+                 'approx_sync':False,
+                 'wait_imu_to_init':True}
+    
+    sync_parameters=[{'approx_sync':True,
+                      'approx_sync_max_interval':0.005}]
+
+    remappings=[('imu', '/imu/data')]
+
+    return LaunchDescription([
+
+        # Launch camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('depthai_examples'), 'launch'),
+                '/stereo_inertial_node.launch.py']),
+                launch_arguments={'depth_aligned': 'false', # not color mode
+                                  'enableRviz': 'false',
+                                  'monoResolution': '400p'}.items(),
+        ),
+
+        # Sync right/depth/camera_info together
+        Node(   
+            package='rtabmap_sync', executable='stereo_sync', output='screen',
+            parameters=sync_parameters,
+            remappings=[('left/image', '/left/image_rect'),
+                        ('left/camera_info', '/left/camera_info'),
+                        ('right/image', '/right/image_rect'),
+                        ('right/camera_info', '/right/camera_info')]),
+
+        # Compute quaternion of the IMU
+        Node(
+            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
+            parameters=[{'use_mag': False, 
+                         'world_frame':'enu', 
+                         'publish_tf':False}],
+            remappings=[('imu/data_raw', '/imu')]),
+
+        # Visual odometry
+        Node(
+            package='rtabmap_odom', executable='stereo_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,
+                        {'odometry_node_name': "stereo_odometry"}],
+            remappings=remappings)
+    ])