diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index d03d723498..8811667458 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -19,9 +19,14 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import NotEqualsSubstitution +from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node +from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml @@ -32,12 +37,17 @@ def generate_launch_description(): # Constant parameters lifecycle_nodes = ['collision_monitor'] autostart = True + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] # Launch arguments # 1. Create the launch configuration variables namespace = LaunchConfiguration('namespace') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) # 2. Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( @@ -55,33 +65,74 @@ def generate_launch_description(): default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'), description='Full path to the ROS2 parameters file to use for all launched nodes') - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'use_sim_time': use_sim_time} + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='True', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') configured_params = RewrittenYaml( source_file=params_file, root_key=namespace, - param_rewrites=param_substitutions, + param_rewrites={}, convert_types=True) - # Nodes launching commands - start_lifecycle_manager_cmd = Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager', - output='screen', - emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]) - - start_collision_monitor_cmd = Node( - package='nav2_collision_monitor', - executable='collision_monitor', - output='screen', - emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[configured_params]) + # Declare node launching commands + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushRosNamespace( + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), + namespace=namespace), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_collision_monitor', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}], + remappings=remappings), + Node( + package='nav2_collision_monitor', + executable='collision_monitor', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params], + remappings=remappings) + ] + ) + + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushRosNamespace( + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), + namespace=namespace), + LoadComposableNodes( + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_collision_monitor', + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}], + remappings=remappings), + ComposableNode( + package='nav2_collision_monitor', + plugin='nav2_collision_monitor::CollisionMonitor', + name='collision_monitor', + parameters=[configured_params], + remappings=remappings) + ], + ) + ] + ) ld = LaunchDescription() @@ -89,9 +140,11 @@ def generate_launch_description(): ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) # Node launching commands - ld.add_action(start_lifecycle_manager_cmd) - ld.add_action(start_collision_monitor_cmd) + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) return ld diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 25bf9335f1..ffc7981e2a 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -50,7 +50,7 @@ collision_monitor: observation_sources: ["scan"] scan: type: "scan" - topic: "/scan" + topic: "scan" pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points"