From 7fb82f8608dfc2977467ec32bb7e2bf2822edc9a Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Wed, 7 Jun 2023 17:29:03 +0300 Subject: [PATCH 1/3] Refactor sim_time and composable node usage in Collision Monitor launch script --- .../launch/collision_monitor_node.launch.py | 95 ++++++++++++------- 1 file changed, 63 insertions(+), 32 deletions(-) diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index d03d723498..b98bd7c16f 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -19,10 +19,12 @@ 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_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node -from nav2_common.launch import RewrittenYaml +from launch_ros.descriptions import ComposableNode def generate_launch_description(): @@ -38,6 +40,9 @@ def generate_launch_description(): 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 +60,57 @@ 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} - - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - 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_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') + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + 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}]), + Node( + package='nav2_collision_monitor', + executable='collision_monitor', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[params_file]) + ] + ) + + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + 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}]), + ComposableNode( + package='nav2_collision_monitor', + plugin='nav2_collision_monitor::CollisionMonitor', + name='collision_monitor', + parameters=[params_file]), + ], + ) + ] + ) ld = LaunchDescription() @@ -89,9 +118,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 From 65c762168955107ed7ad209b8ba6172884379c9c Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Thu, 8 Jun 2023 12:19:16 +0300 Subject: [PATCH 2/3] Added namespace support --- .../launch/collision_monitor_node.launch.py | 30 ++++++++++++++++--- .../params/collision_monitor_params.yaml | 2 +- 2 files changed, 27 insertions(+), 5 deletions(-) diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index b98bd7c16f..95cc44690e 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -22,9 +22,12 @@ 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 def generate_launch_description(): @@ -34,6 +37,8 @@ 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 @@ -68,10 +73,20 @@ def generate_launch_description(): '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={}, + convert_types=True) + + # Declare launch 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', @@ -79,13 +94,15 @@ def generate_launch_description(): output='screen', emulate_tty=True, # https://github.com/ros2/launch/issues/188 parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}]), + {'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=[params_file]) + parameters=[configured_params], + remappings=remappings) ] ) @@ -93,6 +110,9 @@ def generate_launch_description(): 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=[ @@ -101,12 +121,14 @@ def generate_launch_description(): plugin='nav2_lifecycle_manager::LifecycleManager', name='lifecycle_manager_collision_monitor', parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}]), + {'node_names': lifecycle_nodes}], + remappings=remappings), ComposableNode( package='nav2_collision_monitor', plugin='nav2_collision_monitor::CollisionMonitor', name='collision_monitor', - parameters=[params_file]), + parameters=[configured_params], + remappings=remappings) ], ) ] 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" From d92087b49c2c3e6f50a97c6ed22c634a50bf5901 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Thu, 8 Jun 2023 13:01:27 +0300 Subject: [PATCH 3/3] Fix comment --- nav2_collision_monitor/launch/collision_monitor_node.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index 95cc44690e..8811667458 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -79,7 +79,7 @@ def generate_launch_description(): param_rewrites={}, convert_types=True) - # Declare launch commands + # Declare node launching commands load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[