diff --git a/src/gc_navigation2_slamtoolbox/CMakeLists.txt b/src/gc_navigation2_slamtoolbox/CMakeLists.txt index 1b3c08c..25a809b 100644 --- a/src/gc_navigation2_slamtoolbox/CMakeLists.txt +++ b/src/gc_navigation2_slamtoolbox/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(ament_cmake REQUIRED) # 查找slam_toolbox包 find_package(slam_toolbox REQUIRED) install( - DIRECTORY launch config params maps + DIRECTORY launch config params maps world DESTINATION share/${PROJECT_NAME} ) if(BUILD_TESTING) diff --git a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml b/src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml index 671fe8c..f0d2734 100644 --- a/src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml +++ b/src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml @@ -12,7 +12,7 @@ slam_toolbox: # ============================ ROS 基础参数 ========================================= odom_frame: odom # 里程计坐标系 map_frame: map # 地图坐标系 - base_frame: base_link # 机器人基座坐标系 + base_frame: base_footprint # 机器人基座坐标系（与 URDF 根帧一致） scan_topic: /scan # 激光雷达话题 use_map_saver: true # 启用地图保存功能 mode: mapping # mapping = 定位 + 实时建图（地图可更新） diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_slam_mapping.launch.py b/src/gc_navigation2_slamtoolbox/launch/gc_slam_mapping.launch.py new file mode 100644 index 0000000..1247175 --- /dev/null +++ b/src/gc_navigation2_slamtoolbox/launch/gc_slam_mapping.launch.py @@ -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 +# ============================================================================ + +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, + ]) diff --git a/src/gc_navigation2_slamtoolbox/maps/zhihui.data b/src/gc_navigation2_slamtoolbox/maps/zhihui.data new file mode 100644 index 0000000..31d30e4 Binary files /dev/null and b/src/gc_navigation2_slamtoolbox/maps/zhihui.data differ diff --git a/src/gc_navigation2_slamtoolbox/maps/zhihui.pgm b/src/gc_navigation2_slamtoolbox/maps/zhihui.pgm new file mode 100644 index 0000000..9a4fc43 Binary files /dev/null and b/src/gc_navigation2_slamtoolbox/maps/zhihui.pgm differ diff --git a/src/gc_navigation2_slamtoolbox/maps/zhihui.posegraph b/src/gc_navigation2_slamtoolbox/maps/zhihui.posegraph new file mode 100644 index 0000000..5e49a17 Binary files /dev/null and b/src/gc_navigation2_slamtoolbox/maps/zhihui.posegraph differ diff --git a/src/gc_navigation2_slamtoolbox/maps/zhihui.yaml b/src/gc_navigation2_slamtoolbox/maps/zhihui.yaml new file mode 100644 index 0000000..43ed9c8 --- /dev/null +++ b/src/gc_navigation2_slamtoolbox/maps/zhihui.yaml @@ -0,0 +1,7 @@ +image: zhihui.pgm +mode: trinary +resolution: 0.05 +origin: [-4.98, -4.71, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/src/gc_navigation2_slamtoolbox/world/zhihui/model.config b/src/gc_navigation2_slamtoolbox/world/zhihui/model.config new file mode 100644 index 0000000..70585d9 --- /dev/null +++ b/src/gc_navigation2_slamtoolbox/world/zhihui/model.config @@ -0,0 +1,11 @@ + + + zhihui + 1.0 + model.sdf + + + + + + diff --git a/src/gc_navigation2_slamtoolbox/world/zhihui/model.sdf b/src/gc_navigation2_slamtoolbox/world/zhihui/model.sdf new file mode 100644 index 0000000..e1ef463 --- /dev/null +++ b/src/gc_navigation2_slamtoolbox/world/zhihui/model.sdf @@ -0,0 +1,471 @@ + + + + -2.11635 -1.73906 0 0 -0 0 + + + + + 1.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 1.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -1.61999 -0.744014 0 0 -0 0 + + + + + + 2.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 2.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 1.62754 -0.744014 0 0 -0 0 + + + + + + 1 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 1 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -0.827543 -0.3078 0 0 -0 1.5708 + + + + + + 1 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 1 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.327543 -0.319014 0 0 -0 1.5708 + + + + + + 5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.062177 -2.89677 0 0 -0 0 + + + + + + 3.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 3.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -0.305988 0.758719 0 0 -0 0 + + + + + + 3.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 3.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -0.300519 2.09188 0 0 -0 0 + + + + + + 1.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 1.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 1.65085 1.41824 0 0 -0 -1.5708 + + + + + + 2.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 2.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 2.3125 1.40828 0 0 -0 1.5708 + + + + + + 5.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 5.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -0.19189 2.74413 0 0 -0 3.14159 + + + + + + 2 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 2 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 2.6265 -1.8264 0 0 -0 1.5708 + + + + + + 1.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 1.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -2.25486 1.43754 0 0 -0 -1.5708 + + + + + + 3 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -2.89932 1.47176 0 0 -0 -1.5708 + + + + + + 2.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 2.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -1.87754 0.1172 0 0 -0 3.14159 + + + + + + 2.13496 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 2.13496 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 1.32002 0.107134 0 0 -0 0.001157 + + + + + + 2 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 2 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -2.50971 -1.81518 0 0 -0 1.5708 + + 1 + + diff --git a/src/origincar_description/world/zhihui.world b/src/origincar_description/world/zhihui.world new file mode 100644 index 0000000..77ef20e --- /dev/null +++ b/src/origincar_description/world/zhihui.world @@ -0,0 +1,79 @@ + + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + + + + + model://zhihui + + + + 0 0 -9.8 + + 0.001 + 1 + 1000 + + + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + \ No newline at end of file