ported rtabmap_ros_pkg_split to ros2

2026-10-04 08:47:45 +08:00 · 2023-02-22 23:11:49 -08:00
parent 469b8311f8
commit 778a474b96
452 changed files with 4572 additions and 58667 deletions
@@ -0,0 +1,60 @@
+# 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_stereo.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_stereo':True,
+          'subscribe_odom_info':True,
+          'wait_imu_to_init':True}]
+
+    remappings=[
+          ('imu', '/imu/data'),
+          ('left/image_rect', '/camera/infra1/image_rect_raw'),
+          ('left/camera_info', '/camera/infra1/camera_info'),
+          ('right/image_rect', '/camera/infra2/image_rect_raw'),
+          ('right/camera_info', '/camera/infra2/camera_info')]
+
+    return LaunchDescription([
+
+        # Nodes to launch       
+        Node(
+            package='rtabmap_ros', executable='stereo_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']),
+    ])