zbw/yiliao2026
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

gc_navigation2_slamtoolbox可使用amcl

Browse Source
This commit is contained in:
陌欢醉
2026-06-04 15:07:47 +08:00
parent c87b5d7990
commit 92dfaebaad
5 changed files with 103 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files

189
src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slamtoolbox.launch.py
View File

@@ -1,4 +1,3 @@
from launch.actions import IncludeLaunchDescription
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
@@ -6,147 +5,20 @@ from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, Time
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
# def generate_launch_description():
# # 获取包路径
# gc_navigation2_dir = get_package_share_directory(
# 'gc_navigation2_slamtoolbox')
# nav2_bringup_dir = get_package_share_directory('nav2_bringup')
# # 声明参数
# use_sim_time = LaunchConfiguration('use_sim_time', default='True')
# slam = LaunchConfiguration('slam', default='True')
# localization = LaunchConfiguration('localization', default='True')
# # 定义文件路径
# map_yaml_path = os.path.join(gc_navigation2_dir, 'maps', 'my_map.yaml')
# nav2_param_path = os.path.join(
# gc_navigation2_dir, 'params', 'gc_navigation.yaml')
# slam_params_file = os.path.join(
# gc_navigation2_dir, 'config', 'slam_toolbox_localization.yaml')
# # 定义地图基础路径（用于 .posegraph 文件）
# map_base_path = os.path.join(gc_navigation2_dir, 'maps', 'my_map')
# # 检查 .posegraph 文件是否存在
# posegraph_path = map_base_path + '.posegraph'
# if not os.path.exists(posegraph_path):
# print(f"警告: .posegraph 文件不存在: {posegraph_path}")
# print("将使用 AMCL 替代")
# use_slam_toolbox = False
# else:
# use_slam_toolbox = True
# print(f"找到 .posegraph 文件: {posegraph_path}")
# # 创建启动描述
# ld = LaunchDescription([
# DeclareLaunchArgument('use_sim_time', default_value='True'),
# DeclareLaunchArgument('slam', default_value='True'),
# DeclareLaunchArgument('localization', default_value='True'),
# ])
# # 如果使用 slam_toolbox
# if use_slam_toolbox:
# slam_toolbox_node = Node(
# package='slam_toolbox',
# executable='localization_slam_toolbox_node',
# name='slam_toolbox',
# output='screen',
# parameters=[
# slam_params_file,
# {'map_file_name': map_base_path}
# ]
# )
# ld.add_action(slam_toolbox_node)
# # 使用 slam_toolbox 时，Nav2 不启用定位
# localization_arg = 'False'
# print("slam成功配置")
# ld.add_action(TimerAction(
# period=1.5,
# actions=[]
# ))
# else:
# # 使用 AMCL
# localization_arg = 'True'
# # Nav2 启动 - 使用正确的路径
# nav2_launch_path = os.path.join(
# nav2_bringup_dir, 'launch', 'bringup_launch.py')
# if not os.path.exists(nav2_launch_path):
# print(f"错误: 找不到 Nav2 启动文件: {nav2_launch_path}")
# return ld
# else:
# print("nav2桥，通过")
# nav2_bringup_launch = IncludeLaunchDescription(
# PythonLaunchDescriptionSource(nav2_launch_path),
# launch_arguments={
# 'map': map_yaml_path,
# 'use_sim_time': use_sim_time,
# 'params_file': nav2_param_path,
# 'slam': slam,
# 'localization': localization_arg,
# 'autostart': 'True',
# }.items(),
# )
# print("结束配置")
# ld.add_action(nav2_bringup_launch)
# return ld
# def generate_launch_description():
# ld = LaunchDescription()
# # 获取参数文件路径
# slam_pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
# slam_file = os.path.join(
# slam_pkg_share, 'config', 'slam_toolbox_localization.yaml')
# use_sim_time = LaunchConfiguration('use_sim_time', default='true')
# map_base_path = os.path.join(slam_pkg_share, 'maps', 'my_map')
# posegraph_path = map_base_path+'.posegraph'
# # 创建SLAM Toolbox节点
# slam_toolbox_node = Node(
# package='slam_toolbox',
# executable='async_slam_toolbox_node', # 或 sync_slam_toolbox_node
# name='slam_toolbox',
# output='screen',
# parameters=[slam_file,
# {
# 'map_file_name': map_base_path,
# 'use_sim_time': use_sim_time
# }]
# )
# # ld.add_action(slam_toolbox_node)
# nav2_bringup_dir = get_package_share_directory('nav2_bringup')
# nav_pkg = get_package_share_directory(
# 'gc_navigation2_slamtoolbox')
# map_yaml_path = LaunchConfiguration('map', default=os.path.join(
# nav_pkg, 'maps', 'my_map.yaml'))
# nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
# nav_pkg, 'params', 'gc_navigation.yaml'))
# nav2_bringup_launch = IncludeLaunchDescription(
# PythonLaunchDescriptionSource(
# [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
# launch_arguments={
# 'map': map_yaml_path,
# 'use_sim_time': use_sim_time,
# 'params_file': nav2_param_path}.items(),
# )
# time=TimerAction(period=2.0,
# actions=[])
# ld.add_action(time)
# ld.add_action(nav2_bringup_launch)
# return ld
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command
from launch.actions import ExecuteProcess
def generate_launch_description():
ld = LaunchDescription()
# =============================1.定位到包的地址=============================================================
gc_navigation_fish_dir = get_package_share_directory(
'gc_navigation2_slamtoolbox')
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
origincar_urdf_dir = get_package_share_directory(
'origincar_description')
# =============================2.声明参数，获取配置文件路径===================================================
# use_sim_time 这里要设置成true,因为gazebo是仿真环境，其时间是通过/clock话题获取，而不是系统时间
@@ -155,6 +27,7 @@ def generate_launch_description():
gc_navigation_fish_dir, 'maps', 'my_map.yaml'))
nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
gc_navigation_fish_dir, 'params', 'gc_navigation.yaml'))
# =============================3.声明启动launch文件，传入：地图路径、是否使用仿真时间以及nav2参数文件==============
nav2_bringup_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
@@ -165,6 +38,48 @@ def generate_launch_description():
'params_file': nav2_param_path}.items(),
)
return LaunchDescription([nav2_bringup_launch])
# =============================4.Gazebo仿真设置==========================================================
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')
start_gazebo_cmd = ExecuteProcess(
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
'-s', 'libgazebo_ros_factory.so', 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],
output='screen'
)
robot_description = ParameterValue(Command(["xacro ", LaunchConfiguration("model")]),
value_type=str)
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[{"robot_description": robot_description,
'use_sim_time': True, 'publish_frequency': 30.0}]
)
# 2.启动 joint_state_publisher 节点发布非固定关节状态
joint_state_publisher = Node(
package="joint_state_publisher",
executable="joint_state_publisher"
)
ld.add_action(model)
ld.add_action(nav2_bringup_launch)
ld.add_action(start_gazebo_cmd)
ld.add_action(spawn_entity_cmd)
ld.add_action(robot_state_publisher)
# ld.add_action(joint_state_publisher)
return ld