From 4e49f7f981ac7d97efa68b5379625626d0b61bab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=99=8C=E6=AC=A2=E9=86=89?= <264854363@qq.com> Date: Sat, 6 Jun 2026 14:20:37 +0800 Subject: [PATCH] =?UTF-8?q?=E6=88=90=E5=8A=9F=E5=AE=9E=E7=8E=B0=E6=99=BA?= =?UTF-8?q?=E6=85=A7=E5=8C=BB=E7=96=97=E5=9C=B0=E5=9B=BE=E5=B5=8C=E5=85=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/gc_nav2_with_slam_online.launch.py | 51 +- src/origincar_description/world/zhihui.world | 736 +++++++++++++++++- 2 files changed, 767 insertions(+), 20 deletions(-) diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py index 6c5b0d1..c2020d6 100644 --- a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py +++ b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py @@ -25,7 +25,7 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, SetEnvironmentVariable from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -45,11 +45,11 @@ def generate_launch_description(): # =============================2.参数================================================================== use_sim_time = LaunchConfiguration('use_sim_time', default='true') - map_yaml_path = os.path.join(pkg_dir, 'maps', 'my_map.yaml') + map_yaml_path = os.path.join(pkg_dir, 'maps', 'zhihui.yaml') nav2_param_path = LaunchConfiguration('params_file', default=os.path.join( pkg_dir, 'params', 'gc_navigation_slam.yaml')) slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml') - posegraph_path = os.path.join(pkg_dir, 'maps', 'my_map.posegraph') + posegraph_path = os.path.join(pkg_dir, 'maps', 'zhihui.posegraph') # =============================3.map_server：全量静态地图（Foxglove 看到的全局地图）======================== map_server_node = Node( @@ -108,7 +108,7 @@ def generate_launch_description(): robot_name_in_model = 'mycar' default_model_path = os.path.join(origincar_urdf_dir, 'urdf', 'origincar.urdf') model = DeclareLaunchArgument(name='model', default_value=default_model_path) - gazebo_world_path = os.path.join(origincar_urdf_dir, 'world', 'test.world') + gazebo_world_path = os.path.join(origincar_urdf_dir, 'world', 'zhihui.world') start_gazebo_cmd = ExecuteProcess( cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', @@ -116,12 +116,31 @@ def generate_launch_description(): gazebo_world_path], output='screen', ) - spawn_entity_cmd = Node( - package='gazebo_ros', - executable='spawn_entity.py', - arguments=['-entity', robot_name_in_model, '-file', default_model_path], + + # =============================7a.机器人初始位姿参数===================================================== + # 通过命令行参数覆盖，例如： + # 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') + + 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) + + # =============================7b.在 Gazebo 中生成机器人================================================= + # 通过 bash 读取环境变量，确保位姿参数正确传递 + spawn_entity_cmd = ExecuteProcess( + cmd=['bash', '-c', + 'ros2 run gazebo_ros spawn_entity.py ' + '-entity mycar ' + f'-file {default_model_path} ' + '-x $SPAWN_X -y $SPAWN_Y -z $SPAWN_Z -Y $SPAWN_YAW'], output='screen', ) + robot_description = ParameterValue( Command(['xacro ', LaunchConfiguration('model')]), value_type=str, ) @@ -134,7 +153,23 @@ def generate_launch_description(): # =============================8.组装================================================================== ld.add_action(model) + ld.add_action(DeclareLaunchArgument( + 'spawn_x', default_value='-4.311092', + description='机器人初始 x 坐标（m）')) + ld.add_action(DeclareLaunchArgument( + 'spawn_y', default_value='-4.299756', + description='机器人初始 y 坐标（m）')) + ld.add_action(DeclareLaunchArgument( + 'spawn_z', default_value='0.0', + description='机器人初始 z 坐标（m）')) + ld.add_action(DeclareLaunchArgument( + 'spawn_yaw', default_value='0.0', + description='机器人初始偏航角（rad）')) ld.add_action(start_gazebo_cmd) + ld.add_action(set_spawn_x) + ld.add_action(set_spawn_y) + ld.add_action(set_spawn_z) + ld.add_action(set_spawn_yaw) ld.add_action(spawn_entity_cmd) ld.add_action(robot_state_publisher) ld.add_action(map_server_node) diff --git a/src/origincar_description/world/zhihui.world b/src/origincar_description/world/zhihui.world index 77ef20e..5a6012d 100644 --- a/src/origincar_description/world/zhihui.world +++ b/src/origincar_description/world/zhihui.world @@ -1,6 +1,5 @@ - 1 0 0 10 0 -0 0 @@ -14,8 +13,6 @@ -0.5 0.1 -0.9 - - 1 @@ -55,21 +52,736 @@ - - - - model://zhihui - - - + + -2.11635 -1.73906 0 0 -0 0 + + + + + 1.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 2.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 1 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 1 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 3.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 3.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 1.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 2.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 5.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 2 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 1.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 3 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 2.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 2.13496 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + + + + + 2 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 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 + 0 + 0 + 0 + + 1 + 0 0 -9.8 0.001 1 1000 - - 0.4 0.4 0.4 1 0.7 0.7 0.7 1