Added more d435i examples (#848)

2026-10-08 10:47:46 +08:00 · 2022-11-27 18:01:16 -08:00
parent 5c3c269652
commit 198da0b072
2 changed files with 136 additions and 0 deletions
@@ -0,0 +1,77 @@
+# Requirements:
+#   A realsense D435i
+#   Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
+# Example:
+#   $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_sync:=true
+#
+#   $ ros2 launch rtabmap_ros realsense_d435i_color.launch.py
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    parameters=[{
+          'frame_id':'camera_link',
+          'subscribe_depth':True,
+          'approx_sync':False,
+          'wait_imu_to_init':True}]
+
+    remappings=[
+          ('imu', '/imu/data'),
+          ('rgb/image', '/camera/color/image_raw'),
+          ('rgb/camera_info', '/camera/color/camera_info'),
+          ('depth/image', '/camera/realigned_depth_to_color/image_raw')]
+
+    return LaunchDescription([
+
+        # Nodes to launch       
+        Node(
+            package='rtabmap_ros', executable='rgbd_odometry', output='screen',
+            parameters=parameters,
+            remappings=remappings),
+
+        Node(
+            package='rtabmap_ros', executable='rtabmap', output='screen',
+            parameters=parameters,
+            remappings=remappings,
+            arguments=['-d']),
+
+        Node(
+            package='rtabmap_ros', executable='rtabmapviz', output='screen',
+            parameters=parameters,
+            remappings=remappings),
+        
+        # Because of this issue: https://github.com/IntelRealSense/realsense-ros/issues/2564
+        # Generate point cloud from not aligned depth
+        Node(
+            package='rtabmap_ros', executable='point_cloud_xyz', output='screen',
+            parameters=[{'approx_sync':False}],
+            remappings=[('depth/image',       '/camera/depth/image_rect_raw'),
+                        ('depth/camera_info', '/camera/depth/camera_info'),
+                        ('cloud',             '/camera/cloud_from_depth')]),
+        
+        # Generate aligned depth to color camera from the point cloud above       
+        Node(
+            package='rtabmap_ros', executable='pointcloud_to_depthimage', output='screen',
+            parameters=[{ 'decimation':2,
+                          'fixed_frame_id':'camera_link',
+                          'fill_holes_size':1}],
+            remappings=[('camera_info', '/camera/color/camera_info'),
+                        ('cloud',       '/camera/cloud_from_depth'),
+                        ('image_raw',   '/camera/realigned_depth_to_color/image_raw')]),
+        
+        # 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', '/camera/imu')]),
+        
+        # The IMU frame is mising in TF tree, add it:
+        Node(
+            package='tf2_ros', executable='static_transform_publisher', output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']),
+    ])
@@ -0,0 +1,59 @@
+# Requirements:
+#   A realsense D435i
+#   Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
+# Example:
+#   $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true
+#   $ ros2 param set /camera/camera depth_module.emitter_enabled 0
+#
+#   $ ros2 launch rtabmap_ros realsense_d435i_infra.launch.py
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    parameters=[{
+          'frame_id':'camera_link',
+          'subscribe_depth':True,
+          'approx_sync':False,
+          'wait_imu_to_init':True}]
+
+    remappings=[
+          ('imu', '/imu/data'),
+          ('rgb/image', '/camera/infra1/image_rect_raw'),
+          ('rgb/camera_info', '/camera/infra1/camera_info'),
+          ('depth/image', '/camera/depth/image_rect_raw')]
+
+    return LaunchDescription([
+
+        # Nodes to launch       
+        Node(
+            package='rtabmap_ros', executable='rgbd_odometry', output='screen',
+            parameters=parameters,
+            remappings=remappings),
+
+        Node(
+            package='rtabmap_ros', executable='rtabmap', output='screen',
+            parameters=parameters,
+            remappings=remappings,
+            arguments=['-d']),
+
+        Node(
+            package='rtabmap_ros', executable='rtabmapviz', output='screen',
+            parameters=parameters,
+            remappings=remappings),
+                
+        # 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', '/camera/imu')]),
+        
+        # The IMU frame is mising in TF tree, add it:
+        Node(
+            package='tf2_ros', executable='static_transform_publisher', output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']),
+    ])