ported rtabmap_ros_pkg_split to ros2

2026-10-11 03:59:53 +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,74 @@
+# Requirements:
+#   A Kinect for Azure
+#   Install Azure_Kinect_ROS_Driver ros2 package (https://github.com/microsoft/Azure_Kinect_ROS_Driver/tree/humble)
+#   Install imu_filter_madgwick ros2 package
+# Example:
+#   $ ros2 launch rtabmap_ros k4a.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_base',
+          'subscribe_rgbd':True,
+          'subscribe_odom_info':True,
+          'qos':1}]
+
+    remappings=[
+          ('imu', '/imu/data'),
+          ('rgb/image', '/rgb/image_raw'),
+          ('rgb/camera_info', '/rgb/camera_info'),
+          ('depth/image', '/depth_to_rgb/image_raw'),
+          ('rgbd_image', 'odom_rgbd_image')]
+
+    return LaunchDescription([
+
+        # Nodes to launch 
+        
+        # Visual odometry node      
+        Node(
+            package='rtabmap_ros', executable='rgbd_odometry', output='screen',
+            parameters=[{ 'frame_id':'camera_base',
+                          'subscribe_odom_info':True,
+                          'approx_sync':True,
+                          'approx_sync_max_interval':0.01,
+                          'wait_imu_to_init':True,
+                          'qos':1,
+                          'queue_size':30,
+                          'keep_color':True,
+                          # Color image needs to be rectified, 
+                          # this will tell vo to rectify them for convenience:
+                          'Rtabmap/ImagesAlreadyRectified':'False'}],
+            remappings=remappings),
+
+        # SLAM node
+        Node(
+            package='rtabmap_ros', executable='rtabmap', output='screen',
+            parameters=parameters,
+            remappings=remappings,
+            arguments=['-d']),
+
+        # Visualization node
+        Node(
+            package='rtabmap_ros', executable='rtabmapviz', output='screen',
+            parameters=parameters,
+            remappings=remappings),
+            
+        # Kinect for azure
+        Node(
+            package='azure_kinect_ros_driver', executable='node', output='screen',
+            parameters=[{'color_enabled': True, 
+                         'fps':15, 
+                         'depth_mode':'WFOV_2X2BINNED'}]),
+        
+        # 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')]),
+    ])