197 lines
7.9 KiB
Python
197 lines
7.9 KiB
Python
#!/usr/bin/env python3
|
||
# ============================================================================
|
||
# gc_slam_mapping.launch.py
|
||
# 功能：Gazebo 仿真环境 + SLAM 建图启动文件
|
||
#
|
||
# 适用场景：
|
||
# - 在 Gazebo 仿真中遥控机器人建立环境地图
|
||
# - 在已有地图基础上增量更新
|
||
# - 单独调试 slam_toolbox 建图参数
|
||
#
|
||
# 架构：
|
||
# Gazebo 仿真器 — 加载 zhihui.world（智慧楼墙体环境）
|
||
# spawn_entity — 在 Gazebo 中生成机器人模型
|
||
# robot_state_publisher — 发布机器人 TF 树（URDF 模型）
|
||
# joint_state_publisher — 发布关节状态
|
||
# async_slam_toolbox_node — 激光 SLAM 建图（mapping 模式）
|
||
# 发布 map→odom TF
|
||
# 发布 /map 话题（实时地图）
|
||
#
|
||
# 使用方式：
|
||
# ros2 launch gc_navigation2_slamtoolbox gc_slam_mapping.launch.py
|
||
#
|
||
# 保存地图：
|
||
# ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \
|
||
# "{filename: '/home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map'}"
|
||
# ros2 run nav2_map_server map_saver_cli -f <map_name>
|
||
# ============================================================================
|
||
|
||
import os
|
||
from ament_index_python.packages import get_package_share_directory
|
||
from launch import LaunchDescription
|
||
from launch.actions import DeclareLaunchArgument, ExecuteProcess, SetEnvironmentVariable
|
||
from launch.substitutions import LaunchConfiguration
|
||
from launch_ros.actions import Node
|
||
from launch_ros.parameter_descriptions import ParameterValue
|
||
from launch.substitutions import Command
|
||
|
||
|
||
def generate_launch_description():
|
||
"""生成 LaunchDescription，启动 Gazebo + slam_toolbox 建图"""
|
||
|
||
# ============================ 1. 包路径 =========================================
|
||
pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
|
||
origincar_urdf_dir = get_package_share_directory('origincar_description')
|
||
|
||
# ============================ 2. 声明启动参数 ====================================
|
||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
||
|
||
# slam_params_file: slam_toolbox 配置文件
|
||
slam_params_file = LaunchConfiguration(
|
||
'slam_params_file',
|
||
default=os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml'))
|
||
|
||
# map_file_name: 可选，加载已有序列化地图继续建图
|
||
map_file_name = LaunchConfiguration('map_file_name', default='')
|
||
|
||
# model: 机器人 URDF 模型路径（用于 xacro 处理）
|
||
# origincar.urdf 是纯 URDF（不含 xacro 宏），xacro 会原样透传
|
||
default_model_path = os.path.join(
|
||
origincar_urdf_dir, 'urdf', 'origincar.urdf')
|
||
model = DeclareLaunchArgument(
|
||
name='model', default_value=default_model_path)
|
||
|
||
# spawn_model: Gazebo 中生成机器人用的 URDF（不含 xacro 宏）
|
||
spawn_model_path = os.path.join(
|
||
origincar_urdf_dir, 'urdf', 'origincar.urdf')
|
||
|
||
# ============================ 3. 设置 GAZEBO_MODEL_PATH ==========================
|
||
# 让 Gazebo 能找到 zhihui 模型，同时保留标准模型路径
|
||
gazebo_model_path = os.path.join(pkg_dir, 'world')
|
||
existing_gazebo_path = os.environ.get('GAZEBO_MODEL_PATH', '')
|
||
if existing_gazebo_path:
|
||
full_gazebo_path = f'{gazebo_model_path}:{existing_gazebo_path}'
|
||
else:
|
||
full_gazebo_path = gazebo_model_path
|
||
set_gazebo_model_path = SetEnvironmentVariable(
|
||
name='GAZEBO_MODEL_PATH',
|
||
value=full_gazebo_path
|
||
)
|
||
|
||
# ============================ 4. 启动 Gazebo 仿真器 ==============================
|
||
gazebo_world_path = os.path.join(pkg_dir, 'world', 'zhihui.world')
|
||
start_gazebo_cmd = ExecuteProcess(
|
||
cmd=['gazebo', '--verbose',
|
||
'-s', 'libgazebo_ros_init.so',
|
||
'-s', 'libgazebo_ros_factory.so',
|
||
gazebo_world_path],
|
||
output='screen'
|
||
)
|
||
|
||
# ============================ 5. 机器人初始位姿参数 ==============================
|
||
# 通过命令行参数覆盖，例如：
|
||
# ros2 launch ... spawn_x:=1.0 spawn_y:=-2.0 spawn_yaw:=1.57
|
||
spawn_x = LaunchConfiguration('spawn_x', default='-4.311092')
|
||
spawn_y = LaunchConfiguration('spawn_y', default='-4.299756')
|
||
spawn_z = LaunchConfiguration('spawn_z', default='0.0')
|
||
spawn_yaw = LaunchConfiguration('spawn_yaw', default='0.0')
|
||
|
||
# 将位姿参数设为环境变量，供 spawn 命令使用
|
||
set_spawn_x = SetEnvironmentVariable('SPAWN_X', spawn_x)
|
||
set_spawn_y = SetEnvironmentVariable('SPAWN_Y', spawn_y)
|
||
set_spawn_z = SetEnvironmentVariable('SPAWN_Z', spawn_z)
|
||
set_spawn_yaw = SetEnvironmentVariable('SPAWN_YAW', spawn_yaw)
|
||
|
||
# ============================ 6. 在 Gazebo 中生成机器人 ==========================
|
||
# 通过 bash 读取环境变量，确保位姿参数正确传递
|
||
robot_name_in_model = 'mycar'
|
||
spawn_entity_cmd = ExecuteProcess(
|
||
cmd=['bash', '-c',
|
||
'ros2 run gazebo_ros spawn_entity.py '
|
||
'-entity mycar '
|
||
f'-file {spawn_model_path} '
|
||
'-x $SPAWN_X -y $SPAWN_Y -z $SPAWN_Z -Y $SPAWN_YAW'],
|
||
output='screen'
|
||
)
|
||
|
||
# ============================ 7. robot_state_publisher ===========================
|
||
robot_description = ParameterValue(
|
||
Command(['xacro ', LaunchConfiguration('model')]),
|
||
value_type=str
|
||
)
|
||
robot_state_publisher_node = Node(
|
||
package='robot_state_publisher',
|
||
executable='robot_state_publisher',
|
||
name='robot_state_publisher',
|
||
output='screen',
|
||
parameters=[{
|
||
'robot_description': robot_description,
|
||
'use_sim_time': True,
|
||
'publish_frequency': 30.0,
|
||
}],
|
||
)
|
||
|
||
# ============================ 8. joint_state_publisher ===========================
|
||
joint_state_publisher_node = Node(
|
||
package='joint_state_publisher',
|
||
executable='joint_state_publisher',
|
||
name='joint_state_publisher',
|
||
output='screen',
|
||
parameters=[{'use_sim_time': use_sim_time}],
|
||
)
|
||
|
||
# ============================ 9. slam_toolbox 建图节点 ============================
|
||
slam_toolbox_node = Node(
|
||
package='slam_toolbox',
|
||
executable='async_slam_toolbox_node',
|
||
name='slam_toolbox',
|
||
output='screen',
|
||
parameters=[
|
||
slam_params_file,
|
||
{
|
||
'use_sim_time': use_sim_time,
|
||
'map_file_name': map_file_name,
|
||
},
|
||
],
|
||
)
|
||
|
||
# ============================ 10. 组装 LaunchDescription =========================
|
||
return LaunchDescription([
|
||
DeclareLaunchArgument(
|
||
'use_sim_time',
|
||
default_value='true',
|
||
description='使用仿真时间（Gazebo=true, 实车=false)'),
|
||
DeclareLaunchArgument(
|
||
'slam_params_file',
|
||
default_value=os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml'),
|
||
description='slam_toolbox 参数配置文件路径'),
|
||
DeclareLaunchArgument(
|
||
'map_file_name',
|
||
default_value='',
|
||
description='已有序列化地图路径（.posegraph），留空则从零开始建图'),
|
||
DeclareLaunchArgument(
|
||
'spawn_x', default_value='-4.311092',
|
||
description='机器人初始 x 坐标（m）'),
|
||
DeclareLaunchArgument(
|
||
'spawn_y', default_value='-4.299756',
|
||
description='机器人初始 y 坐标（m）'),
|
||
DeclareLaunchArgument(
|
||
'spawn_z', default_value='0.0',
|
||
description='机器人初始 z 坐标（m）'),
|
||
DeclareLaunchArgument(
|
||
'spawn_yaw', default_value='0.0',
|
||
description='机器人初始偏航角（rad）'),
|
||
model,
|
||
|
||
set_gazebo_model_path,
|
||
set_spawn_x,
|
||
set_spawn_y,
|
||
set_spawn_z,
|
||
set_spawn_yaw,
|
||
start_gazebo_cmd,
|
||
spawn_entity_cmd,
|
||
robot_state_publisher_node,
|
||
joint_state_publisher_node,
|
||
slam_toolbox_node,
|
||
])
|