update rtabmap config (#292)

Signed-off-by: Borong Yuan <[email protected]>
luxonis · Apr 25, 2023 · d4666b9 · d4666b9
1 parent c292264
commit d4666b9
Showing 1 changed file with 60 additions and 25 deletions.
diff --git a/depthai_ros_driver/launch/rtabmap.launch.py b/depthai_ros_driver/launch/rtabmap.launch.py
@@ -3,9 +3,11 @@
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
+from launch_ros.actions import LoadComposableNodes, Node
+from launch_ros.descriptions import ComposableNode
 
 def launch_setup(context, *args, **kwargs):
     name = LaunchConfiguration('name').perform(context)
@@ -15,45 +17,77 @@ def launch_setup(context, *args, **kwargs):
     parameters = [
         {
             "frame_id": name,
+            "subscribe_rgb": True,
             "subscribe_depth": True,
+            "subscribe_odom_info": True,
             "approx_sync": True,
             "Rtabmap/DetectionRate": "3.5",
         }
     ]
 
     remappings = [
-        ("rgb/image", name+"/rgb/image_raw"),
+        ("rgb/image", name+"/rgb/image_rect"),
         ("rgb/camera_info", name+"/rgb/camera_info"),
         ("depth/image", name+"/stereo/image_raw"),
     ]
+
     return [
-        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,
-            ),
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
                 os.path.join(depthai_prefix, 'launch', 'camera.launch.py')),
             launch_arguments={"name": name,
-                              "params_file": params_file}.items())
+                              "params_file": params_file}.items()),
+
+        LoadComposableNodes(
+            condition=IfCondition(LaunchConfiguration("rectify_rgb")),
+            target_container=name+"_container",
+            composable_node_descriptions=[
+                ComposableNode(
+                    package="image_proc",
+                    plugin="image_proc::RectifyNode",
+                    name="rectify_color_node",
+                    remappings=[('image', name+'/rgb/image_raw'),
+                                ('camera_info', name+'/rgb/camera_info'),
+                                ('image_rect', name+'/rgb/image_rect'),
+                                ('image_rect/compressed', name+'/rgb/image_rect/compressed'),
+                                ('image_rect/compressedDepth', name+'/rgb/image_rect/compressedDepth'),
+                                ('image_rect/theora', name+'/rgb/image_rect/theora')]
+                )
+            ]),
+
+        LoadComposableNodes(
+            target_container=name+"_container",
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='rtabmap_ros',
+                    plugin='rtabmap_ros::RGBDOdometry',
+                    name='rgbd_odometry',
+                    parameters=parameters,
+                    remappings=remappings,
+                ),
+            ],
+        ),
+
+        LoadComposableNodes(
+            target_container=name+"_container",
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='rtabmap_ros',
+                    plugin='rtabmap_ros::CoreWrapper',
+                    name='rtabmap',
+                    parameters=parameters,
+                    remappings=remappings,
+                ),
+            ],
+        ),
+
+        Node(
+            package="rtabmap_ros",
+            executable="rtabmapviz",
+            output="screen",
+            parameters=parameters,
+            remappings=remappings,
+        ),
     ]
 
 
@@ -62,6 +96,7 @@ def generate_launch_description():
     declared_arguments = [
         DeclareLaunchArgument("name", default_value="oak"),
         DeclareLaunchArgument("params_file", default_value=os.path.join(depthai_prefix, 'config', 'rgbd.yaml')),
+        DeclareLaunchArgument("rectify_rgb", default_value="True"),
     ]
 
     return LaunchDescription(