From 18f2788a375ceb7f2827632831a23afa30b5310a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 5 Sep 2024 21:05:50 -0700 Subject: [PATCH] zed.launch.py: Fixed use_zed_odometry arg behavior --- rtabmap_examples/launch/zed.launch.py | 49 +++++++++++++++------------ 1 file changed, 28 insertions(+), 21 deletions(-) diff --git a/rtabmap_examples/launch/zed.launch.py b/rtabmap_examples/launch/zed.launch.py index 9344867e..ccefaee7 100644 --- a/rtabmap_examples/launch/zed.launch.py +++ b/rtabmap_examples/launch/zed.launch.py @@ -8,17 +8,20 @@ import os 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) ])