mirs
読み取り中…
検索中…
一致する文字列を見つけられません
mirs_launch.py
[詳解]
1# Copyright 2026 mirs260x
2#
3# Permission is hereby granted, free of charge, to any person obtaining a copy
4# of this software and associated documentation files (the "Software"), to deal
5# in the Software without restriction, including without limitation the rights
6# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7# copies of the Software, and to permit persons to whom the Software is
8# furnished to do so, subject to the following conditions:
9#
10# The above copyright notice and this permission notice shall be included in
11# all copies or substantial portions of the Software.
12#
13# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19# THE SOFTWARE.
20
21"""機体のROS通信・センサ・制御パラメータを起動し、設定に応じて地図作成や走行処理を追加する。"""
22from launch import LaunchDescription
23from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, TimerAction
24from launch.conditions import IfCondition
25from launch.launch_description_sources import (
26 AnyLaunchDescriptionSource, PythonLaunchDescriptionSource)
27from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
28from launch_ros.actions import Node
29from launch_ros.parameter_descriptions import ParameterValue
30from launch_ros.substitutions import FindPackageShare
31from nav2_common.launch import RewrittenYaml
32
33
35
36 # パッケージ指定
37 mirs_pkg = FindPackageShare('mirs')
38 nav2_bringup_pkg = FindPackageShare('nav2_bringup')
39
40 return LaunchDescription([
41
42 # ファイル設定
43 DeclareLaunchArgument(
44 'map',
45 default_value=PathJoinSubstitution([mirs_pkg, 'maps', 'youkanato.yaml'])
46 ),
47 DeclareLaunchArgument(
48 'ekf_config_file',
49 default_value=PathJoinSubstitution([mirs_pkg, 'config', 'ekf', 'ekf_params.yaml'])
50 ),
51 DeclareLaunchArgument(
52 'rviz_config_file',
53 default_value=PathJoinSubstitution([mirs_pkg, 'config', 'rviz', 'default.rviz'])
54 ),
55 DeclareLaunchArgument(
56 'nav2_params_file',
57 default_value=PathJoinSubstitution(
58 [mirs_pkg, 'config', 'navigation', 'nav2_params.yaml'])
59 ),
60 DeclareLaunchArgument(
61 'slam_toolbox_config_file',
62 default_value=PathJoinSubstitution(
63 [mirs_pkg, 'config', 'slam', 'slam_toolbox_config.yaml'])
64 ),
65 # ポート設定
66 DeclareLaunchArgument('esp_port', default_value='/dev/ttyUSB1'),
67 DeclareLaunchArgument('lidar_port', default_value='/dev/ttyUSB0'),
68 DeclareLaunchArgument('lidar_baudrate', default_value='256000'),
69 DeclareLaunchArgument('lidar_frame', default_value='laser'),
70 DeclareLaunchArgument('foxglove_address', default_value='0.0.0.0'),
71 DeclareLaunchArgument('foxglove_port', default_value='8765'),
72
73 # 真偽値
74 # simulation time
75 DeclareLaunchArgument('use_sim_time', default_value='false'),
76 # node
77 DeclareLaunchArgument('enable_lidar', default_value='true'),
78 DeclareLaunchArgument('scan_safety_stop_distance', default_value='0.35'),
79 DeclareLaunchArgument('scan_safety_resume_distance', default_value='0.50'),
80 DeclareLaunchArgument('scan_safety_forward_angle', default_value='30.0'),
81 DeclareLaunchArgument('enable_parameter_publisher', default_value='true'),
82 DeclareLaunchArgument('enable_micro_ros', default_value='true'),
83 DeclareLaunchArgument('enable_ekf_local', default_value='false'),
84 DeclareLaunchArgument('enable_rviz', default_value='false'),
85 DeclareLaunchArgument('enable_nav2', default_value='false'),
86 DeclareLaunchArgument('enable_slam_toolbox', default_value='false'),
87 DeclareLaunchArgument('autostart', default_value='true'),
88 DeclareLaunchArgument('slam', default_value='False'),
89 DeclareLaunchArgument(
90 'enable_auto_global_localization', default_value='true',
91 description='AMCLを地図全体から自動初期化する'),
92 # tf
93 DeclareLaunchArgument('enable_static_base_link_to_laser_tf', default_value='true'),
94 DeclareLaunchArgument('enable_static_base_footprint_to_base_link_tf',
95 default_value='true'),
96 DeclareLaunchArgument('enable_static_base_link_to_imu_link_tf', default_value='false'),
97 DeclareLaunchArgument('enable_slam_toolbox', default_value='false'),
98 DeclareLaunchArgument('enable_foxglove_bridge', default_value='true'),
99
100 Node(
101 package='mirs',
102 executable='parameter_publisher',
103 name='parameter_publisher',
104 output='screen',
105 parameters=[PathJoinSubstitution([mirs_pkg, 'config', 'config.yaml']), {
106 'use_sim_time': LaunchConfiguration('use_sim_time')}],
107 condition=IfCondition(LaunchConfiguration('enable_parameter_publisher')),
108 ),
109
110 Node(
111 package='micro_ros_agent',
112 executable='micro_ros_agent',
113 name='micro_ros_agent',
114 output='screen',
115 arguments=['serial', '--dev', LaunchConfiguration('esp_port'), '-v6'],
116 condition=IfCondition(LaunchConfiguration('enable_micro_ros')),
117 ),
118
119 Node(
120 package='robot_localization',
121 executable='ekf_node',
122 name='ekf_filter_node_local',
123 output='screen',
124 parameters=[LaunchConfiguration('ekf_config_file'), {
125 'use_sim_time': LaunchConfiguration('use_sim_time')}],
126 remappings=[('/odometry/filtered', '/odometry/local')],
127 condition=IfCondition(LaunchConfiguration('enable_ekf_local')),
128 ),
129
130 Node(
131 package='tf2_ros',
132 executable='static_transform_publisher',
133 name='enable_static_base_link_to_laser_tf',
134 output='screen',
135 arguments=['--x', '0', '--y', '0', '--z', '0.1745', '--roll', '0', '--pitch',
136 '0', '--yaw', '0', '--frame-id', 'base_link',
137 '--child-frame-id', LaunchConfiguration('lidar_frame')],
138 condition=IfCondition(LaunchConfiguration('enable_static_base_link_to_laser_tf')),
139 ),
140
141 Node(
142 package='tf2_ros',
143 executable='static_transform_publisher',
144 name='enable_static_base_footprint_to_base_link_tf',
145 output='screen',
146 arguments=[
147 '--x', '0', '--y', '0', '--z', '0.242', '--roll', '0', '--pitch', '0',
148 '--yaw', '0', '--frame-id', 'base_footprint', '--child-frame-id', 'base_link',
149 ],
150 condition=IfCondition(LaunchConfiguration(
151 'enable_static_base_footprint_to_base_link_tf')),
152 ),
153
154 Node(
155 package='tf2_ros',
156 executable='static_transform_publisher',
157 name='base_link_to_imu_link',
158 output='screen',
159 arguments=[
160 '--x', '0', '--y', '0', '--z', '0.05', '--roll', '0', '--pitch',
161 '0', '--yaw', '0', '--frame-id', 'base_link', '--child-frame-id', 'imu_link',
162 ],
163 condition=IfCondition(LaunchConfiguration('enable_static_base_link_to_imu_link_tf')),
164 ),
165
166 Node(
167 name='rviz2',
168 package='rviz2',
169 executable='rviz2',
170 arguments=['-d', LaunchConfiguration('rviz_config_file')],
171 parameters=[
172 {'use_sim_time': LaunchConfiguration('use_sim_time')}
173 ],
174 condition=IfCondition(LaunchConfiguration('enable_rviz'))
175 ),
176
177 IncludeLaunchDescription(
178 PythonLaunchDescriptionSource(
179 PathJoinSubstitution(
180 [FindPackageShare('sllidar_ros2'), 'launch', 'sllidar_s1_launch.py'])
181 ),
182 launch_arguments={
183 'serial_port': LaunchConfiguration('lidar_port'),
184 'serial_baudrate': LaunchConfiguration('lidar_baudrate'),
185 'frame_id': LaunchConfiguration('lidar_frame'),
186 }.items(),
187 condition=IfCondition(LaunchConfiguration('enable_lidar')),
188 ),
189 Node(
190 package='mirs',
191 executable='scan_safety_guard',
192 name='scan_safety_guard',
193 output='screen',
194 parameters=[{
195 'scan_topic': '/scan',
196 'input_cmd_vel_topic': '/cmd_vel_nav',
197 'output_cmd_vel_topic': '/cmd_vel',
198 'stop_distance': ParameterValue(
199 LaunchConfiguration('scan_safety_stop_distance'), value_type=float),
200 'resume_distance': ParameterValue(
201 LaunchConfiguration('scan_safety_resume_distance'), value_type=float),
202 'forward_angle_deg': ParameterValue(
203 LaunchConfiguration('scan_safety_forward_angle'), value_type=float),
204 'use_sim_time': LaunchConfiguration('use_sim_time'),
205 }],
206 condition=IfCondition(PythonExpression([
207 "'", LaunchConfiguration('enable_lidar'), "'.lower() == 'true' and '",
208 LaunchConfiguration('enable_nav2'), "'.lower() == 'true'",
209 ])),
210 ),
211 # Nav2 スタック本体の起動
212 IncludeLaunchDescription(
213 PythonLaunchDescriptionSource(
214 PathJoinSubstitution([nav2_bringup_pkg, 'launch', 'bringup_launch.py'])
215 ),
216 launch_arguments={
217 'map': LaunchConfiguration('map'),
218 'use_sim_time': LaunchConfiguration('use_sim_time'),
219 'autostart': LaunchConfiguration('autostart'),
220 'slam': LaunchConfiguration('slam'),
221 'params_file': RewrittenYaml(
222 source_file=LaunchConfiguration('nav2_params_file'),
223 param_rewrites={
224 'bt_navigator.ros__parameters.navigate_complete_coverage.plugin':
225 'nav2_bt_navigator::NavigateToPoseNavigator',
226 },
227 convert_types=True,
228 ),
229 }.items(),
230 condition=IfCondition(LaunchConfiguration('enable_nav2'))
231 ),
232
233 # AMCLを地図全体の一様分布で初期化し、2D Pose Estimateを不要にする。
234 # SLAMモードではAMCLが起動しないため呼び出さない。
235 TimerAction(
236 period=5.0,
237 actions=[ExecuteProcess(
238 cmd=[
239 'ros2', 'service', 'call',
240 '/reinitialize_global_localization',
241 'std_srvs/srv/Empty', '{}',
242 ],
243 output='screen',
244 )],
245 condition=IfCondition(PythonExpression([
246 "'", LaunchConfiguration('enable_nav2'),
247 "'.lower() == 'true' and '",
248 LaunchConfiguration('slam'),
249 "'.lower() == 'false' and '",
250 LaunchConfiguration('enable_auto_global_localization'),
251 "'.lower() == 'true'",
252 ])),
253 ),
254
255 # slam toolboxの起動
256 IncludeLaunchDescription(
257 PythonLaunchDescriptionSource(
258 PathJoinSubstitution(
259 [FindPackageShare('slam_toolbox'), 'launch', 'online_async_launch.py'])
260 ),
261 launch_arguments={
262 'slam_params_file': LaunchConfiguration('slam_toolbox_config_file'),
263 'use_sim_time': LaunchConfiguration('use_sim_time'),
264 'autostart': 'true',
265 }.items(),
266 condition=IfCondition(LaunchConfiguration('enable_slam_toolbox'))
267 ),
268
269 # foxglove bridgeの起動
270 IncludeLaunchDescription(
271 AnyLaunchDescriptionSource(
272 PathJoinSubstitution([
273 FindPackageShare('foxglove_bridge'), 'launch',
274 'foxglove_bridge_launch.xml']),
275 ),
276 launch_arguments={
277 'address': LaunchConfiguration('foxglove_address'),
278 'port': LaunchConfiguration('foxglove_port'),
279 }.items(),
280 condition=IfCondition(LaunchConfiguration('enable_foxglove_bridge'))
281 ),
282 ])