simulation
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
simulation_launch.py
[詳解]
1
"""Gazebo、機体モデル、ROS・Gazebo間の接続、Foxglove接続、センサを起動する。"""
2
3
import
os
4
5
from
ament_index_python.packages
import
get_package_share_directory
6
from
launch
import
LaunchDescription
7
from
launch.actions
import
(
8
DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction,
9
SetEnvironmentVariable,
10
)
11
from
launch.launch_description_sources
import
(
12
AnyLaunchDescriptionSource, PythonLaunchDescriptionSource,
13
)
14
from
launch.substitutions
import
Command, LaunchConfiguration
15
from
launch_ros.actions
import
Node
16
from
launch_ros.parameter_descriptions
import
ParameterValue
17
18
19
def
_value
(context, name):
20
return
LaunchConfiguration(name).perform(context)
21
22
23
def
_enabled
(context, name):
24
return
_value
(context, name).lower()
in
(
'true'
,
'1'
,
'yes'
)
25
26
27
def
_optional_path
(context, name, default):
28
return
_value
(context, name)
or
default
29
30
31
def
_base_link_height_above_ground
(context):
32
"""mirs.urdf.xacroと同じbase_linkの取付位置を使う。"""
33
return
2.0 * float(
_value
(context,
'wheel_radius'
)) + 0.109
34
35
36
def
_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
59
def
_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
131
def
_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'
:
274
_base_link_height_above_ground
(context),
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
359
def
generate_launch_description
():
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
])
simulation_launch._base_link_height_above_ground
_base_link_height_above_ground(context)
mirs.urdf.xacroと同じbase_linkの取付位置を使う。
定義
simulation_launch.py:31
simulation_launch._launch_setup
_launch_setup(context)
定義
simulation_launch.py:131
simulation_launch._rtabmap_actions
_rtabmap_actions(is_3d, localization, database_path, show_viz, initial_pose='')
GazeboのLiDARとオドメトリを使ってRTAB-Mapを起動する。
定義
simulation_launch.py:60
simulation_launch._point_cloud_assembler
_point_cloud_assembler(name, input_topic, output_topic, circular=False)
odomで機体の移動を考慮し、首振り一往復分の点群を機体座標系にまとめる。
定義
simulation_launch.py:36
simulation_launch.generate_launch_description
generate_launch_description()
定義
simulation_launch.py:359
simulation_launch._optional_path
_optional_path(context, name, default)
定義
simulation_launch.py:27
simulation_launch._value
_value(context, name)
定義
simulation_launch.py:19
simulation_launch._enabled
_enabled(context, name)
定義
simulation_launch.py:23
launch
simulation_launch.py
構築:
1.18.0