From 92dfaebaadbfd33487bf490dde03127670482c44 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=99=8C=E6=AC=A2=E9=86=89?= <264854363@qq.com> Date: Thu, 4 Jun 2026 15:07:47 +0800 Subject: [PATCH] =?UTF-8?q?gc=5Fnavigation2=5Fslamtoolbox=E5=8F=AF?= =?UTF-8?q?=E4=BD=BF=E7=94=A8amcl?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 6 +- src/gc_bringup/launch/navigation.launch.py | 1 + .../launch/gc_nav2_with_slamtoolbox.launch.py | 189 +++++------------- .../maps/my_map.yaml | 2 +- .../param/gc_navigation.yaml | 92 ++++----- 5 files changed, 103 insertions(+), 187 deletions(-) diff --git a/.gitignore b/.gitignore index ca74fd5..96e8f20 100644 --- a/.gitignore +++ b/.gitignore @@ -1,8 +1,8 @@ # Build artifacts -build/ +build # Install artifacts -install/ +install # Log artifacts -log/ +log diff --git a/src/gc_bringup/launch/navigation.launch.py b/src/gc_bringup/launch/navigation.launch.py index b5bed29..aa035b8 100644 --- a/src/gc_bringup/launch/navigation.launch.py +++ b/src/gc_bringup/launch/navigation.launch.py @@ -40,6 +40,7 @@ def generate_launch_description(): robot_description = ParameterValue( Command(['xacro ', default_model_path]), value_type=str) + # ==================== Gazebo ==================== start_gazebo = ExecuteProcess( condition=IfCondition(use_gazebo), diff --git a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slamtoolbox.launch.py b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slamtoolbox.launch.py index 4a22630..cb94c4d 100644 --- a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slamtoolbox.launch.py +++ b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slamtoolbox.launch.py @@ -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 diff --git a/src/gc_navigation2_slamtoolbox/maps/my_map.yaml b/src/gc_navigation2_slamtoolbox/maps/my_map.yaml index 938b0ed..d5afba9 100644 --- a/src/gc_navigation2_slamtoolbox/maps/my_map.yaml +++ b/src/gc_navigation2_slamtoolbox/maps/my_map.yaml @@ -1,4 +1,4 @@ -image: map_1770592084.pgm +image: my_map.pgm mode: trinary resolution: 0.05 origin: [-2.15, -5.81, 0] diff --git a/src/gc_navigation_fish/param/gc_navigation.yaml b/src/gc_navigation_fish/param/gc_navigation.yaml index c2bb006..e24e71e 100644 --- a/src/gc_navigation_fish/param/gc_navigation.yaml +++ b/src/gc_navigation_fish/param/gc_navigation.yaml @@ -1,51 +1,51 @@ -# amcl: -# ros__parameters: -# use_sim_time: True -# alpha1: 0.2 -# alpha2: 0.2 -# alpha3: 0.2 -# alpha4: 0.2 -# alpha5: 0.2 -# base_frame_id: "base_link" -# beam_skip_distance: 0.5 -# beam_skip_error_threshold: 0.9 -# beam_skip_threshold: 0.3 -# do_beamskip: false -# global_frame_id: "map" -# lambda_short: 0.1 -# laser_likelihood_max_dist: 2.0 -# laser_max_range: 100.0 -# laser_min_range: -1.0 -# laser_model_type: "likelihood_field" -# max_beams: 60 -# max_particles: 2000 -# min_particles: 500 -# odom_frame_id: "odom" -# pf_err: 0.05 -# pf_z: 0.99 -# recovery_alpha_fast: 0.0 -# recovery_alpha_slow: 0.0 -# resample_interval: 1 -# robot_model_type: "nav2_amcl::DifferentialMotionModel" -# save_pose_rate: 0.5 -# sigma_hit: 0.2 -# tf_broadcast: true -# transform_tolerance: 10.0 -# update_min_a: 0.2 -# update_min_d: 0.25 -# z_hit: 0.5 -# z_max: 0.05 -# z_rand: 0.5 -# z_short: 0.05 -# scan_topic: scan +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" # 全局坐标系 + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 10.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan -# amcl_map_client: -# ros__parameters: -# use_sim_time: True +amcl_map_client: + ros__parameters: + use_sim_time: True -# amcl_rclcpp_node: -# ros__parameters: -# use_sim_time: True +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True bt_navigator: ros__parameters: