diff --git a/ouster-ros/config/os_sensor_cloud_image_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml index 063774f4..b6241be2 100644 --- a/ouster-ros/config/os_sensor_cloud_image_params.yaml +++ b/ouster-ros/config/os_sensor_cloud_image_params.yaml @@ -29,4 +29,5 @@ ouster/os_cloud: # value the range [0, sensor_beams_count) point_type: original # choose from: {original, native, xyz, xyzi, xyzir} ouster/os_image: + ros__parameters: use_system_default_qos: False # needs to match the value defined for os_sensor node diff --git a/ouster-ros/launch/sensor.composite.launch.py b/ouster-ros/launch/sensor.composite.launch.py index 5932403f..4abec6ed 100644 --- a/ouster-ros/launch/sensor.composite.launch.py +++ b/ouster-ros/launch/sensor.composite.launch.py @@ -4,44 +4,36 @@ """Launch ouster nodes using a composite container""" from pathlib import Path -import launch +from launch import LaunchDescription, LaunchContext, LaunchDescriptionEntity from ament_index_python.packages import get_package_share_directory from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, - ExecuteProcess, TimerAction) + ExecuteProcess, TimerAction, OpaqueFunction) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, FindExecutable +import yaml +from typing import Optional, List -def generate_launch_description(): +def composable_node_setup(context: LaunchContext) -> Optional[List[LaunchDescriptionEntity]]: """ - Generate launch description for running ouster_ros components in a single - process/container. + Generating launch description for composable nodes with correctly parsed node parameters, + this is a workaround for issue https://github.com/ros2/launch_ros/issues/156 in foxy. """ - ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') - default_params_file = \ - Path(ouster_ros_pkg_dir) / 'config' / 'os_sensor_cloud_image_params.yaml' params_file = LaunchConfiguration('params_file') - params_file_arg = DeclareLaunchArgument('params_file', - default_value=str( - default_params_file), - description='name or path to the parameters file to use.') - ouster_ns = LaunchConfiguration('ouster_ns') - ouster_ns_arg = DeclareLaunchArgument( - 'ouster_ns', default_value='ouster') - rviz_enable = LaunchConfiguration('viz') - rviz_enable_arg = DeclareLaunchArgument('viz', default_value='True') + with open(params_file.perform(context), 'r') as f: + ros_params = yaml.safe_load(f) os_sensor = ComposableNode( package='ouster_ros', plugin='ouster_ros::OusterSensor', name='os_sensor', namespace=ouster_ns, - parameters=[params_file] + parameters=[ros_params[ouster_ns.perform(context) + '/os_sensor']['ros__parameters']] ) os_cloud = ComposableNode( @@ -49,7 +41,7 @@ def generate_launch_description(): plugin='ouster_ros::OusterCloud', name='os_cloud', namespace=ouster_ns, - parameters=[params_file] + parameters=[ros_params[ouster_ns.perform(context) + '/os_cloud']['ros__parameters']] ) os_image = ComposableNode( @@ -57,10 +49,10 @@ def generate_launch_description(): plugin='ouster_ros::OusterImage', name='os_image', namespace=ouster_ns, - parameters=[params_file] + parameters=[ros_params[ouster_ns.perform(context) + '/os_image']['ros__parameters']] ) - os_container = ComposableNodeContainer( + return [ComposableNodeContainer( name='os_container', namespace=ouster_ns, package='rclcpp_components', @@ -71,7 +63,28 @@ def generate_launch_description(): os_image ], output='screen', - ) + )] + + +def generate_launch_description(): + """ + Generate launch description for running ouster_ros components in a single + process/container. + """ + ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') + default_params_file = \ + Path(ouster_ros_pkg_dir) / 'config' / 'os_sensor_cloud_image_params.yaml' + params_file_arg = DeclareLaunchArgument('params_file', + default_value=str( + default_params_file), + description='name or path to the parameters file to use.') + + ouster_ns = LaunchConfiguration('ouster_ns') + ouster_ns_arg = DeclareLaunchArgument( + 'ouster_ns', default_value='ouster') + + rviz_enable = LaunchConfiguration('viz') + rviz_enable_arg = DeclareLaunchArgument('viz', default_value='True') rviz_launch_file_path = \ Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py' @@ -93,12 +106,12 @@ def invoke_lifecycle_cmd(node_name, verb): sensor_configure_cmd = invoke_lifecycle_cmd('os_sensor', 'configure') sensor_activate_cmd = invoke_lifecycle_cmd('os_sensor', 'activate') - return launch.LaunchDescription([ + return LaunchDescription([ params_file_arg, ouster_ns_arg, rviz_enable_arg, rviz_launch, - os_container, + OpaqueFunction(function=composable_node_setup), sensor_configure_cmd, TimerAction(period=1.0, actions=[sensor_activate_cmd]) ])