zed.launch.py: Fixed use_zed_odometry arg behavior

rtabmap_examples/launch/zed.launch.py

@@ -8,17 +8,20 @@
 
 from ament_index_python.packages import get_package_share_directory
 
-from launch import LaunchDescription
+from launch import LaunchDescription, LaunchContext
 from launch.actions import DeclareLaunchArgument
 from launch_ros.actions import Node
-from launch.actions import IncludeLaunchDescription
+from launch.actions import IncludeLaunchDescription, OpaqueFunction
 from launch.substitutions import LaunchConfiguration
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.conditions import UnlessCondition
 
 import tempfile
 
-def generate_launch_description():
+parameters = []
+remappings = []
+
+def launch_setup(context: LaunchContext, *args, **kwargs):
 
     # Hack to override grab_resolution parameter without changing any files
     with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file:
@@ -28,36 +31,28 @@ def generate_launch_description():
                   "        general:\n"+
                   "            grab_resolution: 'VGA'")
 
-    camera_model = LaunchConfiguration('camera_model')
-    use_zed_odometry = LaunchConfiguration('use_zed_odometry')
-
     parameters=[{'frame_id':'zed_camera_link',
                  'subscribe_rgbd':True,
-                 'subscribe_odom_info': not use_zed_odometry,
                  'approx_sync':False,
                  'wait_imu_to_init':True}]
 
     remappings=[('imu', '/zed/zed_node/imu/data')]
 
-    if use_zed_odometry:
+    if LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"]:
         remappings.append(('odom', '/zed/zed_node/odom'))
-
-    return LaunchDescription([
-
-        # Launch arguments
-        DeclareLaunchArgument(
-            'use_zed_odometry', default_value='false',
-            description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'),
-
+    else:
+        parameters.append({'subscribe_odom_info': True})
+
+    return [
         # Launch camera driver
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource([os.path.join(
                 get_package_share_directory('zed_wrapper'), 'launch'),
                 '/zed_camera.launch.py']),
-                launch_arguments={'camera_model': camera_model,
-                                  'ros_params_override_path': zed_override_file.name,
-                                  'publish_tf': use_zed_odometry,
-                                  'publish_map_tf': 'false'}.items(),
+                launch_arguments={'camera_model': LaunchConfiguration('camera_model'),
+                                    'ros_params_override_path': zed_override_file.name,
+                                    'publish_tf': LaunchConfiguration('use_zed_odometry'),
+                                    'publish_map_tf': 'false'}.items(),
         ),
 
         # Sync right/depth/camera_info together
@@ -71,7 +66,7 @@ def generate_launch_description():
         # Visual odometry
         Node(
             package='rtabmap_odom', executable='rgbd_odometry', output='screen',
-            condition=UnlessCondition(use_zed_odometry),
+            condition=UnlessCondition(LaunchConfiguration('use_zed_odometry')),
             parameters=parameters,
             remappings=remappings,),
 
@@ -87,4 +82,16 @@ def generate_launch_description():
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',
             parameters=parameters,
             remappings=remappings)
+    ]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_zed_odometry', default_value='false',
+            description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'),
+
+        OpaqueFunction(function=launch_setup)
     ])