File size: 2,779 Bytes
15b8951 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 | # Mapping launch - SLAM Toolbox để quét bản đồ.
# Base (driver + lidar->scan + teleop) lấy từ bringup.launch.py.
# Usage: MAP_NAME=my_map MAP_SAVE=true ros2 launch go2_navigation mapping.launch.py
import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
"""Mapping: bringup + SLAM Toolbox + RViz."""
pkg_dir = get_package_share_directory('go2_navigation')
# Kết nối robot từ config/robot.yaml (env override) — dùng để chọn rviz single/multi
_cfg = {}
try:
with open(os.path.join(pkg_dir, 'config', 'robot.yaml')) as f:
_cfg = yaml.safe_load(f) or {}
except Exception:
pass
robot_ip = os.getenv('ROBOT_IP') or _cfg.get('robot_ip', '') or ''
conn_type = os.getenv('CONN_TYPE') or _cfg.get('conn_type', 'webrtc') or 'webrtc'
robot_ip_list = robot_ip.replace(" ", "").split(",") if robot_ip else []
conn_mode = "single" if len(robot_ip_list) == 1 and conn_type != "cyclonedds" else "multi"
slam_params = os.path.join(pkg_dir, 'config', 'mapper_params_online_async.yaml')
rviz_config = 'single_robot_conf.rviz' if conn_mode == 'single' else 'multi_robot_conf.rviz'
rviz_path = os.path.join(pkg_dir, 'config', rviz_config)
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
with_rviz = LaunchConfiguration('rviz', default='true')
print("🗺️ Go2 Mapping Mode:")
print(f" Robot IPs: {robot_ip_list} Map name: {os.getenv('MAP_NAME', 'my_map')}")
bringup = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_dir, 'launch', 'bringup.launch.py')),
)
rviz = Node(
package='rviz2',
executable='rviz2',
name='go2_rviz2',
output='screen',
condition=IfCondition(with_rviz),
arguments=['-d', rviz_path],
parameters=[{'use_sim_time': False}],
)
slam = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('slam_toolbox'),
'launch', 'online_async_launch.py')),
launch_arguments={
'slam_params_file': slam_params,
'use_sim_time': use_sim_time,
}.items(),
)
return LaunchDescription([
DeclareLaunchArgument('rviz', default_value='true', description='Launch RViz2'),
bringup, rviz, slam,
])
|