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/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