目前完成了比赛地图的建图，但是还没有实现定位导航
This commit is contained in:
陌欢醉
196
src/gc_navigation2_slamtoolbox/launch/gc_slam_mapping.launch.py
Normal file
196
src/gc_navigation2_slamtoolbox/launch/gc_slam_mapping.launch.py
Normal file
@@ -0,0 +1,196 @@
|
||||
#!/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,
|
||||
])
|
||||
Reference in New Issue
Block a user