|
|
#!/usr/bin/env python3
|
|
|
"""
|
|
|
步骤2：SLAM 建图
|
|
|
功能：启动 Gazebo + SLAM Toolbox + RViz2
|
|
|
用途：手动控制机器人进行 SLAM 建图
|
|
|
配置：使用 Task_3 的 SLAM 配置（已验证稳定）
|
|
|
"""
|
|
|
|
|
|
import os
|
|
|
from launch import LaunchDescription
|
|
|
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
|
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command
|
|
|
from launch_ros.actions import Node
|
|
|
from launch_ros.substitutions import FindPackageShare
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
|
|
|
|
|
def generate_launch_description():
|
|
|
# 包名和路径
|
|
|
pkg_name = 'final_navigation'
|
|
|
pkg_share = FindPackageShare(pkg_name).find(pkg_name)
|
|
|
|
|
|
# 文件路径
|
|
|
xacro_file = os.path.join(pkg_share, 'urdf', 'fusion_robot.urdf.xacro')
|
|
|
world_file = os.path.join(pkg_share, 'worlds', 'simple_house.world')
|
|
|
slam_params_file = os.path.join(pkg_share, 'config', 'slam_params.yaml')
|
|
|
rviz_config = os.path.join(pkg_share, 'rviz', 'slam_view.rviz')
|
|
|
|
|
|
# Launch 参数
|
|
|
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
|
|
|
|
|
# 处理 xacro 文件
|
|
|
robot_description = Command(['xacro ', xacro_file])
|
|
|
|
|
|
# ========================================
|
|
|
# 1. Gazebo 仿真
|
|
|
# ========================================
|
|
|
gazebo = IncludeLaunchDescription(
|
|
|
PythonLaunchDescriptionSource([
|
|
|
PathJoinSubstitution([
|
|
|
FindPackageShare('gazebo_ros'),
|
|
|
'launch',
|
|
|
'gazebo.launch.py'
|
|
|
])
|
|
|
]),
|
|
|
launch_arguments={
|
|
|
'world': world_file,
|
|
|
'verbose': 'false'
|
|
|
}.items()
|
|
|
)
|
|
|
|
|
|
# ========================================
|
|
|
# 2. 机器人状态发布器
|
|
|
# ========================================
|
|
|
robot_state_publisher = Node(
|
|
|
package='robot_state_publisher',
|
|
|
executable='robot_state_publisher',
|
|
|
name='robot_state_publisher',
|
|
|
output='screen',
|
|
|
parameters=[{
|
|
|
'robot_description': robot_description,
|
|
|
'use_sim_time': use_sim_time
|
|
|
}]
|
|
|
)
|
|
|
|
|
|
# ========================================
|
|
|
# 3. 在 Gazebo 中生成机器人
|
|
|
# ========================================
|
|
|
spawn_entity = Node(
|
|
|
package='gazebo_ros',
|
|
|
executable='spawn_entity.py',
|
|
|
arguments=[
|
|
|
'-entity', 'fusion_robot',
|
|
|
'-topic', 'robot_description',
|
|
|
'-x', '0.0',
|
|
|
'-y', '0.0',
|
|
|
'-z', '0.1'
|
|
|
],
|
|
|
output='screen'
|
|
|
)
|
|
|
|
|
|
# ========================================
|
|
|
# 4. SLAM Toolbox（使用 Task_3 的配置）
|
|
|
# ========================================
|
|
|
slam_toolbox = Node(
|
|
|
package='slam_toolbox',
|
|
|
executable='async_slam_toolbox_node',
|
|
|
name='slam_toolbox',
|
|
|
output='screen',
|
|
|
parameters=[
|
|
|
slam_params_file,
|
|
|
{'use_sim_time': use_sim_time}
|
|
|
]
|
|
|
)
|
|
|
|
|
|
# ========================================
|
|
|
# 5. RViz2 可视化
|
|
|
# ========================================
|
|
|
rviz2 = Node(
|
|
|
package='rviz2',
|
|
|
executable='rviz2',
|
|
|
name='rviz2',
|
|
|
arguments=['-d', rviz_config],
|
|
|
parameters=[{'use_sim_time': use_sim_time}],
|
|
|
output='screen'
|
|
|
)
|
|
|
|
|
|
|
|
|
return LaunchDescription([
|
|
|
DeclareLaunchArgument(
|
|
|
'use_sim_time',
|
|
|
default_value='true',
|
|
|
description='Use simulation time'
|
|
|
),
|
|
|
|
|
|
gazebo,
|
|
|
robot_state_publisher,
|
|
|
spawn_entity,
|
|
|
slam_toolbox,
|
|
|
rviz2,
|
|
|
])
|
|
|
|
