m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

feat: improve AGV navigation performance

Browse Source
This commit is contained in:
clhchan
2026-05-26 11:15:41 +08:00
parent 39cb5cf26f
commit f966a123e4
24 changed files with 2208 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_pro_navigation2/launch/navigation2_active.launch.py
+46 -11
View File
@@ -3,11 +3,16 @@ import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription
from launch.actions import (
DeclareLaunchArgument,
GroupAction,
IncludeLaunchDescription,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node, SetRemap
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
@@ -27,7 +32,8 @@ def generate_launch_description():
'param',
param_file_name))
nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')
nav2_launch_file_dir = os.path.join(
get_package_share_directory('nav2_bringup'), 'launch')
rviz_config_dir = os.path.join(
get_package_share_directory('agv_pro_navigation2'),
@@ -39,18 +45,47 @@ def generate_launch_description():
'map',
default_value=map_dir,
description='Full path to map file to load'),
DeclareLaunchArgument(
'params_file',
default_value=param_dir,
description='Full path to param file to load'),
IncludeLaunchDescription(
PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']),
launch_arguments={
'map': map_dir,
'params_file': param_dir}.items(),
),
GroupAction(actions=[SetRemap(
src='/goal_pose',
dst='/goal_pose_nav2',
)] + [
SetRemap(
src=f'/{action}/_action/{suffix}',
dst=f'/{action}_nav2/_action/{suffix}')
for action in ('navigate_to_pose', 'navigate_through_poses')
for suffix in ('send_goal', 'get_result', 'cancel_goal', 'feedback', 'status')
] + [
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[nav2_launch_file_dir, '/bringup_launch.py']),
launch_arguments={
'map': map_dir,
'params_file': param_dir,
}.items()),
], scoped=True),
Node(
package='agv_pro_calibration',
executable='navigate_to_pose_refiner_proxy',
name='navigate_to_pose_refiner_proxy',
output='screen',
parameters=[{'use_sim_time': use_sim_time}]),
Node(
package='agv_pro_calibration',
executable='final_pose_refiner',
name='final_pose_refiner',
output='screen',
parameters=[{
'use_sim_time': use_sim_time,
'final_pose_refiner_auto_start_on_nav_success': False,
}]),
Node(
package='rviz2',
agv_pro_navigation2/map/map.pgm
+2 -2
View File
File diff suppressed because one or more lines are too long
agv_pro_navigation2/map/map.yaml
+2 -2
View File
@@ -1,7 +1,7 @@
image: map.pgm
mode: trinary
resolution: 0.05
origin: [-10, -24.4, 0]
origin: [-22.8, -10, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
free_thresh: 0.25
agv_pro_navigation2/map/map1.pgm
+4
View File
File diff suppressed because one or more lines are too long
agv_pro_navigation2/map/map1.yaml
+7
View File
@@ -0,0 +1,7 @@
image: map1.pgm
mode: trinary
resolution: 0.05
origin: [-10, -10, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
agv_pro_navigation2/map/map2.pgm
+4
View File
File diff suppressed because one or more lines are too long
agv_pro_navigation2/map/map2.yaml
+7
View File
@@ -0,0 +1,7 @@
image: map.pgm
mode: trinary
resolution: 0.05
origin: [-21.2, -22.8, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
agv_pro_navigation2/param/agvpro.yaml
+31 -17
View File
@@ -14,10 +14,13 @@ amcl:
global_frame_id: "map"
lambda_short: 0.1
laser_likelihood_max_dist: 2.0
laser_max_range: 100.0
laser_min_range: -1.0
# /* 阶段4修改：AMCL 激光匹配最大有效距离，限制远距离无效数据影响粒子权重，初始值为 100.0m */
laser_max_range: 10.0
# /* 阶段4修改：AMCL 激光匹配最小有效距离，过滤雷达近距盲区数据，初始值为 -1.0 */
laser_min_range: 0.2
laser_model_type: "likelihood_field"
max_beams: 60
# /* 阶段4修改：AMCL 每次更新采样的激光束数量，提高定位匹配信息量，初始值为 60 */
max_beams: 90
max_particles: 2000
min_particles: 500
odom_frame_id: "odom"
@@ -25,14 +28,19 @@ amcl:
pf_z: 0.99
recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0
resample_interval: 2
robot_model_type: "nav2_amcl::OmniMotionModel"
# /* 阶段4修改：AMCL 粒子重采样间隔，提高低速接近目标时的定位收敛及时性，初始值为 2 */
resample_interval: 1
# /* 阶段4修改：AMCL 里程计运动模型，减少未使用 y 方向速度时的横向运动假设，初始值为 nav2_amcl::OmniMotionModel */
robot_model_type: "nav2_amcl::DifferentialMotionModel"
save_pose_rate: 0.5
sigma_hit: 0.02
# /* 阶段4修改：AMCL 激光命中似然标准差，降低真实车地图匹配误差导致的权重过敏感，初始值为 0.02 */
sigma_hit: 0.04
tf_broadcast: true
transform_tolerance: 0.3
update_min_a: 0.06
update_min_d: 0.025
# /* 方案2修改：降低 AMCL 角度更新触发阈值，让目标附近低速小角度调整时定位更新更及时，初始值为 0.06rad */
update_min_a: 0.04
# /* 方案2修改：降低 AMCL 位移更新触发阈值，让目标附近低速接近时更频繁使用激光修正定位，初始值为 0.025m */
update_min_d: 0.015
z_hit: 0.7
z_max: 0.001
z_rand: 0.059
@@ -132,13 +140,15 @@ controller_server:
general_goal_checker:
stateful: True
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
# /* 阶段2修改：最终位置成功阈值，避免 25cm 内提前判定到点，初始值为 0.25m */
xy_goal_tolerance: 0.05
# /* 阶段2临时验证修改：最终姿态成功阈值，放宽到约 5.7 度以验证大角度掉头时末端旋转振荡是否由 0.05rad 过严导致，初始值为 0.25rad*/
yaw_goal_tolerance: 0.8
# DWB parameters
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: True
min_vel_x: 0.0
min_vel_x: -0.03
min_vel_y: 0.0
max_vel_x: 0.26
max_vel_y: 0.0
@@ -150,10 +160,10 @@ controller_server:
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
acc_lim_theta: 2.5
decel_lim_x: -2.5
decel_lim_y: 0.0
decel_lim_theta: -3.2
decel_lim_theta: -2.5
vx_samples: 20
vy_samples: 5
vtheta_samples: 40
@@ -161,8 +171,10 @@ controller_server:
linear_granularity: 0.05
angular_granularity: 0.025
transform_tolerance: 0.1
xy_goal_tolerance: 0.25
trans_stopped_velocity: 0.1
# /* 阶段2修改：DWB RotateToGoal 进入末端减速/旋转模式的位置窗口，避免 25cm 内过早进入末端旋转逻辑，初始值为 0.25m */
xy_goal_tolerance: 0.03 #0.03
# /* 阶段2修改：进入只旋转阶段前允许的最大平移速度，避免平移速度小于 0.1m/s 时过早停止平移修正，初始值为 0.1m/s */
trans_stopped_velocity: 0.01 #0.01
short_circuit_trajectory_evaluation: True
stateful: True
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
@@ -292,8 +304,10 @@ planner_server:
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 2.0
# /* 阶段2修改：全局规划目标替代容差，避免 2.0m 范围内替代终点影响精度测试，初始值为 2.0m */
tolerance: 0.05 #0.05
use_astar: false
# /* 阶段2修改：是否允许规划到未知区域，避免精度测试阶段出现不可控路径终点，初始值为 true */
allow_unknown: true
planner_server_rclcpp_node:
@@ -358,4 +372,4 @@ velocity_smoother:
odom_topic: "odom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
velocity_timeout: 1.0
agv_pro_navigation2/scripts/example_nav_to_pose.py
+103 -30
View File
@@ -1,17 +1,36 @@
#! /usr/bin/env python3
import argparse
import sys
import yaml
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
from rclpy.duration import Duration
"""
Basic navigation demo to go to pose.
"""
def parse_arguments():
parser = argparse.ArgumentParser(description='Send navigation test goals.')
parser.add_argument(
'--localization',
choices=('amcl', 'slam'),
default='amcl',
help='Localization backend started by navigation2_active.launch.py.')
parser.add_argument(
'targets',
nargs='*',
help='Waypoint letters to execute once, for example AB. Omit to loop ABCDE.')
return parser.parse_args()
def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float):
"""
Set the initial pose of the robot for AMCL localization.
Set the initial pose of the robot for the active localization backend.
Args:
navigator (BasicNavigator): The navigator instance controlling the robot.
@@ -30,21 +49,7 @@ def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, o
navigator.setInitialPose(initial_pose)
def navigate_to_goal(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float, verbose: bool = False) -> bool:
"""
Navigate the robot to a target goal pose.
Args:
navigator (BasicNavigator): The navigator instance controlling the robot.
x (float): Goal X position in the map frame.
y (float): Goal Y position in the map frame.
oz (float): Orientation Z component (quaternion).
ow (float): Orientation W component (quaternion).
verbose (bool, optional): If True, prints navigation feedback such as estimated arrival time. Default is False.
Returns:
bool: True if navigation succeeded, False otherwise.
"""
def make_goal_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float) -> PoseStamped:
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
@@ -52,6 +57,22 @@ def navigate_to_goal(navigator: BasicNavigator, x: float, y: float, oz: float, o
goal_pose.pose.position.y = y
goal_pose.pose.orientation.z = oz
goal_pose.pose.orientation.w = ow
return goal_pose
def navigate_to_goal(navigator: BasicNavigator, goal_pose: PoseStamped, verbose: bool = False) -> bool:
"""
Navigate the robot to a target goal pose.
Args:
navigator (BasicNavigator): The navigator instance controlling the robot.
goal_pose (PoseStamped): Goal pose in the map frame.
verbose (bool, optional): If True, prints navigation feedback such as estimated arrival time. Default is False.
Returns:
bool: True if navigation succeeded, False otherwise.
"""
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
navigator.goToPose(goal_pose)
@@ -74,24 +95,76 @@ def navigate_to_goal(navigator: BasicNavigator, x: float, y: float, oz: float, o
return False
if __name__ == '__main__':
cli_args = parse_arguments()
rclpy.init()
navigator = BasicNavigator()
# Set robot initial pose
# set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)
if cli_args.localization == 'slam':
# slam_toolbox on Humble is not a Nav2 lifecycle-managed localizer.
navigator._waitForNodeToActivate('bt_navigator')
navigator.info('Nav2 is ready for use!')
else:
# AMCL obtains its initial origin pose from agvpro.yaml.
navigator.initial_pose_received = True
navigator.waitUntilNav2Active()
# Wait for navigation to fully activate, since autostarting nav2
# navigator.waitUntilNav2Active()
# Try to load waypoints from YAML, fallback to hardcoded defaults
waypoints = {}
try:
with open('waypoints.yaml', 'r') as f:
waypoints = (yaml.safe_load(f) or {}).get('waypoints', {})
except FileNotFoundError:
with open('waypoints.yaml', 'w') as f:
yaml.dump({'waypoints': {}}, f, default_flow_style=False)
goal_A = [1.6766083240509033,0.37930558800697327,-0.03491306994337919, 0.9993903529387947]
goal_B = [-0.5062443017959595,1.559376835823059,0.6869307039904945,0.7267229237578264]
goals = {
'A': waypoints.get('A', [4.89649,-0.617371,0.706899,0.707315]),
'B': waypoints.get('B', [0.90387,-0.446105,0.273676,0.961822]),
'C': waypoints.get('C', [4.46734,-0.532388,0.969886,-0.243558]),
'D': waypoints.get('D', [-0.0233348,0.00798563,0.999322,0.0368173]),
'E': waypoints.get('E', [2.33651,-0.440663,0.937234,-0.3487]),
}
x_goal, y_goal, orientation_z, orientation_w = goal_A
success = navigate_to_goal(navigator, x_goal, y_goal, orientation_z, orientation_w)
print("Navigation result:", success)
args = cli_args.targets
loop_targets = False
if args:
targets = []
for arg in args:
for c in arg.upper():
if c in goals:
targets.append(c)
else:
targets = ['A', 'B', 'C', 'D', 'E']
loop_targets = True
print('No target arguments provided; running A-B-C-D-E repeatedly. Press Ctrl+C to stop.')
x_goal, y_goal, orientation_z, orientation_w = goal_B
success = navigate_to_goal(navigator, x_goal, y_goal, orientation_z, orientation_w)
print("Navigation result:", success)
if not targets:
print('No valid target names provided. Use names such as A, B, C, D, E or AB.')
rclpy.shutdown()
sys.exit(1)
rclpy.shutdown()
try:
cycle_index = 1
while rclpy.ok():
if loop_targets:
print(f'============= cycle {cycle_index}: ABCDE =============')
for name in targets:
if not rclpy.ok():
break
x_goal, y_goal, orientation_z, orientation_w = goals[name]
input(f'============={name}==================\n')
goal_pose = make_goal_pose(navigator, x_goal, y_goal, orientation_z, orientation_w)
success = navigate_to_goal(navigator, goal_pose)
print("Navigation result:", goals[name], success)
if not loop_targets:
break
cycle_index += 1
except KeyboardInterrupt:
print('Navigation loop interrupted by user.')
navigator.cancelTask()
finally:
if rclpy.ok():
rclpy.shutdown()
agv_pro_navigation2/scripts/power_on.py
+3
View File
@@ -0,0 +1,3 @@
from pymycobot import MyAGVPro
m = MyAGVPro('/dev/agvpro_controller')
m.power_on()
agv_pro_navigation2/scripts/record_waypoints.py
+119
View File
@@ -0,0 +1,119 @@
#!/usr/bin/env python3
"""Record waypoints by sampling stable map->base_footprint pose."""
import sys
import threading
import time
import rclpy
from rclpy.node import Node
from tf2_ros import Buffer, TransformListener
import yaml
class WaypointRecorder(Node):
def __init__(self):
super().__init__('waypoint_recorder')
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.yaml_path = 'waypoints.yaml'
self.waypoints = {}
self._load_existing()
def _load_existing(self):
try:
with open(self.yaml_path, 'r') as f:
data = yaml.safe_load(f) or {}
self.waypoints = data.get('waypoints', {})
n = len(self.waypoints)
if n > 0:
self.get_logger().info(f'Loaded {n} existing waypoints from {self.yaml_path}')
except FileNotFoundError:
self.waypoints = {}
def _save(self):
data = {'waypoints': self.waypoints}
with open(self.yaml_path, 'w') as f:
yaml.dump(data, f, default_flow_style=False, sort_keys=False)
self.get_logger().info(f'Saved waypoints to {self.yaml_path}')
def _sample_pose(self, duration_sec=3.0, rate_hz=20):
xs, ys, zs, ws = [], [], [], []
dt = 1.0 / rate_hz
start = time.time()
while time.time() - start < duration_sec:
try:
trans = self.tf_buffer.lookup_transform(
'map', 'base_footprint', rclpy.time.Time()
)
t = trans.transform.translation
r = trans.transform.rotation
xs.append(t.x)
ys.append(t.y)
zs.append(r.z)
ws.append(r.w)
except Exception:
pass
time.sleep(dt)
if not xs:
return None
xs.sort()
ys.sort()
zs.sort()
ws.sort()
n = len(xs)
mid = n // 2
if n % 2 == 1:
return [xs[mid], ys[mid], zs[mid], ws[mid]]
return [
(xs[mid - 1] + xs[mid]) / 2,
(ys[mid - 1] + ys[mid]) / 2,
(zs[mid - 1] + zs[mid]) / 2,
(ws[mid - 1] + ws[mid]) / 2,
]
def run(self):
self.get_logger().info('Waypoint recorder ready.')
self.get_logger().info('Enter A/B/C/D/E to record, q to quit.')
while rclpy.ok():
try:
cmd = input('> ').strip().upper()
except EOFError:
break
if cmd == 'Q':
break
if cmd in 'ABCDE':
self.get_logger().info(
f'Sampling pose for {cmd} ({3}s, keep still)...'
)
pose = self._sample_pose()
if pose is None:
self.get_logger().error(
'Failed to sample pose. Is AMCL running?'
)
continue
self.waypoints[cmd] = [float(f'{v:.6f}') for v in pose]
self.get_logger().info(f'{cmd}: {self.waypoints[cmd]}')
self._save()
else:
self.get_logger().warn('Use A/B/C/D/E or q.')
def main(args=None):
rclpy.init(args=args)
node = WaypointRecorder()
spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
spin_thread.start()
try:
node.run()
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
agv_pro_navigation2/scripts/waypoints.yaml
+26
View File
@@ -0,0 +1,26 @@
waypoints:
A:
- 4.24531
- 1.866955
- 0.584964
- 0.811059
B:
- 3.624886
- -2.221018
- -0.569358
- 0.82209
C:
- 1.900274
- 1.940145
- -0.737521
- 0.675324
D:
- 0.882323
- -0.456797
- 0.745742
- 0.666235
E:
- -1.63798
- -0.847909
- 0.978385
- 0.206792