From d90118218f8a229066b2d4cca727310a6dfed2e7 Mon Sep 17 00:00:00 2001 From: stevedanomodolor Date: Tue, 6 Sep 2022 06:03:01 +0200 Subject: [PATCH 1/2] implement launch priority for the mapserver parameter yaml_filename minor fix fix commit reincluded rewritten function comment remaining lines for yaml_filename removed default_value issue with composable node alternative soltion to the condition param not working in composable node remove unused import remove comments and reorder composablenode execution fixing commit fixing format fixing lint Update nav2_bringup/params/nav2_params.yaml Co-authored-by: Steve Macenski --- nav2_bringup/launch/bringup_launch.py | 7 +-- nav2_bringup/launch/localization_launch.py | 51 +++++++++++++++++++--- nav2_bringup/params/nav2_params.yaml | 11 ++--- 3 files changed, 52 insertions(+), 17 deletions(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 065337adb85..8fddcc32531 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -53,14 +53,10 @@ def generate_launch_description(): remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'yaml_filename': map_yaml_file} - configured_params = RewrittenYaml( source_file=params_file, root_key=namespace, - param_rewrites=param_substitutions, + param_rewrites={}, convert_types=True) stdout_linebuf_envvar = SetEnvironmentVariable( @@ -83,6 +79,7 @@ def generate_launch_description(): declare_map_yaml_cmd = DeclareLaunchArgument( 'map', + default_value='', description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index dca5bf23e42..1630aca6255 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -17,8 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.actions import SetEnvironmentVariable from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node @@ -51,14 +54,10 @@ def generate_launch_description(): remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'yaml_filename': map_yaml_file} - configured_params = RewrittenYaml( source_file=params_file, root_key=namespace, - param_rewrites=param_substitutions, + param_rewrites={}, convert_types=True) stdout_linebuf_envvar = SetEnvironmentVariable( @@ -71,6 +70,7 @@ def generate_launch_description(): declare_map_yaml_cmd = DeclareLaunchArgument( 'map', + default_value='', description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( @@ -108,6 +108,7 @@ def generate_launch_description(): actions=[ SetParameter("use_sim_time", use_sim_time), Node( + condition=LaunchConfigurationEquals('map', ''), package='nav2_map_server', executable='map_server', name='map_server', @@ -117,6 +118,18 @@ def generate_launch_description(): parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], remappings=remappings), + Node( + condition=LaunchConfigurationNotEquals('map', ''), + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params, + {'yaml_filename': map_yaml_file}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), Node( package='nav2_amcl', executable='amcl', @@ -137,13 +150,19 @@ def generate_launch_description(): {'node_names': lifecycle_nodes}]) ] ) - + # LoadComposableNode for map server twice depending if we should use the + # value of map from a CLI or launch default or user defined value in the + # yaml configuration file. They are separated since the conditions + # currently only work on the LoadComposableNodes commands and not on the + # ComposableNode node function itself + # See https://github.com/ros2/launch_ros/issues/328 load_composable_nodes = GroupAction( condition=IfCondition(use_composition), actions=[ SetParameter("use_sim_time", use_sim_time), LoadComposableNodes( target_container=container_name, + condition=LaunchConfigurationEquals('map', ''), composable_node_descriptions=[ ComposableNode( package='nav2_map_server', @@ -151,6 +170,24 @@ def generate_launch_description(): name='map_server', parameters=[configured_params], remappings=remappings), + ], + ), + LoadComposableNodes( + target_container=container_name, + condition=LaunchConfigurationNotEquals('map', ''), + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='map_server', + parameters=[configured_params, + {'yaml_filename': map_yaml_file}], + remappings=remappings), + ], + ), + LoadComposableNodes( + target_container=container_name, + composable_node_descriptions=[ ComposableNode( package='nav2_amcl', plugin='nav2_amcl::AmclNode', diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 0bd4f4e45ba..4577f495507 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -243,11 +243,12 @@ global_costmap: inflation_radius: 0.55 always_send_full_costmap: True -map_server: - ros__parameters: - # Overridden in launch by the "map" launch configuration or provided default value. - # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. - yaml_filename: "" +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" map_saver: ros__parameters: From a2a2af4343e28d5a3eb7b99db1cf9d3e89879f4a Mon Sep 17 00:00:00 2001 From: stevedanomodolor Date: Mon, 7 Nov 2022 20:49:38 +0100 Subject: [PATCH 2/2] state new ros-rolling release changes and deprecation --- nav2_bringup/launch/localization_launch.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 1630aca6255..f2469197bd9 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -155,7 +155,12 @@ def generate_launch_description(): # yaml configuration file. They are separated since the conditions # currently only work on the LoadComposableNodes commands and not on the # ComposableNode node function itself - # See https://github.com/ros2/launch_ros/issues/328 + # EqualsSubstitution and NotEqualsSubstitution susbsitutions was recently + # added to solve this problem but it has not been ported yet to + # ros-rolling. See https://github.com/ros2/launch_ros/issues/328. + # LaunchConfigurationEquals and LaunchConfigurationNotEquals are scheduled + # for deprecation once a Rolling sync is conducted. Switching to this new + # would be required for both ComposableNode and normal nodes. load_composable_nodes = GroupAction( condition=IfCondition(use_composition), actions=[