cyy/yiliao2026
1
0
Fork 0
forked from zbw/yiliao2026
Code Pull Requests Activity

成功实现智慧医疗地图嵌入

Browse Source
This commit is contained in:
陌欢醉
2026-06-06 14:20:37 +08:00
parent 5da3d07656
commit 4e49f7f981
2 changed files with 767 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py
View File

@@ -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)

736
src/origincar_description/world/zhihui.world
View File

@@ -1,6 +1,5 @@
<sdf version='1.7'>
<world name='default'>
<!-- ===== 光照 ===== -->
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
@@ -14,8 +13,6 @@
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<!-- ===== 地面 ===== -->
<model name='ground_plane'>
<static>1</static>
<link name='link'>
@@ -55,21 +52,736 @@
</visual>
</link>
</model>
<!-- ===== 智慧楼墙体模型 ===== -->
<include>
<uri>model://zhihui</uri>
</include>
<!-- ===== 物理引擎 ===== -->
<model name='zhihui'>
<pose>-2.11635 -1.73906 0 0 -0 0</pose>
<link name='Wall_12'>
<collision name='Wall_12_Collision'>
<geometry>
<box>
<size>1.75 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_12_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>1.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-1.61999 -0.744014 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_15'>
<collision name='Wall_15_Collision'>
<geometry>
<box>
<size>2.75 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_15_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>2.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>1.62754 -0.744014 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_17'>
<collision name='Wall_17_Collision'>
<geometry>
<box>
<size>1 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_17_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>1 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-0.827543 -0.3078 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_19'>
<collision name='Wall_19_Collision'>
<geometry>
<box>
<size>1 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_19_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>1 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>0.327543 -0.319014 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_2'>
<collision name='Wall_2_Collision'>
<geometry>
<box>
<size>5 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_2_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>0.062177 -2.89677 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_29'>
<collision name='Wall_29_Collision'>
<geometry>
<box>
<size>3.75 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_29_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-0.305988 0.758719 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_31'>
<collision name='Wall_31_Collision'>
<geometry>
<box>
<size>3.75 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_31_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-0.300519 2.09188 0 0 -0 0</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_33'>
<collision name='Wall_33_Collision'>
<geometry>
<box>
<size>1.5 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_33_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>1.65085 1.41824 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_36'>
<collision name='Wall_36_Collision'>
<geometry>
<box>
<size>2.75 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_36_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>2.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>2.3125 1.40828 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_38'>
<collision name='Wall_38_Collision'>
<geometry>
<box>
<size>5.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_38_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>5.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-0.19189 2.74413 0 0 -0 3.14159</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_4'>
<collision name='Wall_4_Collision'>
<geometry>
<box>
<size>2 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_4_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>2 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>2.6265 -1.8264 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_40'>
<collision name='Wall_40_Collision'>
<geometry>
<box>
<size>1.5 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_40_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-2.25486 1.43754 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_44'>
<collision name='Wall_44_Collision'>
<geometry>
<box>
<size>3 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_44_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-2.89932 1.47176 0 0 -0 -1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_46'>
<collision name='Wall_46_Collision'>
<geometry>
<box>
<size>2.25 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_46_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>2.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-1.87754 0.1172 0 0 -0 3.14159</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_48'>
<collision name='Wall_48_Collision'>
<geometry>
<box>
<size>2.13496 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_48_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>2.13496 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>1.32002 0.107134 0 0 -0 0.001157</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<link name='Wall_6'>
<collision name='Wall_6_Collision'>
<geometry>
<box>
<size>2 0.15 2.5</size>
</box>
</geometry>
<pose>0 0 1.25 0 -0 0</pose>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='Wall_6_Visual'>
<pose>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>2 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
<meta>
<layer>0</layer>
</meta>
</visual>
<pose>-2.50971 -1.81518 0 0 -0 1.5708</pose>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<static>1</static>
</model>
<gravity>0 0 -9.8</gravity>
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<!-- ===== 场景 ===== -->
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>