nodding_lidar
読み取り中…
検索中…
一致する文字列を見つけられません
slam_launch.py
[詳解]
1"""機体の制御とEKFによる位置推定を起動し、首振り2D LiDARの点群でRTAB-Mapの地図を作る。"""
2from launch import LaunchDescription
3from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription
4from launch.conditions import IfCondition
5from launch.launch_description_sources import PythonLaunchDescriptionSource
6from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution
7from launch_ros.actions import Node
8from launch_ros.substitutions import FindPackageShare
9
10
12 database_path = LaunchConfiguration('database_path')
13 use_sim_time = LaunchConfiguration('use_sim_time')
14 mirs = IncludeLaunchDescription(
15 PythonLaunchDescriptionSource(PathJoinSubstitution([
16 FindPackageShare('mirs'), 'launch', 'mirs_launch.py'])),
17 launch_arguments={
18 'esp_port': LaunchConfiguration('esp_port'),
19 'lidar_port': LaunchConfiguration('lidar_port'),
20 'lidar_baudrate': LaunchConfiguration('lidar_baudrate'),
21 'use_sim_time': LaunchConfiguration('use_sim_time'),
22 'enable_lidar': 'false',
23 'enable_ekf_local': 'true',
24 'enable_static_base_link_to_laser_tf': 'false',
25 'enable_static_base_link_to_imu_link_tf': 'true',
26 'enable_rviz': 'false',
27 'enable_nav2': 'false',
28 'enable_slam_toolbox': 'false',
29 }.items(),
30 )
31 nodding = IncludeLaunchDescription(
32 PythonLaunchDescriptionSource(PathJoinSubstitution([
33 FindPackageShare('nodding_lidar'), 'launch', 'lidar_launch.py'])),
34 launch_arguments={
35 'enable_nodding_lidar': 'true',
36 'servo_serial_port': LaunchConfiguration('servo_angle_port'),
37 'lidar_port': LaunchConfiguration('lidar_port'),
38 'lidar_baudrate': LaunchConfiguration('lidar_baudrate'),
39 'enable_lidar': 'true',
40 'use_sim_time': LaunchConfiguration('use_sim_time'),
41 'fixed_frame': 'odom',
42 'enable_rviz': 'false',
43 }.items(),
44 )
45 rtabmap = Node(
46 package='rtabmap_slam', executable='rtabmap', name='rtabmap',
47 output='screen', parameters=[
48 PathJoinSubstitution([
49 FindPackageShare('nodding_lidar'), 'config', 'rtabmap.yaml']),
50 {'use_sim_time': use_sim_time, 'database_path': database_path},
51 ], remappings=[('scan_cloud', '/points'), ('odom', '/odometry/local')],
52 )
53 rviz = Node(
54 package='rviz2', executable='rviz2', name='rtabmap_3d_rviz',
55 arguments=['-d', PathJoinSubstitution([
56 FindPackageShare('nodding_lidar'), 'rviz', 'rtabmap_3d.rviz'])],
57 parameters=[{'use_sim_time': use_sim_time}], output='screen',
58 condition=IfCondition(LaunchConfiguration('enable_rviz')),
59 )
60 return LaunchDescription([
61 DeclareLaunchArgument('esp_port', default_value='/dev/ttyUSB1'),
62 DeclareLaunchArgument('lidar_port', default_value='/dev/ttyUSB0'),
63 DeclareLaunchArgument('servo_angle_port', default_value='/dev/ttyACM0'),
64 DeclareLaunchArgument('lidar_baudrate', default_value='256000'),
65 DeclareLaunchArgument('use_sim_time', default_value='false'),
66 DeclareLaunchArgument('enable_rviz', default_value='true'),
67 DeclareLaunchArgument(
68 'database_path',
69 default_value=PathJoinSubstitution([
70 EnvironmentVariable('HOME'), '.ros', 'mirs_3d.db']),
71 description='RTAB-Map 3D mapping database'),
72 GroupAction(actions=[mirs], scoped=True),
73 # Keep the child RViz override from disabling the SLAM viewer.
74 GroupAction(actions=[nodding], scoped=True),
75 rtabmap,
76 rviz,
77 ])