# Copyright (c) 2018 Intel Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import (DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, SetEnvironmentVariable) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.actions import PushRosNamespace from launch_ros.descriptions import ParameterFile from nav2_common.launch import RewrittenYaml, ReplaceString def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('plc_bringup') launch_dir = os.path.join(bringup_dir, 'launch') # Create the launch configuration variables namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') slam = LaunchConfiguration('slam') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') # Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative # https://github.com/ros/geometry2/issues/32 # https://github.com/ros/robot_state_publisher/pull/30 # TODO(orduno) Substitute with `PushNodeRemapping` # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Create our own temporary YAML files that include substitutions param_substitutions = { 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file} # Only it applys when `use_namespace` is True. # '' keyword shall be replaced by 'namespace' launch argument # in config file 'nav2_multirobot_params.yaml' as a default & example. # User defined config file should contain '' keyword for the replacements. params_file = ReplaceString( source_file=params_file, replacements={'': ('/', namespace)}, condition=IfCondition(use_namespace)) configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites=param_substitutions, convert_types=True), allow_substs=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value='', description='Top-level namespace') declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', description='Whether to apply a namespace to the navigation stack') declare_slam_cmd = DeclareLaunchArgument( 'slam', default_value='False', description='Whether run a SLAM') declare_map_yaml_cmd = DeclareLaunchArgument( 'map', description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true') declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), description='Full path to the ROS2 parameters file to use for all launched nodes') declare_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='true', description='Automatically startup the nav2 stack') declare_use_composition_cmd = DeclareLaunchArgument( 'use_composition', default_value='True', description='Whether to use composed bringup') declare_use_respawn_cmd = DeclareLaunchArgument( 'use_respawn', default_value='False', description='Whether to respawn if a node crashes. Applied when composition is disabled.') declare_log_level_cmd = DeclareLaunchArgument( 'log_level', default_value='info', description='log level') # Specify the actions bringup_cmd_group = GroupAction([ PushRosNamespace( condition=IfCondition(use_namespace), namespace=namespace), Node( condition=IfCondition(use_composition), name='nav2_container', package='rclcpp_components', executable='component_container_isolated', parameters=[configured_params, {'autostart': autostart}], arguments=['--ros-args', '--log-level', log_level], remappings=remappings, output='screen'), IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')), condition=IfCondition(slam), launch_arguments={'namespace': namespace, 'use_sim_time': use_sim_time, 'autostart': autostart, 'use_respawn': use_respawn, 'params_file': params_file}.items()), # Node( # package='nav2_map_server', # executable='map_server', # name='map_server', # output='screen', # respawn=use_respawn, # respawn_delay=2.0, # parameters={'map':map_yaml_file}, # arguments=['--ros-args', '--log-level', log_level], # remappings=remappings), # 直接发布 odom 到 map 省去amcl Node( package='tf2_ros', executable='static_transform_publisher', arguments = ['--x', '0', '--y', '0', '--z', '0', '--yaw', '0', '--pitch', '0', '--roll', '0', '--frame-id', 'map', '--child-frame-id', 'odom']), IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'localization_launch.py')), condition=IfCondition(PythonExpression(['not ', slam])), launch_arguments={'namespace': namespace, 'map': map_yaml_file, 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file, 'use_composition': use_composition, 'use_respawn': use_respawn, 'container_name': 'nav2_container'}.items()), IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')), launch_arguments={'namespace': namespace, 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file, 'use_composition': use_composition, 'use_respawn': use_respawn, 'container_name': 'nav2_container'}.items()), ]) # Create the launch description and populate ld = LaunchDescription() # Set environment variables ld.add_action(stdout_linebuf_envvar) # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_use_composition_cmd) ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_log_level_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) return ld