forked from zbw/yiliao2026
gc_navigation2_slamtoolbox可使用amcl
This commit is contained in:
陌欢醉
6
.gitignore
vendored
6
.gitignore
vendored
@@ -1,8 +1,8 @@
|
||||
# Build artifacts
|
||||
build/
|
||||
build
|
||||
|
||||
# Install artifacts
|
||||
install/
|
||||
install
|
||||
|
||||
# Log artifacts
|
||||
log/
|
||||
log
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
image: map_1770592084.pgm
|
||||
image: my_map.pgm
|
||||
mode: trinary
|
||||
resolution: 0.05
|
||||
origin: [-2.15, -5.81, 0]
|
||||
|
||||
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user