15 mapping_3d_share = get_package_share_directory(
'mapping-3d')
16 mirs_share = get_package_share_directory(
'mirs')
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')
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')
26 robot_desc = ParameterValue(
27 Command([
'xacro ', urdf_path]),
32 localization_arg = DeclareLaunchArgument(
34 default_value=
'false',
35 description=
'位置推定モードで起動'
37 use_rviz_arg = DeclareLaunchArgument(
42 use_rtabmap_viz_arg = DeclareLaunchArgument(
45 description=
'rtabmap_viz (RTAB-Map専用の可視化ツール) を起動'
47 database_path_arg = DeclareLaunchArgument(
49 default_value=os.path.expanduser(
'~/.ros/rtabmap.db'),
50 description=
'RTABMap の地図データベースファイルパス'
52 use_sim_time_arg = DeclareLaunchArgument(
54 default_value=
'false',
55 description=
'シミュレーション時刻を使うかどうか'
57 esp_port_arg = DeclareLaunchArgument(
59 default_value=
'/dev/ttyUSB0',
60 description=
'ESP32のUSBポート'
62 lidar_port_arg = DeclareLaunchArgument(
64 default_value=
'/dev/ttyUSB1',
65 description=
'LiDARのUSBポート'
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')
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'
83 (
'left/image_rect', left_image),
84 (
'left/camera_info', left_info),
85 (
'right/image_rect', right_image),
86 (
'right/camera_info', right_info),
90 realsense_launch = IncludeLaunchDescription(
91 PythonLaunchDescriptionSource(
92 os.path.join(mapping_3d_share,
'launch',
'realsense.launch.py')
97 robot_state_publisher = Node(
98 package=
'robot_state_publisher',
99 executable=
'robot_state_publisher',
100 name=
'robot_state_publisher',
103 'robot_description': robot_desc,
104 'use_sim_time': use_sim_time,
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}]
119 odometry_node = Node(
121 executable=
'odometry_publisher',
122 name=
'odometry_publisher',
124 parameters=[mirs_config_path, {
'use_sim_time': use_sim_time}]
127 parameter_node = Node(
129 executable=
'parameter_publisher',
130 name=
'parameter_publisher',
132 parameters=[mirs_config_path, {
'use_sim_time': use_sim_time}]
136 package=
'micro_ros_agent',
137 executable=
'micro_ros_agent',
138 name=
'micro_ros_agent',
140 arguments=[
'serial',
'--dev', esp_port,
'-v6']
144 sllidar_launch = IncludeLaunchDescription(
145 PythonLaunchDescriptionSource(
146 os.path.join(get_package_share_directory(
'sllidar_ros2'),
'launch',
'sllidar_s1_launch.py')
148 launch_arguments={
'serial_port': lidar_port,
'serial_baudrate':
'256000'}.items()
156 stereo_odometry = Node(
157 package=
'rtabmap_odom',
158 executable=
'stereo_odometry',
159 name=
'stereo_odometry',
161 parameters=[rtabmap_config, {
163 'use_sim_time': use_sim_time,
165 remappings=stereo_remappings + [
166 (
'odom',
'/odom/stereo'),
179 ekf_node_local = Node(
180 package=
'robot_localization',
181 executable=
'ekf_node',
182 name=
'ekf_filter_node_local',
184 parameters=[ekf_config_path, {
'use_sim_time': use_sim_time}],
185 remappings=[(
'/odometry/filtered',
'/odometry/local')]
190 package=
'rtabmap_slam',
191 executable=
'rtabmap',
194 condition=UnlessCondition(localization),
195 parameters=[rtabmap_config, {
196 'database_path': database_path,
197 'Mem/InitWMWithAllNodes':
'false',
198 'use_sim_time': use_sim_time,
200 remappings=stereo_remappings,
201 arguments=[
'--delete_db_on_start'],
205 rtabmap_localization = Node(
206 package=
'rtabmap_slam',
207 executable=
'rtabmap',
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,
217 remappings=stereo_remappings,
221 disparity_node = Node(
222 package=
'stereo_image_proc',
223 executable=
'disparity_node',
224 name=
'disparity_node',
225 namespace=
'device/head_camera',
228 (
'left/image_rect', left_image),
229 (
'left/camera_info', left_info),
230 (
'right/image_rect', right_image),
231 (
'right/camera_info', right_info),
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',
242 (
'left/image_rect', left_image),
243 (
'left/camera_info', left_info),
244 (
'right/camera_info', right_info),
245 (
'points2',
'/rtabmap/point_cloud'),
255 condition=IfCondition(use_rviz),
256 arguments=[
'-d', rviz_config],
260 rtabmap_viz_node = Node(
261 package=
'rtabmap_viz',
262 executable=
'rtabmap_viz',
265 condition=IfCondition(use_rtabmap_viz),
266 parameters=[rtabmap_config],
267 remappings=stereo_remappings,
271 ld = LaunchDescription()
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)
282 ld.add_action(odometry_node)
283 ld.add_action(parameter_node)
284 ld.add_action(micro_ros)
285 ld.add_action(sllidar_launch)
288 ld.add_action(robot_state_publisher)
289 ld.add_action(joint_state_publisher_node)
292 ld.add_action(realsense_launch)
293 ld.add_action(stereo_odometry)
296 ld.add_action(ekf_node_local)
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)