mapping-3d
読み取り中…
検索中…
一致する文字列を見つけられません
rtabmap.launch.py
[詳解]
1import os
2
3from ament_index_python.packages import get_package_share_directory
4from launch import LaunchDescription
5from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
6from launch.conditions import IfCondition, UnlessCondition
7from launch.launch_description_sources import PythonLaunchDescriptionSource
8from launch.substitutions import LaunchConfiguration, Command
9from launch_ros.actions import Node
10from launch_ros.parameter_descriptions import ParameterValue
11
12
14 # ─── パッケージのパス ───────────────────────────────────────────
15 mapping_3d_share = get_package_share_directory('mapping-3d')
16 mirs_share = get_package_share_directory('mirs')
17
18 rtabmap_config = os.path.join(mapping_3d_share, 'config', 'rtabmap.yaml')
19 rviz_config = os.path.join(mapping_3d_share, 'config', 'rtabmap.rviz')
20 urdf_path = os.path.join(mirs_share, 'urdf', 'mirs.urdf.xacro')
21
22 mirs_config_path = os.path.join(mirs_share, 'config', 'config.yaml')
23 ekf_config_path = os.path.join(mirs_share, 'config', 'ekf_params.yaml')
24
25 # xacro で変数展開してから robot_description に渡す
26 robot_desc = ParameterValue(
27 Command(['xacro ', urdf_path]),
28 value_type=str
29 )
30
31 # ─── Launch 引数 ────────────────────────────────────────────────
32 localization_arg = DeclareLaunchArgument(
33 'localization',
34 default_value='false',
35 description='位置推定モードで起動'
36 )
37 use_rviz_arg = DeclareLaunchArgument(
38 'use_rviz',
39 default_value='true',
40 description='RViz2起動'
41 )
42 use_rtabmap_viz_arg = DeclareLaunchArgument(
43 'use_rtabmap_viz',
44 default_value='true',
45 description='rtabmap_viz (RTAB-Map専用の可視化ツール) を起動'
46 )
47 database_path_arg = DeclareLaunchArgument(
48 'database_path',
49 default_value=os.path.expanduser('~/.ros/rtabmap.db'),
50 description='RTABMap の地図データベースファイルパス'
51 )
52 use_sim_time_arg = DeclareLaunchArgument(
53 'use_sim_time',
54 default_value='false',
55 description='シミュレーション時刻を使うかどうか'
56 )
57 esp_port_arg = DeclareLaunchArgument(
58 'esp_port',
59 default_value='/dev/ttyUSB0',
60 description='ESP32のUSBポート'
61 )
62 lidar_port_arg = DeclareLaunchArgument(
63 'lidar_port',
64 default_value='/dev/ttyUSB1',
65 description='LiDARのUSBポート'
66 )
67
68 localization = LaunchConfiguration('localization')
69 use_rviz = LaunchConfiguration('use_rviz')
70 use_rtabmap_viz = LaunchConfiguration('use_rtabmap_viz')
71 database_path = LaunchConfiguration('database_path')
72 use_sim_time = LaunchConfiguration('use_sim_time')
73 esp_port = LaunchConfiguration('esp_port')
74 lidar_port = LaunchConfiguration('lidar_port')
75
76 # 左右カメラの共通トピック(infra1=左, infra2=右)
77 left_image = '/device/head_camera/realsense_node/infra1/image_rect_raw'
78 left_info = '/device/head_camera/realsense_node/infra1/camera_info'
79 right_image = '/device/head_camera/realsense_node/infra2/image_rect_raw'
80 right_info = '/device/head_camera/realsense_node/infra2/camera_info'
81
82 stereo_remappings = [
83 ('left/image_rect', left_image),
84 ('left/camera_info', left_info),
85 ('right/image_rect', right_image),
86 ('right/camera_info', right_info),
87 ]
88
89 # ─── 1. RealSense カメラ起動 ─────────────────────────────────────
90 realsense_launch = IncludeLaunchDescription(
91 PythonLaunchDescriptionSource(
92 os.path.join(mapping_3d_share, 'launch', 'realsense.launch.py')
93 )
94 )
95
96 # ─── 2. Robot State Publisher (mirs.urdf.xacro) ──────────────────
97 robot_state_publisher = Node(
98 package='robot_state_publisher',
99 executable='robot_state_publisher',
100 name='robot_state_publisher',
101 output='screen',
102 parameters=[{
103 'robot_description': robot_desc,
104 'use_sim_time': use_sim_time,
105 }]
106 )
107
108 # Joint State Publisher(ホイールの回転角度を配信)
109 joint_state_publisher_node = Node(
110 package='joint_state_publisher',
111 executable='joint_state_publisher',
112 name='joint_state_publisher',
113 parameters=[{'use_sim_time': use_sim_time}]
114 )
115
116 # ─── 3. ESP32 まわり(足回りオドメトリ・パラメータ・micro-ROS)────
117 # 注意: odometry_publisher は C++側でTF配信をOFFにしてある前提。
118 # TFを発行するのは ekf_filter_node_local だけにする。
119 odometry_node = Node(
120 package='mirs',
121 executable='odometry_publisher',
122 name='odometry_publisher',
123 output='screen',
124 parameters=[mirs_config_path, {'use_sim_time': use_sim_time}]
125 )
126
127 parameter_node = Node(
128 package='mirs',
129 executable='parameter_publisher',
130 name='parameter_publisher',
131 output='screen',
132 parameters=[mirs_config_path, {'use_sim_time': use_sim_time}]
133 )
134
135 micro_ros = Node(
136 package='micro_ros_agent',
137 executable='micro_ros_agent',
138 name='micro_ros_agent',
139 output='screen',
140 arguments=['serial', '--dev', esp_port, '-v6']
141 )
142
143 # ─── 4. LiDARドライバ ─────────────────────────────────────────────
144 sllidar_launch = IncludeLaunchDescription(
145 PythonLaunchDescriptionSource(
146 os.path.join(get_package_share_directory('sllidar_ros2'), 'launch', 'sllidar_s1_launch.py')
147 ),
148 launch_arguments={'serial_port': lidar_port, 'serial_baudrate': '256000'}.items()
149 )
150
151 # ─── 5. Stereo Odometry ノード ────────────────────────────────────
152 # 変更点: publish_tf を false にし、TFの発行はEKFに一本化する。
153 # 代わりに /odom トピック(ESP側と衝突するため)を /odom/stereo に
154 # remap し、EKFの2つ目の入力ソースとして使う。
155 # ※ rtabmap.yaml 側の publish_tf: true は、下記 parameters で上書きする。
156 stereo_odometry = Node(
157 package='rtabmap_odom',
158 executable='stereo_odometry',
159 name='stereo_odometry',
160 output='screen',
161 parameters=[rtabmap_config, {
162 'publish_tf': False,
163 'use_sim_time': use_sim_time,
164 }],
165 remappings=stereo_remappings + [
166 ('odom', '/odom/stereo'),
167 ],
168 )
169
170 # ─── 6. EKF(センサーフュージョン。odom -> base_footprint を発行)──
171 # ※ ekf_params.yaml 側で以下を満たしていることが前提:
172 # - base_link_frame: base_footprint (URDFの静的階層と整合させる)
173 # - world_frame: odom
174 # - publish_tf: true
175 # - odom0: /odom (ESP32 ホイールエンコーダ)
176 # - odom1: /odom/stereo (stereo_odometry、視覚オドメトリ)
177 # ekf_params.yaml 未調整の場合、odom1 は読み込まれず単純に無視される
178 # だけなので、まずはESP単体でTFが正しく出るかを先に確認してもよい。
179 ekf_node_local = Node(
180 package='robot_localization',
181 executable='ekf_node',
182 name='ekf_filter_node_local',
183 output='screen',
184 parameters=[ekf_config_path, {'use_sim_time': use_sim_time}],
185 remappings=[('/odometry/filtered', '/odometry/local')]
186 )
187
188 # ─── 7. RTABMap ノード (SLAM モード) ─────────────────────────────
189 rtabmap_slam = Node(
190 package='rtabmap_slam',
191 executable='rtabmap',
192 name='rtabmap',
193 output='screen',
194 condition=UnlessCondition(localization),
195 parameters=[rtabmap_config, {
196 'database_path': database_path,
197 'Mem/InitWMWithAllNodes': 'false',
198 'use_sim_time': use_sim_time,
199 }],
200 remappings=stereo_remappings,
201 arguments=['--delete_db_on_start'],
202 )
203
204 # ─── 8. RTABMap ノード (Localization モード) ──────────────────────
205 rtabmap_localization = Node(
206 package='rtabmap_slam',
207 executable='rtabmap',
208 name='rtabmap',
209 output='screen',
210 condition=IfCondition(localization),
211 parameters=[rtabmap_config, {
212 'database_path': database_path,
213 'Mem/IncrementalMemory': 'false',
214 'Mem/InitWMWithAllNodes': 'true',
215 'use_sim_time': use_sim_time,
216 }],
217 remappings=stereo_remappings,
218 )
219
220 # ─── 9. ステレオ点群ノード ────────────────────────────────────────
221 disparity_node = Node(
222 package='stereo_image_proc',
223 executable='disparity_node',
224 name='disparity_node',
225 namespace='device/head_camera',
226 output='screen',
227 remappings=[
228 ('left/image_rect', left_image),
229 ('left/camera_info', left_info),
230 ('right/image_rect', right_image),
231 ('right/camera_info', right_info),
232 ],
233 )
234
235 point_cloud_node = Node(
236 package='stereo_image_proc',
237 executable='point_cloud_node',
238 name='point_cloud_node',
239 namespace='device/head_camera',
240 output='screen',
241 remappings=[
242 ('left/image_rect', left_image),
243 ('left/camera_info', left_info),
244 ('right/camera_info', right_info),
245 ('points2', '/rtabmap/point_cloud'),
246 ],
247 )
248
249 # ─── 10. RViz2 ────────────────────────────────────────────────────
250 rviz_node = Node(
251 package='rviz2',
252 executable='rviz2',
253 name='rviz2',
254 output='screen',
255 condition=IfCondition(use_rviz),
256 arguments=['-d', rviz_config],
257 )
258
259 # ─── 11. rtabmap_viz ────────────────────────────────────────────
260 rtabmap_viz_node = Node(
261 package='rtabmap_viz',
262 executable='rtabmap_viz',
263 name='rtabmap_viz',
264 output='screen',
265 condition=IfCondition(use_rtabmap_viz),
266 parameters=[rtabmap_config],
267 remappings=stereo_remappings,
268 )
269
270 # ─── LaunchDescription にまとめる ─────────────────────────────────
271 ld = LaunchDescription()
272
273 ld.add_action(localization_arg)
274 ld.add_action(use_rviz_arg)
275 ld.add_action(use_rtabmap_viz_arg)
276 ld.add_action(database_path_arg)
277 ld.add_action(use_sim_time_arg)
278 ld.add_action(esp_port_arg)
279 ld.add_action(lidar_port_arg)
280
281 # 足回り・センサー系
282 ld.add_action(odometry_node)
283 ld.add_action(parameter_node)
284 ld.add_action(micro_ros)
285 ld.add_action(sllidar_launch)
286
287 # ロボットモデル・状態
288 ld.add_action(robot_state_publisher)
289 ld.add_action(joint_state_publisher_node)
290
291 # カメラ・視覚オドメトリ
292 ld.add_action(realsense_launch)
293 ld.add_action(stereo_odometry)
294
295 # センサーフュージョン(唯一 odom->base_footprint のTFを発行する)
296 ld.add_action(ekf_node_local)
297
298 # SLAM本体・可視化
299 ld.add_action(rtabmap_slam)
300 ld.add_action(rtabmap_localization)
301 ld.add_action(disparity_node)
302 ld.add_action(point_cloud_node)
303 ld.add_action(rviz_node)
304 ld.add_action(rtabmap_viz_node)
305
306 return ld