From 0608d4f2b5b78d007c8f2a154b912b4214660bad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= Date: Tue, 22 Aug 2023 12:21:33 +0200 Subject: [PATCH] added amcl node --- launch/robot_amcl.launch.py | 119 ++++++++++++------------------------ 1 file changed, 39 insertions(+), 80 deletions(-) diff --git a/launch/robot_amcl.launch.py b/launch/robot_amcl.launch.py index bb2b8fd..fafee46 100644 --- a/launch/robot_amcl.launch.py +++ b/launch/robot_amcl.launch.py @@ -17,15 +17,12 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction -from launch.actions import SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition -from launch.substitutions import EqualsSubstitution from launch.substitutions import LaunchConfiguration, PythonExpression -from launch.substitutions import NotEqualsSubstitution -from launch_ros.actions import LoadComposableNodes, SetParameter +from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode, ParameterFile +from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml @@ -55,13 +52,16 @@ def generate_launch_description(): remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - configured_params = ParameterFile( - RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites={}, - convert_types=True), - allow_substs=True) + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'yaml_filename': map_yaml_file} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') @@ -73,7 +73,6 @@ 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( @@ -109,9 +108,7 @@ def generate_launch_description(): load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ - SetParameter('use_sim_time', use_sim_time), Node( - condition=IfCondition(EqualsSubstitution(LaunchConfiguration('map'), '')), package='nav2_map_server', executable='map_server', name='map_server', @@ -121,18 +118,6 @@ def generate_launch_description(): parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], remappings=remappings), - Node( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('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', @@ -149,62 +134,36 @@ def generate_launch_description(): name='lifecycle_manager_localization', output='screen', arguments=['--ros-args', '--log-level', log_level], - parameters=[{'autostart': autostart}, + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, {'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 - load_composable_nodes = GroupAction( + + load_composable_nodes = LoadComposableNodes( condition=IfCondition(use_composition), - actions=[ - SetParameter('use_sim_time', use_sim_time), - LoadComposableNodes( - target_container=container_name_full, - condition=IfCondition(EqualsSubstitution(LaunchConfiguration('map'), '')), - composable_node_descriptions=[ - ComposableNode( - package='nav2_map_server', - plugin='nav2_map_server::MapServer', - name='map_server', - parameters=[configured_params], - remappings=remappings), - ], - ), - LoadComposableNodes( - target_container=container_name_full, - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('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_full, - composable_node_descriptions=[ - ComposableNode( - package='nav2_amcl', - plugin='nav2_amcl::AmclNode', - name='amcl', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_lifecycle_manager', - plugin='nav2_lifecycle_manager::LifecycleManager', - name='lifecycle_manager_localization', - parameters=[{'autostart': autostart, - 'node_names': lifecycle_nodes}]), - ], - ) - ] + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='map_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_amcl', + plugin='nav2_amcl::AmclNode', + name='amcl', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_localization', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], ) # Create the launch description and populate