60 lines
2.4 KiB
Python
60 lines
2.4 KiB
Python
|
import os
|
||
|
|
|
||
|
|
from launch import LaunchDescription
|
||
|
|
from launch.actions import DeclareLaunchArgument, LogInfo
|
||
|
|
from launch.conditions import UnlessCondition
|
||
|
|
from launch.substitutions import LaunchConfiguration, PythonExpression
|
||
|
|
from launch_ros.actions import Node
|
||
|
|
from ament_index_python.packages import get_package_share_directory
|
||
|
|
from nav2_common.launch import HasNodeParams
|
||
|
|
|
||
|
|
|
||
|
|
def generate_launch_description():
|
||
|
|
use_sim_time = LaunchConfiguration('use_sim_time', default=False)
|
||
|
|
params_file = LaunchConfiguration('params_file')
|
||
|
|
default_params_file = os.path.join(get_package_share_directory("sportmode_nav2"),
|
||
|
|
'params', 'localization_async.yaml')
|
||
|
|
|
||
|
|
declare_use_sim_time_argument = DeclareLaunchArgument(
|
||
|
|
'use_sim_time',
|
||
|
|
default_value='false',
|
||
|
|
description='Use simulation/Gazebo clock')
|
||
|
|
declare_params_file_cmd = DeclareLaunchArgument(
|
||
|
|
'params_file',
|
||
|
|
default_value=default_params_file,
|
||
|
|
description='Full path to the ROS2 parameters file to use for the slam_toolbox node')
|
||
|
|
|
||
|
|
# If the provided param file doesn't have slam_toolbox params, we must pass the
|
||
|
|
# default_params_file instead. This could happen due to automatic propagation of
|
||
|
|
# LaunchArguments. See:
|
||
|
|
# https://github.com/ros-planning/navigation2/pull/2243#issuecomment-800479866
|
||
|
|
has_node_params = HasNodeParams(source_file=params_file,
|
||
|
|
node_name='slam_toolbox')
|
||
|
|
|
||
|
|
actual_params_file = PythonExpression(['"', params_file, '" if ', has_node_params,
|
||
|
|
' else "', default_params_file, '"'])
|
||
|
|
|
||
|
|
log_param_change = LogInfo(msg=['provided params_file ', params_file,
|
||
|
|
' does not contain slam_toolbox parameters. Using default: ',
|
||
|
|
default_params_file],
|
||
|
|
condition=UnlessCondition(has_node_params))
|
||
|
|
|
||
|
|
start_async_slam_toolbox_node = Node(
|
||
|
|
parameters=[
|
||
|
|
actual_params_file,
|
||
|
|
{'use_sim_time': use_sim_time}
|
||
|
|
],
|
||
|
|
package='slam_toolbox',
|
||
|
|
executable='async_slam_toolbox_node',
|
||
|
|
name='slam_toolbox',
|
||
|
|
output='screen')
|
||
|
|
|
||
|
|
ld = LaunchDescription()
|
||
|
|
|
||
|
|
ld.add_action(declare_use_sim_time_argument)
|
||
|
|
ld.add_action(declare_params_file_cmd)
|
||
|
|
ld.add_action(log_param_change)
|
||
|
|
ld.add_action(start_async_slam_toolbox_node)
|
||
|
|
|
||
|
|
return ld
|