coverage
読み取り中…
検索中…
一致する文字列を見つけられません
production.launch.py
[詳解]
1"""MIRS車体、Nav2、coverage server、BLE受信、bridgeをまとめて起動する。
2
3地図生成・coverage経路計算・走行追従は、それぞれSLAM/Nav2/coverage serverが担う。
4個別機能はlaunch引数で有効・無効を選択できる。
5"""
6
7from launch import LaunchDescription
8from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo
9from launch.conditions import IfCondition
10from launch.launch_description_sources import PythonLaunchDescriptionSource
11from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
12from launch_ros.actions import Node
13from launch_ros.substitutions import FindPackageShare
14
15
16def _arg(name, default, description=''):
17 return DeclareLaunchArgument(name, default_value=default, description=description)
18
19
21 mirs_share = FindPackageShare('mirs')
22 coverage_share = FindPackageShare('coverage')
23
24 default_map = PathJoinSubstitution([mirs_share, 'maps', 'youkanato.yaml'])
25 ekf_config_file = PathJoinSubstitution(
26 [mirs_share, 'config', 'ekf', 'ekf_params.yaml'])
27 nav2_params = PathJoinSubstitution(
28 [mirs_share, 'config', 'navigation', 'nav2_params.yaml'])
29 default_behavior_tree = PathJoinSubstitution(
30 [coverage_share, 'behavior_trees', 'navigate_coverage_w_recovery.xml'])
31 rviz_config = PathJoinSubstitution([coverage_share, 'rviz', 'coverage.rviz'])
32
33 args = [
34 _arg('map', default_map, 'Map yaml for Nav2'),
35 _arg('nav2_params_file', nav2_params, 'Nav2 parameter file'),
36 _arg('use_sim_time', 'false'),
37 _arg('use_rviz', 'true', 'Start RViz with coverage.rviz'),
38 _arg('esp_port', '/dev/ttyUSB1'),
39 _arg('lidar_port', '/dev/ttyUSB0'),
40 _arg('lidar_baudrate', '256000'),
41 _arg('lidar_frame', 'laser'),
42 _arg('enable_lidar', 'true'),
43 _arg('enable_micro_ros', 'true'),
44 _arg('enable_hardware', 'true', 'Start mirs base and sensor nodes'),
45 _arg('enable_nav2', 'true', 'Include nav2_bringup'),
46 _arg('enable_coverage_server', 'true', 'Start opennav_coverage server'),
47 _arg('enable_ble', 'true', 'Start ble_receiver_node (falseで手動テスト用)'),
48 _arg('enable_bridge', 'true', 'Start coverage_bridge'),
49 _arg('enable_monitor', 'true', 'Publish coverage diagnostics'),
50 _arg('enable_ekf_local', 'true', 'EKF odom->base_footprint (off時はstatic TFで代用可)'),
51 _arg('ekf_config_file', ekf_config_file, 'EKF parameter file'),
52 _arg('enable_static_base_link_to_laser_tf', 'true'),
53 _arg('enable_static_base_footprint_to_base_link_tf', 'true'),
54 _arg('enable_static_base_link_to_imu_link_tf', 'false'),
55 _arg('autostart', 'true', 'Autostart Nav2 lifecycle'),
56 # nav2_bringupのPythonExpressionで評価されるためPython bool表記を渡す。
57 _arg('slam', 'False', 'Set True for SLAM mode (no map_server)'),
58 _arg('zone_topic', '/cleaning_zone'),
59 _arg('state_topic', '/coverage/state'),
60 _arg('behavior_tree', default_behavior_tree,
61 'BT XML override (default=recovery BT, empty=server default)'),
62 ]
63
64 # mirs_launch.py API経由で機体、Nav2、安全ゲートを起動する。
65 mirs_bringup = IncludeLaunchDescription(
66 PythonLaunchDescriptionSource(
67 PathJoinSubstitution([mirs_share, 'launch', 'mirs_launch.py'])
68 ),
69 launch_arguments={
70 'esp_port': LaunchConfiguration('esp_port'),
71 'lidar_port': LaunchConfiguration('lidar_port'),
72 'lidar_baudrate': LaunchConfiguration('lidar_baudrate'),
73 'lidar_frame': LaunchConfiguration('lidar_frame'),
74 'use_sim_time': LaunchConfiguration('use_sim_time'),
75 'map': LaunchConfiguration('map'),
76 'nav2_params_file': LaunchConfiguration('nav2_params_file'),
77 'enable_lidar': LaunchConfiguration('enable_lidar'),
78 'enable_micro_ros': LaunchConfiguration('enable_micro_ros'),
79 'enable_ekf_local': LaunchConfiguration('enable_ekf_local'),
80 'ekf_config_file': LaunchConfiguration('ekf_config_file'),
81 'enable_static_base_link_to_laser_tf':
82 LaunchConfiguration('enable_static_base_link_to_laser_tf'),
83 'enable_static_base_footprint_to_base_link_tf':
84 LaunchConfiguration('enable_static_base_footprint_to_base_link_tf'),
85 'enable_static_base_link_to_imu_link_tf':
86 LaunchConfiguration('enable_static_base_link_to_imu_link_tf'),
87 'enable_rviz': 'false',
88 'enable_nav2': LaunchConfiguration('enable_nav2'),
89 'enable_slam_toolbox': 'false',
90 'autostart': LaunchConfiguration('autostart'),
91 'slam': LaunchConfiguration('slam'),
92 }.items(),
93 condition=IfCondition(LaunchConfiguration('enable_hardware')),
94 )
95
96 # coverage server設定はnav2_params.yamlを使い、時刻設定だけlaunchから渡す。
97 # Nav2本体のlifecycle一覧に含まれないため、専用managerでactive化する。
98 # enable_coverage_server=trueのときcoverage task serverを起動する。
99 coverage_server = Node(
100 package='opennav_coverage',
101 executable='opennav_coverage',
102 name='coverage_server',
103 output='screen',
104 parameters=[
105 LaunchConfiguration('nav2_params_file'),
106 {'use_sim_time': LaunchConfiguration('use_sim_time')},
107 ],
108 condition=IfCondition(LaunchConfiguration('enable_coverage_server')),
109 )
110
111 # serverを起動するときだけ専用lifecycle managerも起動する。
112 coverage_lifecycle = Node(
113 package='nav2_lifecycle_manager',
114 executable='lifecycle_manager',
115 name='lifecycle_manager_coverage',
116 output='screen',
117 parameters=[{
118 'use_sim_time': LaunchConfiguration('use_sim_time'),
119 'autostart': True,
120 'node_names': ['coverage_server'],
121 'bond_timeout': 10.0,
122 }],
123 condition=IfCondition(LaunchConfiguration('enable_coverage_server')),
124 )
125
126 # BLEパッケージが公開する起動APIで、JSON受信ノードを起動する。
127 ble_receiver = IncludeLaunchDescription(
128 PythonLaunchDescriptionSource(
129 PathJoinSubstitution([
130 FindPackageShare('ble_server'), 'launch',
131 'ble_receiver.launch.py',
132 ])
133 ),
134 launch_arguments={
135 'enabled': 'true',
136 'default_frame_id': 'map',
137 'cleaning_zone_topic': LaunchConfiguration('zone_topic'),
138 'initial_pose_topic': '/initialpose',
139 'use_sim_time': LaunchConfiguration('use_sim_time'),
140 }.items(),
141 condition=IfCondition(LaunchConfiguration('enable_ble')),
142 )
143
144 # enable_bridge=trueのときcleaning_zoneをcoverage actionへ中継する。
145 bridge = Node(
146 package='coverage',
147 executable='coverage_bridge',
148 name='coverage_bridge',
149 output='screen',
150 parameters=[{
151 'zone_topic': LaunchConfiguration('zone_topic'),
152 'state_topic': LaunchConfiguration('state_topic'),
153 'behavior_tree': LaunchConfiguration('behavior_tree'),
154 'frame_id': 'map',
155 'use_sim_time': LaunchConfiguration('use_sim_time'),
156 }],
157 condition=IfCondition(LaunchConfiguration('enable_bridge')),
158 )
159
160 # enable_monitor=trueのとき実機のtopic・TF診断を発行する。
161 monitor = Node(
162 package='coverage',
163 executable='coverage_monitor',
164 name='coverage_monitor',
165 output='screen',
166 parameters=[{
167 'profile': 'hardware',
168 'zone_topic': LaunchConfiguration('zone_topic'),
169 'use_sim_time': LaunchConfiguration('use_sim_time'),
170 }],
171 condition=IfCondition(LaunchConfiguration('enable_monitor')),
172 )
173
174 # use_rviz=trueのときcoverage表示設定を読み込んでRVizを起動する。
175 rviz = Node(
176 package='rviz2',
177 executable='rviz2',
178 name='rviz2',
179 output='screen',
180 arguments=['-d', rviz_config],
181 parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
182 condition=IfCondition(LaunchConfiguration('use_rviz')),
183 )
184
185 return LaunchDescription([
186 *args,
187 LogInfo(msg='[production] starting hardware...'),
188 mirs_bringup,
189 LogInfo(msg='[production] starting Nav2 through mirs_launch API...'),
190 LogInfo(msg='[production] starting coverage_server/ble/bridge/rviz...'),
191 coverage_server,
192 coverage_lifecycle,
193 ble_receiver,
194 bridge,
195 monitor,
196 rviz,
197 ])
_arg(name, default, description='')