nodding_lidar
読み取り中…
検索中…
一致する文字列を見つけられません
bringup_launch.py
[詳解]
1"""機体の制御を起動し、設定に応じて首振り2D LiDARを有効にする。"""
2from launch import LaunchDescription
3from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
4from launch.conditions import IfCondition
5from launch.launch_description_sources import PythonLaunchDescriptionSource
6from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
7from launch_ros.actions import Node
8from launch_ros.substitutions import FindPackageShare
9
10
12 enable_nodding_lidar = LaunchConfiguration('enable_nodding_lidar')
13 lidar_frame = LaunchConfiguration('lidar_frame')
14
15 mirs = IncludeLaunchDescription(
16 PythonLaunchDescriptionSource(
17 PathJoinSubstitution([
18 FindPackageShare('mirs'), 'launch', 'mirs_launch.py']),
19 ),
20 launch_arguments={
21 'esp_port': LaunchConfiguration('esp_port'),
22 'lidar_port': LaunchConfiguration('lidar_port'),
23 'lidar_baudrate': LaunchConfiguration('lidar_baudrate'),
24 'lidar_frame': lidar_frame,
25 'map': LaunchConfiguration('map'),
26 'use_sim_time': LaunchConfiguration('use_sim_time'),
27 # lidar_launch.py owns the driver when nodding mode is enabled.
28 'enable_lidar': PythonExpression([
29 "'", LaunchConfiguration('enable_lidar'), "' == 'true' and '",
30 enable_nodding_lidar, "' == 'false'",
31 ]),
32 'scan_safety_stop_distance': LaunchConfiguration('scan_safety_stop_distance'),
33 'scan_safety_resume_distance': LaunchConfiguration('scan_safety_resume_distance'),
34 'scan_safety_forward_angle': LaunchConfiguration('scan_safety_forward_angle'),
35 'enable_parameter_publisher': LaunchConfiguration('enable_parameter_publisher'),
36 'enable_micro_ros': LaunchConfiguration('enable_micro_ros'),
37 # The EKF provides odom -> base_footprint for scan deskewing and
38 # is the sole local-odometry TF publisher in this bringup.
39 'enable_ekf_local': 'true',
40 'enable_static_base_link_to_imu_link_tf': 'true',
41 'enable_rviz': LaunchConfiguration('enable_rviz'),
42 'enable_nav2': LaunchConfiguration('enable_nav2'),
43 # RTAB-Map replaces slam_toolbox here to avoid competing map->odom TFs.
44 'enable_slam_toolbox': 'false',
45 'enable_static_base_link_to_laser_tf': PythonExpression([
46 "'", enable_nodding_lidar, "' == 'false'",
47 ]),
48 }.items(),
49 )
50
51 nodding_lidar = IncludeLaunchDescription(
52 PythonLaunchDescriptionSource(
53 PathJoinSubstitution([
54 FindPackageShare('nodding_lidar'), 'launch', 'lidar_launch.py']),
55 ),
56 launch_arguments={
57 'enable_nodding_lidar': enable_nodding_lidar,
58 'servo_serial_port': LaunchConfiguration('servo_angle_port'),
59 'lidar_port': LaunchConfiguration('lidar_port'),
60 'lidar_baudrate': LaunchConfiguration('lidar_baudrate'),
61 'lidar_frame': lidar_frame,
62 'enable_lidar': LaunchConfiguration('enable_lidar'),
63 'use_sim_time': LaunchConfiguration('use_sim_time'),
64 'fixed_frame': 'odom',
65 'enable_rviz': LaunchConfiguration('enable_rviz'),
66 'base_to_servo_z': LaunchConfiguration('base_to_servo_z'),
67 }.items(),
68 condition=IfCondition(enable_nodding_lidar),
69 )
70
71 rtabmap = Node(
72 package='rtabmap_slam',
73 executable='rtabmap',
74 name='rtabmap',
75 output='screen',
76 condition=IfCondition(PythonExpression([
77 "'", enable_nodding_lidar, "' == 'true' and '",
78 LaunchConfiguration('enable_rtabmap'), "' == 'true'",
79 ])),
80 parameters=[
81 PathJoinSubstitution([
82 FindPackageShare('nodding_lidar'), 'config', 'rtabmap.yaml']),
83 {'use_sim_time': LaunchConfiguration('use_sim_time')},
84 ],
85 remappings=[
86 ('scan_cloud', '/points'),
87 ('odom', '/odometry/local'),
88 ],
89 )
90
91 return LaunchDescription([
92 DeclareLaunchArgument('enable_nodding_lidar', default_value='true'),
93 DeclareLaunchArgument('enable_rtabmap', default_value='false'),
94 DeclareLaunchArgument('esp_port', default_value='/dev/ttyUSB1'),
95 DeclareLaunchArgument('lidar_port', default_value='/dev/ttyUSB0'),
96 DeclareLaunchArgument('lidar_baudrate', default_value='256000'),
97 DeclareLaunchArgument('servo_angle_port', default_value='/dev/ttyACM0'),
98 DeclareLaunchArgument('lidar_frame', default_value='laser'),
99 DeclareLaunchArgument(
100 'map', default_value=PathJoinSubstitution([
101 FindPackageShare('mirs'), 'maps', 'es.yaml'])),
102 DeclareLaunchArgument('use_sim_time', default_value='false'),
103 DeclareLaunchArgument('enable_lidar', default_value='true'),
104 DeclareLaunchArgument('scan_safety_stop_distance', default_value='0.35'),
105 DeclareLaunchArgument('scan_safety_resume_distance', default_value='0.50'),
106 DeclareLaunchArgument('scan_safety_forward_angle', default_value='30.0'),
107 DeclareLaunchArgument('enable_parameter_publisher', default_value='true'),
108 DeclareLaunchArgument('enable_micro_ros', default_value='true'),
109 DeclareLaunchArgument('enable_rviz', default_value='false'),
110 DeclareLaunchArgument('enable_nav2', default_value='false'),
111 DeclareLaunchArgument('base_to_servo_z', default_value='0.142'),
112 mirs,
113 nodding_lidar,
114 rtabmap,
115 ])