65 lines
1.9 KiB
Python
Executable File
65 lines
1.9 KiB
Python
Executable File
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument
|
|
from launch.actions import GroupAction
|
|
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
|
from launch_ros.actions import Node
|
|
from launch_ros.descriptions import ParameterFile
|
|
from launch.substitutions import PythonExpression
|
|
from launch.conditions import IfCondition, UnlessCondition
|
|
from nav2_common.launch import RewrittenYaml
|
|
|
|
|
|
ARGUMENTS = [
|
|
DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09']),
|
|
]
|
|
|
|
def generate_launch_description():
|
|
robot = LaunchConfiguration('robot')
|
|
|
|
remappings=[
|
|
('/tf', 'tf'),
|
|
('/tf_static', 'tf_static'),
|
|
('odom', 'icp_odom'),
|
|
('scan_cloud', '/lidar_filtered')]
|
|
|
|
# Directories
|
|
pkg_localization = get_package_share_directory(
|
|
'localization_pkg')
|
|
|
|
rtabmap_params_file = PathJoinSubstitution([
|
|
pkg_localization,
|
|
'config',
|
|
PythonExpression(["'rtabmap_' + '", robot, "' + '.yaml'"])])
|
|
param_substitutions = {
|
|
'use_sim_time': 'true'
|
|
}
|
|
configured_params = ParameterFile(
|
|
RewrittenYaml(
|
|
source_file=rtabmap_params_file,
|
|
param_rewrites=param_substitutions,
|
|
convert_types=True),
|
|
allow_substs=True)
|
|
|
|
load_nodes = GroupAction(
|
|
actions=[
|
|
Node(
|
|
package='rtabmap_odom',
|
|
executable='icp_odometry',
|
|
name='icp_odometry',
|
|
output='screen',
|
|
parameters=[configured_params, {'use_sim_time': True}],
|
|
remappings=remappings,
|
|
)
|
|
],
|
|
)
|
|
|
|
# Create launch description and add actions
|
|
ld = LaunchDescription(ARGUMENTS)
|
|
ld.add_action(load_nodes)
|
|
|
|
return ld
|
|
|