simulation
読み取り中…
検索中…
一致する文字列を見つけられません
simulation_launch.py
[詳解]
1"""Gazebo、機体モデル、ROS・Gazebo間の接続、Foxglove接続、センサを起動する。"""
2
3import os
4
5from ament_index_python.packages import get_package_share_directory
6from launch import LaunchDescription
7from launch.actions import (
8 DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction,
9 SetEnvironmentVariable,
10)
11from launch.launch_description_sources import (
12 AnyLaunchDescriptionSource, PythonLaunchDescriptionSource,
13)
14from launch.substitutions import Command, LaunchConfiguration
15from launch_ros.actions import Node
16from launch_ros.parameter_descriptions import ParameterValue
17
18
19def _value(context, name):
20 return LaunchConfiguration(name).perform(context)
21
22
23def _enabled(context, name):
24 return _value(context, name).lower() in ('true', '1', 'yes')
25
26
27def _optional_path(context, name, default):
28 return _value(context, name) or default
29
30
32 """mirs.urdf.xacroと同じbase_linkの取付位置を使う。"""
33 return 2.0 * float(_value(context, 'wheel_radius')) + 0.109
34
35
36def _point_cloud_assembler(name, input_topic, output_topic, circular=False):
37 """odomで機体の移動を考慮し、首振り一往復分の点群を機体座標系にまとめる。"""
38 return Node(
39 package='rtabmap_util',
40 executable='point_cloud_assembler',
41 name=name,
42 parameters=[{'use_sim_time': True}, {
43 'assembling_time': ParameterValue(
44 LaunchConfiguration('cloud_assembling_time'), value_type=float),
45 'fixed_frame_id': 'odom',
46 'frame_id': 'base_link',
47 'wait_for_transform': 0.5,
48 'voxel_size': 0.1,
49 'qos': 1,
50 'circular_buffer': circular,
51 }],
52 remappings=[
53 ('cloud', input_topic),
54 ('assembled_cloud', output_topic),
55 ],
56 output='screen')
57
58
59def _rtabmap_actions(is_3d, localization, database_path, show_viz,
60 initial_pose=''):
61 """GazeboのLiDARとオドメトリを使ってRTAB-Mapを起動する。"""
62 odometry_topic = '/odometry/filtered' if is_3d else '/odom'
63 parameters = {
64 'use_sim_time': True,
65 'frame_id': 'base_link',
66 'odom_frame_id': '',
67 'odom_frame_id_init': 'odom',
68 'map_frame_id': 'map',
69 'publish_tf': True,
70 'subscribe_odom': True,
71 'subscribe_scan_cloud': is_3d,
72 'subscribe_scan': not is_3d,
73 'subscribe_rgb': False,
74 'subscribe_depth': False,
75 'subscribe_rgbd': False,
76 'approx_sync': True,
77 'wait_for_transform': 0.5,
78 'Rtabmap/DetectionRate': '10',
79 'log_to_rosout_level': 2,
80 'RGBD/LinearUpdate': '0.05',
81 'RGBD/AngularUpdate': '0.05',
82 'Reg/Strategy': '1',
83 'Reg/Force3DoF': 'true',
84 'Grid/Sensor': '0',
85 'Grid/3D': 'true' if is_3d else 'false',
86 'Grid/RangeMax': '10',
87 'database_path': database_path,
88 }
89 if is_3d:
90 parameters.update({
91 'Icp/PointToPlane': 'false',
92 'Icp/VoxelSize': '0.1',
93 'Icp/MaxCorrespondenceDistance': '0.5',
94 'Grid/NormalsSegmentation': 'false',
95 'Grid/MaxGroundHeight': '-0.12',
96 'Grid/RayTracing': 'true',
97 })
98 if localization:
99 parameters.update({
100 'Mem/IncrementalMemory': 'false',
101 'Mem/InitWMWithAllNodes': 'true',
102 })
103 if initial_pose:
104 # Override the last localization pose stored in the database.
105 parameters['initial_pose'] = initial_pose
106
107 remappings = [
108 ('scan_cloud', '/points_assembled_icp'),
109 ('scan', '/scan'),
110 ('odom', odometry_topic),
111 ('grid_map', '/map'),
112 ]
113 actions = [Node(
114 package='rtabmap_slam',
115 executable='rtabmap',
116 name='rtabmap',
117 parameters=[parameters],
118 arguments=[] if localization else ['--delete_db_on_start'],
119 remappings=remappings,
120 output='screen')]
121 if show_viz:
122 actions.append(Node(
123 package='rtabmap_viz',
124 executable='rtabmap_viz',
125 parameters=[parameters],
126 remappings=remappings,
127 output='screen'))
128 return actions
129
130
131def _launch_setup(context):
132 is_3d = _enabled(context, 'enable_nodding_lidar')
133 mapping = _enabled(context, 'enable_slam')
134 navigation = _enabled(context, 'enable_nav2')
135 diagnostic_stage = _value(context, 'diagnostic_stage')
136 stages = ('full', 'scan', 'cloud', 'filtered', 'assembled', 'localization')
137 if diagnostic_stage not in stages:
138 raise RuntimeError(f'diagnostic_stage must be one of: {", ".join(stages)}')
139 if diagnostic_stage != 'full' and (not is_3d or mapping or navigation):
140 raise RuntimeError(
141 'diagnostic_stage requires 3D LiDAR and disables SLAM and Nav2')
142
143 def stage_at_least(name):
144 return diagnostic_stage == 'full' or stages.index(diagnostic_stage) >= stages.index(name)
145
146 if mapping and navigation:
147 raise RuntimeError('enable_slam and enable_nav2 cannot both be true')
148 share = get_package_share_directory('simulation')
149 sim_time = {'use_sim_time': True}
150 world = _optional_path(
151 context, 'world',
152 os.path.join(share, 'worlds',
153 'nodding_forest.sdf' if is_3d else 'park.sdf'))
154
155 ekf_config = _optional_path(
156 context, 'ekf_config_file',
157 os.path.join(share, 'config', 'nodding_ekf.yaml'))
158
159 # Keep simulation DDS traffic isolated from the real MIRS stack. The
160 # physical micro-ROS firmware uses domain 90; Gazebo uses domain 91.
161 actions = [SetEnvironmentVariable('ROS_DOMAIN_ID', '91')]
162 if _enabled(context, 'enable_foxglove_bridge'):
163 foxglove_share = get_package_share_directory('foxglove_bridge')
164 actions.append(IncludeLaunchDescription(
165 AnyLaunchDescriptionSource(os.path.join(
166 foxglove_share, 'launch', 'foxglove_bridge_launch.xml')),
167 launch_arguments={
168 'address': _value(context, 'foxglove_address'),
169 'port': _value(context, 'foxglove_port'),
170 }.items()))
171 if _enabled(context, 'enable_gazebo'):
172 gz_share = get_package_share_directory('ros_gz_sim')
173 actions.append(IncludeLaunchDescription(
174 PythonLaunchDescriptionSource(
175 os.path.join(gz_share, 'launch', 'gz_sim.launch.py')),
176 launch_arguments={
177 'gz_args': ['-r ', world],
178 }.items()))
179
180 if _enabled(context, 'enable_robot'):
181 description = ParameterValue(
182 Command([
183 'xacro ', LaunchConfiguration('robot_model_file'),
184 ' nodding:=', 'true' if is_3d else 'false',
185 ' wheel_radius:=', LaunchConfiguration('wheel_radius'),
186 ' wheel_track:=', LaunchConfiguration('wheel_track'),
187 ' wheel_mu_longitudinal:=', LaunchConfiguration('wheel_mu_longitudinal'),
188 ' wheel_mu_lateral:=', LaunchConfiguration('wheel_mu_lateral'),
189 ' caster_mu:=', LaunchConfiguration('caster_mu'),
190 ]),
191 value_type=str)
192 actions.extend([
193 Node(
194 package='robot_state_publisher',
195 executable='robot_state_publisher',
196 parameters=[sim_time, {'robot_description': description}],
197 output='screen'),
198 Node(
199 package='ros_gz_sim',
200 executable='create',
201 arguments=[
202 '-name', 'mirs', '-topic', 'robot_description',
203 '-x', LaunchConfiguration('spawn_x'),
204 '-y', LaunchConfiguration('spawn_y'),
205 '-z', LaunchConfiguration('spawn_z'),
206 ],
207 parameters=[sim_time],
208 output='screen'),
209 ])
210
211 if _enabled(context, 'enable_bridge'):
212 # 2D publishes the fixed laser scan directly as /scan. In 3D,
213 # nodding_io republishes /scan_raw after adding the servo state.
214 actions.append(Node(
215 package='ros_gz_bridge',
216 executable='parameter_bridge',
217 parameters=[sim_time, {
218 'config_file': LaunchConfiguration('bridge_config_file'),
219 }],
220 # Keep Gazebo's raw odometry TF available only on a diagnostic
221 # topic in 3D mode; robot_localization owns odom -> base_footprint.
222 remappings=[('/tf', '/tf_gazebo')] if is_3d else [('/scan_raw', '/scan')],
223 output='screen'))
224
225 if is_3d:
226 if stage_at_least('cloud'):
227 actions.extend([
228 Node(
229 package='simulation',
230 executable='sensor_covariance_adapter',
231 parameters=[sim_time, {
232 'wheel_linear_velocity_variance': ParameterValue(
233 LaunchConfiguration('wheel_linear_velocity_variance'),
234 value_type=float),
235 'wheel_angular_velocity_variance': ParameterValue(
236 LaunchConfiguration('wheel_angular_velocity_variance'),
237 value_type=float),
238 'imu_yaw_rate_variance': ParameterValue(
239 LaunchConfiguration('imu_yaw_rate_variance'),
240 value_type=float),
241 }],
242 output='screen'),
243 Node(
244 package='robot_localization',
245 executable='ekf_node',
246 name='ekf_filter_node_local',
247 parameters=[sim_time, ekf_config],
248 output='screen'),
249 ])
250 actions.append(Node(
251 package='simulation',
252 executable='nodding_io',
253 parameters=[sim_time],
254 output='screen'))
255 if stage_at_least('cloud'):
256 actions.append(Node(
257 package='nodding_lidar',
258 executable='scan_to_cloud',
259 parameters=[LaunchConfiguration('cloud_config_file')],
260 output='screen'))
261 # RTAB-Map scan registration uses stable returns above the grass.
262 # Ground classification still sees the full cloud.
263 if stage_at_least('filtered'):
264 actions.append(Node(
265 package='simulation',
266 executable='pointcloud_height_filter',
267 name='icp_height_filter',
268 parameters=[sim_time, {
269 'input_topic': '/points',
270 'output_topic': '/points_icp_filtered',
271 'min_height_above_ground': ParameterValue(
272 LaunchConfiguration('icp_min_height'), value_type=float),
273 'base_link_height_above_ground':
275 }],
276 output='screen'))
277 if stage_at_least('assembled'):
278 actions.append(_point_cloud_assembler(
279 'icp_point_cloud_assembler',
280 '/points_icp_filtered', '/points_assembled_icp'))
281 if diagnostic_stage == 'full' and _enabled(context, 'enable_ground_segmentation'):
282 # GSeg3D needs the full nodding sweep to fit ground planes. The
283 # circular window updates every incoming scan after the first sweep.
284 actions.append(_point_cloud_assembler(
285 'ground_point_cloud_assembler',
286 '/points', '/points_assembled', circular=True))
287 actions.append(Node(
288 package='ground_segmentation_ros2',
289 executable='ground_segmentation_ros2_node',
290 name='ground_segmentation',
291 parameters=[LaunchConfiguration('ground_config_file'), {
292 'lidar_to_ground': -_base_link_height_above_ground(context),
293 }],
294 remappings=[
295 ('/ground_segmentation/input_pointcloud', '/points_assembled'),
296 ('/ground_segmentation/input_imu', '/imu/data_raw'),
297 ],
298 output='screen'))
299
300 localization_debug = diagnostic_stage == 'localization'
301 if mapping or navigation or localization_debug:
302 sensor_mode = '3d' if is_3d else '2d'
303 database_path = os.path.expanduser(_optional_path(
304 context, 'database_path',
305 os.path.join(os.path.expanduser('~'), '.ros',
306 f'mirs_gazebo_{sensor_mode}.db')))
307 if (navigation or localization_debug) and not os.path.isfile(database_path):
308 raise RuntimeError(
309 f'RTAB-Map database not found: {database_path}. '
310 f'Run slam_{sensor_mode}.launch.py first or set database_path.')
311 actions.extend(_rtabmap_actions(
312 is_3d, navigation or localization_debug, database_path,
313 _enabled(context, 'enable_rtabmap_viz'),
314 _value(context, 'rtabmap_initial_pose')))
315
316 if navigation:
317 nav2_params = _optional_path(
318 context, 'nav2_params_file',
319 os.path.join(share, 'config',
320 'nodding_nav2.yaml' if is_3d else 'nav2_params.yaml'))
321 actions.append(Node(
322 package='simulation',
323 executable='home_pose_saver',
324 name='home_pose_saver',
325 parameters=[sim_time, {
326 'global_frame': 'map',
327 'robot_frame': 'base_footprint',
328 'home_pose_topic': '/simulation/home_pose',
329 }],
330 output='screen'))
331 nav2_share = get_package_share_directory('nav2_bringup')
332 actions.append(IncludeLaunchDescription(
333 PythonLaunchDescriptionSource(
334 os.path.join(nav2_share, 'launch', 'navigation_launch.py')),
335 launch_arguments={
336 'use_sim_time': 'true',
337 'autostart': 'true',
338 'params_file': nav2_params,
339 }.items()))
340
341 if _enabled(context, 'enable_rviz'):
342 rviz_config = _optional_path(
343 context, 'rviz_config_file',
344 os.path.join(share, 'rviz',
345 'nodding.rviz' if is_3d else 'mapping.rviz'))
346 actions.append(Node(
347 package='rviz2', executable='rviz2',
348 arguments=['-d', rviz_config],
349 parameters=[sim_time], output='screen'))
350 if _enabled(context, 'enable_teleop'):
351 actions.append(Node(
352 package='teleop_twist_keyboard',
353 executable='teleop_twist_keyboard',
354 prefix='xterm -e', output='screen'))
355
356 return actions
357
358
360 share = get_package_share_directory('simulation')
361 return LaunchDescription([
362 DeclareLaunchArgument('world', default_value=''),
363 DeclareLaunchArgument(
364 'robot_model_file',
365 default_value=os.path.join(share, 'urdf', 'mirs_gazebo.urdf.xacro')),
366 DeclareLaunchArgument(
367 'bridge_config_file',
368 default_value=os.path.join(share, 'config', 'nodding_bridge.yaml')),
369 DeclareLaunchArgument(
370 'cloud_config_file',
371 default_value=os.path.join(share, 'config', 'nodding_cloud.yaml')),
372 DeclareLaunchArgument('cloud_assembling_time', default_value='2.0'),
373 DeclareLaunchArgument(
374 'icp_min_height', default_value='0.5',
375 description='Minimum ICP point height above the simulated ground (m)'),
376 DeclareLaunchArgument('wheel_linear_velocity_variance', default_value='0.04'),
377 DeclareLaunchArgument('wheel_angular_velocity_variance', default_value='0.09'),
378 DeclareLaunchArgument('imu_yaw_rate_variance', default_value='0.01'),
379 DeclareLaunchArgument(
380 'ground_config_file',
381 default_value=os.path.join(share, 'config', 'nodding_ground.yaml')),
382 DeclareLaunchArgument(
383 'ekf_config_file',
384 default_value=os.path.join(share, 'config', 'nodding_ekf.yaml')),
385 DeclareLaunchArgument('spawn_x', default_value='0'),
386 DeclareLaunchArgument('spawn_y', default_value='0'),
387 DeclareLaunchArgument('spawn_z', default_value='0.0'),
388 DeclareLaunchArgument('wheel_radius', default_value='0.0665'),
389 DeclareLaunchArgument('wheel_track', default_value='0.465'),
390 DeclareLaunchArgument('wheel_mu_longitudinal', default_value='1.0'),
391 DeclareLaunchArgument('wheel_mu_lateral', default_value='0.2'),
392 DeclareLaunchArgument('caster_mu', default_value='0.001'),
393 DeclareLaunchArgument('enable_gazebo', default_value='true'),
394 DeclareLaunchArgument('enable_robot', default_value='true'),
395 DeclareLaunchArgument('enable_bridge', default_value='true'),
396 DeclareLaunchArgument('enable_foxglove_bridge', default_value='false'),
397 DeclareLaunchArgument('foxglove_address', default_value='0.0.0.0'),
398 DeclareLaunchArgument('foxglove_port', default_value='8765'),
399 DeclareLaunchArgument('enable_nodding_lidar', default_value='false'),
400 DeclareLaunchArgument('diagnostic_stage', default_value='full'),
401 DeclareLaunchArgument('enable_ground_segmentation', default_value='false'),
402 DeclareLaunchArgument('enable_slam', default_value='false'),
403 DeclareLaunchArgument('enable_nav2', default_value='false'),
404 DeclareLaunchArgument('enable_teleop', default_value='false'),
405 DeclareLaunchArgument('enable_rtabmap_viz', default_value='false'),
406 DeclareLaunchArgument('enable_rviz', default_value='false'),
407 DeclareLaunchArgument('database_path', default_value=''),
408 DeclareLaunchArgument('rtabmap_initial_pose', default_value=''),
409 DeclareLaunchArgument('nav2_params_file', default_value=''),
410 DeclareLaunchArgument('rviz_config_file', default_value=''),
411 OpaqueFunction(function=_launch_setup),
412 ])
_base_link_height_above_ground(context)
mirs.urdf.xacroと同じbase_linkの取付位置を使う。
_rtabmap_actions(is_3d, localization, database_path, show_viz, initial_pose='')
GazeboのLiDARとオドメトリを使ってRTAB-Mapを起動する。
_point_cloud_assembler(name, input_topic, output_topic, circular=False)
odomで機体の移動を考慮し、首振り一往復分の点群を機体座標系にまとめる。
_optional_path(context, name, default)