ellensohn/cps_rmp220_support
1
0
Fork 0
mirror of https://github.com/bjoernellens1/cps_rmp220_support.git synced 2024-11-23 15:45:08 +00:00
Code Issues Packages Projects Releases Wiki Activity

added amcl node

Browse Source
This commit is contained in:
Björn Ellensohn 2023-08-22 12:21:33 +02:00
parent 10c6fb2164
commit 0608d4f2b5
1 changed files with 39 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
launch/robot_amcl.launch.py
View File

@ -17,15 +17,12 @@ import os
from ament_index_python.packages import get_package_share_directory from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
from launch.actions import SetEnvironmentVariable
from launch.conditions import IfCondition from launch.conditions import IfCondition
from launch.substitutions import EqualsSubstitution
from launch.substitutions import LaunchConfiguration, PythonExpression from launch.substitutions import LaunchConfiguration, PythonExpression
from launch.substitutions import NotEqualsSubstitution from launch_ros.actions import LoadComposableNodes
from launch_ros.actions import LoadComposableNodes, SetParameter
from launch_ros.actions import Node 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 from nav2_common.launch import RewrittenYaml
@ -55,13 +52,16 @@ def generate_launch_description():
remappings = [('/tf', 'tf'), remappings = [('/tf', 'tf'),
('/tf_static', 'tf_static')] ('/tf_static', 'tf_static')]
configured_params = ParameterFile( # Create our own temporary YAML files that include substitutions
RewrittenYaml( param_substitutions = {
source_file=params_file, 'use_sim_time': use_sim_time,
root_key=namespace, 'yaml_filename': map_yaml_file}
param_rewrites={},
convert_types=True), configured_params = RewrittenYaml(
allow_substs=True) source_file=params_file,
root_key=namespace,
param_rewrites=param_substitutions,
convert_types=True)
stdout_linebuf_envvar = SetEnvironmentVariable( stdout_linebuf_envvar = SetEnvironmentVariable(
'RCUTILS_LOGGING_BUFFERED_STREAM', '1') 'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
@ -73,7 +73,6 @@ def generate_launch_description():
declare_map_yaml_cmd = DeclareLaunchArgument( declare_map_yaml_cmd = DeclareLaunchArgument(
'map', 'map',
default_value='',
description='Full path to map yaml file to load') description='Full path to map yaml file to load')
declare_use_sim_time_cmd = DeclareLaunchArgument( declare_use_sim_time_cmd = DeclareLaunchArgument(
@ -109,9 +108,7 @@ def generate_launch_description():
load_nodes = GroupAction( load_nodes = GroupAction(
condition=IfCondition(PythonExpression(['not ', use_composition])), condition=IfCondition(PythonExpression(['not ', use_composition])),
actions=[ actions=[
SetParameter('use_sim_time', use_sim_time),
Node( Node(
condition=IfCondition(EqualsSubstitution(LaunchConfiguration('map'), '')),
package='nav2_map_server', package='nav2_map_server',
executable='map_server', executable='map_server',
name='map_server', name='map_server',
@ -121,18 +118,6 @@ def generate_launch_description():
parameters=[configured_params], parameters=[configured_params],
arguments=['--ros-args', '--log-level', log_level], arguments=['--ros-args', '--log-level', log_level],
remappings=remappings), 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( Node(
package='nav2_amcl', package='nav2_amcl',
executable='amcl', executable='amcl',
@ -149,62 +134,36 @@ def generate_launch_description():
name='lifecycle_manager_localization', name='lifecycle_manager_localization',
output='screen', output='screen',
arguments=['--ros-args', '--log-level', log_level], arguments=['--ros-args', '--log-level', log_level],
parameters=[{'autostart': autostart}, parameters=[{'use_sim_time': use_sim_time},
{'autostart': autostart},
{'node_names': lifecycle_nodes}]) {'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 load_composable_nodes = LoadComposableNodes(
# 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(
condition=IfCondition(use_composition), condition=IfCondition(use_composition),
actions=[ target_container=container_name_full,
SetParameter('use_sim_time', use_sim_time), composable_node_descriptions=[
LoadComposableNodes( ComposableNode(
target_container=container_name_full, package='nav2_map_server',
condition=IfCondition(EqualsSubstitution(LaunchConfiguration('map'), '')), plugin='nav2_map_server::MapServer',
composable_node_descriptions=[ name='map_server',
ComposableNode( parameters=[configured_params],
package='nav2_map_server', remappings=remappings),
plugin='nav2_map_server::MapServer', ComposableNode(
name='map_server', package='nav2_amcl',
parameters=[configured_params], plugin='nav2_amcl::AmclNode',
remappings=remappings), name='amcl',
], parameters=[configured_params],
), remappings=remappings),
LoadComposableNodes( ComposableNode(
target_container=container_name_full, package='nav2_lifecycle_manager',
condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('map'), '')), plugin='nav2_lifecycle_manager::LifecycleManager',
composable_node_descriptions=[ name='lifecycle_manager_localization',
ComposableNode( parameters=[{'use_sim_time': use_sim_time,
package='nav2_map_server', 'autostart': autostart,
plugin='nav2_map_server::MapServer', 'node_names': lifecycle_nodes}]),
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}]),
],
)
]
) )
# Create the launch description and populate # Create the launch description and populate