feat: improve AGV navigation performance

agv_pro_calibration/agv_pro_calibration/final_pose_refiner.py

@@ -0,0 +1,388 @@
+#!/usr/bin/env python3
+"""Yaw-only final pose refinement helper for AGV Pro."""
+
+import math
+import os
+import shlex
+
+import rclpy
+from action_msgs.msg import GoalStatus, GoalStatusArray
+from geometry_msgs.msg import PoseStamped, Twist
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from std_msgs.msg import String
+from tf2_ros import Buffer, TransformException, TransformListener
+
+class FinalPoseRefiner(Node):
+    """Refine only the final map->base_footprint yaw with direct low-speed cmd_vel."""
+
+    def __init__(self):
+        super().__init__('final_pose_refiner')
+        self.param_prefix = 'final_pose_refiner_'
+        self.start_param = f'{self.param_prefix}start'
+        self.cancel_param = f'{self.param_prefix}cancel'
+        self.auto_start_param = f'{self.param_prefix}auto_start_on_nav_success'
+        self.log_separator = '------------------------------------------------------------'
+        self.cmd_vel_topic = '/cmd_vel'
+        self.goal_topic = '/goal_pose'
+        self.action_goal_topic = '/final_pose_refiner/goal_pose'
+        self.nav_status_topic = '/navigate_to_pose/_action/status'
+        self.nav2_status_topic = '/navigate_to_pose_nav2/_action/status'
+        self.global_frame = 'map'
+        self.base_frame = 'base_footprint'
+
+        self._declare_param('status_topic', '/final_pose_refiner/status')
+        self.declare_parameter(self.start_param, False)
+        self.declare_parameter(self.cancel_param, False)
+        self.declare_parameter(self.auto_start_param, False)
+        self._declare_param('handoff_distance', 0.20)
+        self._declare_param('yaw_tolerance', 0.04)
+        self._declare_param('settle_time', 0.5)
+        self._declare_param('timeout', 20.0)
+        self._declare_param('k_yaw', 0.5)
+        self._declare_param('max_wz', 0.35)
+        self._declare_param('min_cmd_w', 0.006)
+
+        self.cmd_vel_pub = self.create_publisher(Twist, self.cmd_vel_topic, 10)
+        self.status_pub = self.create_publisher(String, self._param('status_topic'), 10)
+        self.goal_sub = self.create_subscription(
+            PoseStamped,
+            self.goal_topic,
+            self._on_goal,
+            10,
+        )
+        self.action_goal_sub = self.create_subscription(
+            PoseStamped,
+            self.action_goal_topic,
+            self._on_goal,
+            10,
+        )
+        self.nav_status_sub = self.create_subscription(
+            GoalStatusArray,
+            self.nav_status_topic,
+            self._on_nav_status,
+            10,
+        )
+        self.nav2_status_sub = self.create_subscription(
+            GoalStatusArray,
+            self.nav2_status_topic,
+            self._on_nav_status,
+            10,
+        )
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.state = 'idle'
+        self.goal = None
+        self.target = None
+        self.waiting_for_nav_success = False
+        self.active_nav_goal_ids = set()
+        self.refined_nav_goal_ids = set()
+        self.start_time = None
+        self.settle_start_time = None
+        self.last_log_time = self.get_clock().now()
+
+        self.timer = self.create_timer(1.0 / 20.0, self.on_timer)
+
+        self.get_logger().info(
+            'final_pose_refiner ready in yaw-only mode. A navigation proxy may submit the '
+            f'target and set {self.start_param}:=true, or these may be provided manually.'
+        )
+
+    def _declare_param(self, name, value):
+        self.declare_parameter(f'{self.param_prefix}{name}', value)
+
+    def _param(self, name):
+        return self.get_parameter(f'{self.param_prefix}{name}').value
+
+    def _on_goal(self, msg):
+        if msg.header.frame_id and msg.header.frame_id != self.global_frame:
+            self.get_logger().warn(
+                f'Ignoring goal in frame "{msg.header.frame_id}". Expected "{self.global_frame}".'
+            )
+            return
+
+        q = msg.pose.orientation
+        target_yaw = self._yaw_from_quaternion(q.x, q.y, q.z, q.w)
+        self.target = (msg.pose.position.x, msg.pose.position.y, target_yaw)
+        self.waiting_for_nav_success = True
+        self.active_nav_goal_ids.clear()
+        self.get_logger().info(
+            f'Updated refine target from topic: x={msg.pose.position.x:.4f}, '
+            f'y={msg.pose.position.y:.4f}, yaw={math.degrees(target_yaw):.2f} deg'
+        )
+
+    def _on_nav_status(self, msg):
+        if not self.get_parameter(self.auto_start_param).value:
+            return
+
+        if self.state != 'idle' or self.target is None or not self.waiting_for_nav_success:
+            return
+
+        for status in msg.status_list:
+            goal_id = tuple(status.goal_info.goal_id.uuid)
+            if status.status in (GoalStatus.STATUS_ACCEPTED, GoalStatus.STATUS_EXECUTING):
+                self.active_nav_goal_ids.add(goal_id)
+            elif status.status == GoalStatus.STATUS_SUCCEEDED:
+                if (
+                    goal_id in self.active_nav_goal_ids and
+                    goal_id not in self.refined_nav_goal_ids
+                ):
+                    self.refined_nav_goal_ids.add(goal_id)
+                    self.get_logger().info(
+                        'Detected Nav2 goal succeeded; starting final yaw refinement.'
+                    )
+                    self._start_refine()
+                    return
+            elif status.status in (GoalStatus.STATUS_CANCELED, GoalStatus.STATUS_ABORTED):
+                if goal_id in self.active_nav_goal_ids:
+                    self.waiting_for_nav_success = False
+                    self.active_nav_goal_ids.discard(goal_id)
+
+    def on_timer(self):
+        if self.get_parameter(self.cancel_param).value:
+            if self.state == 'running':
+                self._finish_refine('canceled')
+            else:
+                self._reset_cancel_refine()
+            return
+
+        if self.state == 'running':
+            self._run_refine_step()
+            return
+
+        if self.get_parameter(self.start_param).value:
+            self._start_refine()
+
+    def _start_refine(self):
+        self._reset_cancel_refine()
+        if self.target is None:
+            self.get_logger().warn(
+                'Cannot start final refinement: no /goal_pose has been received yet.'
+            )
+            self._publish_status('no_goal')
+            self._reset_start_refine()
+            return
+
+        pose = self._lookup_pose()
+        if pose is None:
+            self.get_logger().warn('Cannot start final refinement: TF is not available.')
+            self._publish_status('failed_tf')
+            self._reset_start_refine()
+            return
+
+        target = self.target
+        distance, yaw_error = self._calculate_error(pose, target)
+        handoff_distance = max(self._param('handoff_distance'), 0.0)
+        if distance > handoff_distance:
+            self.get_logger().warn(
+                f'Cannot start final refinement: distance={distance:.3f} m exceeds '
+                f'handoff_distance={handoff_distance:.3f} m.'
+            )
+            self._publish_status('handoff_distance_exceeded')
+            self._reset_start_refine()
+            return
+
+        self.goal = target
+        self.waiting_for_nav_success = False
+        self.start_time = self.get_clock().now()
+        self.settle_start_time = None
+        self.last_log_time = self.get_clock().now()
+        self.state = 'running'
+        self._publish_status('running')
+        self.get_logger().info(
+            f'\n{self.log_separator}\n'
+            'FINAL YAW REFINE START\n'
+            f'target=({target[0]:.4f}, {target[1]:.4f}, {math.degrees(target[2]):.2f} deg)\n'
+            f'initial_distance={distance:.3f} m, '
+            f'initial_yaw_error={math.degrees(yaw_error):+.2f} deg\n'
+            f'{self.log_separator}'
+        )
+
+    def _run_refine_step(self):
+        pose = self._lookup_pose()
+        if pose is None:
+            self._finish_refine('failed_tf', warn=True)
+            return
+
+        distance, yaw_error = self._calculate_error(pose, self.goal)
+        elapsed = (self.get_clock().now() - self.start_time).nanoseconds / 1e9
+        yaw_tolerance = max(self._param('yaw_tolerance'), 0.0)
+        settle_time = max(self._param('settle_time'), 0.0)
+        timeout = self._param('timeout')
+
+        if timeout > 0.0 and elapsed > timeout:
+            self._finish_refine('timeout', pose, distance, yaw_error, warn=True)
+            return
+
+        if abs(yaw_error) <= yaw_tolerance:
+            now = self.get_clock().now()
+            if self.settle_start_time is None:
+                self.settle_start_time = now
+                self._publish_stop()
+            elif (now - self.settle_start_time).nanoseconds / 1e9 >= settle_time:
+                self._finish_refine('succeeded', pose, distance, yaw_error)
+                return
+            else:
+                self._publish_stop()
+            self._log_progress(pose, distance, yaw_error, elapsed, Twist())
+            return
+
+        self.settle_start_time = None
+        cmd = self._make_yaw_command(yaw_error)
+        self.cmd_vel_pub.publish(cmd)
+        self._log_progress(pose, distance, yaw_error, elapsed, cmd)
+
+    def _make_yaw_command(self, yaw_error):
+        cmd = Twist()
+        max_wz = max(abs(self._param('max_wz')), 0.0)
+        cmd.angular.z = self._clip(self._param('k_yaw') * yaw_error, -max_wz, max_wz)
+        cmd.angular.z = self._apply_min_abs(cmd.angular.z, self._param('min_cmd_w'))
+        return cmd
+
+    def _finish_refine(self, status, pose=None, distance=None, yaw_error=None, warn=False):
+        self._stop_robot()
+        self._reset_start_refine()
+        self._reset_cancel_refine()
+        self.state = 'idle'
+        self.settle_start_time = None
+        self._publish_status(status)
+
+        if pose is not None and distance is not None and yaw_error is not None:
+            msg = (
+                f'\n{self.log_separator}\n'
+                f'FINAL YAW REFINE END: {status}\n'
+                f'distance={distance:.4f} m, '
+                f'yaw_error={math.degrees(yaw_error):+.2f} deg, '
+                f'pose=({pose[0]:.4f}, {pose[1]:.4f}, {math.degrees(pose[2]):.2f} deg)\n'
+                f'{self.log_separator}'
+            )
+        else:
+            msg = (
+                f'\n{self.log_separator}\n'
+                f'FINAL YAW REFINE END: {status}\n'
+                f'{self.log_separator}'
+            )
+
+        if warn:
+            self.get_logger().warn(msg)
+        else:
+            self.get_logger().info(msg)
+
+    def _lookup_pose(self):
+        try:
+            trans = self.tf_buffer.lookup_transform(
+                self.global_frame,
+                self.base_frame,
+                rclpy.time.Time(),
+                timeout=Duration(seconds=0.3),
+            )
+        except TransformException as exc:
+            self.get_logger().warn(f'TF lookup failed: {exc}')
+            return None
+
+        translation = trans.transform.translation
+        rotation = trans.transform.rotation
+        return (
+            translation.x,
+            translation.y,
+            self._yaw_from_quaternion(rotation.x, rotation.y, rotation.z, rotation.w),
+        )
+
+    def _calculate_error(self, pose, target):
+        x, y, yaw = pose
+        target_x, target_y, target_yaw = target
+        distance = math.hypot(target_x - x, target_y - y)
+        yaw_error = self._normalize_angle(target_yaw - yaw)
+        return distance, yaw_error
+
+    def _log_progress(self, pose, distance, yaw_error, elapsed, cmd):
+        now = self.get_clock().now()
+        if (now - self.last_log_time).nanoseconds < 1e9:
+            return
+
+        self.get_logger().info(
+            f'[FINAL YAW REFINE RUNNING] '
+            f'distance={distance:.3f} m, yaw_error={math.degrees(yaw_error):+.2f} deg, '
+            f'elapsed={elapsed:.1f} s, cmd_wz={cmd.angular.z:+.3f}, '
+            f'pose=({pose[0]:.3f}, {pose[1]:.3f}, {math.degrees(pose[2]):.1f} deg)'
+        )
+        self.last_log_time = now
+
+    def _publish_status(self, status):
+        msg = String()
+        msg.data = status
+        self.status_pub.publish(msg)
+
+    def _publish_stop(self):
+        try:
+            self.cmd_vel_pub.publish(Twist())
+        except Exception:
+            pass
+
+    def _stop_robot(self):
+        for _ in range(5):
+            self._publish_stop()
+
+    def _stop_robot_with_ros_cli(self):
+        topic = shlex.quote(self.cmd_vel_topic)
+        zero_twist = (
+            '"{linear: {x: 0.0, y: 0.0, z: 0.0}, '
+            'angular: {x: 0.0, y: 0.0, z: 0.0}}"'
+        )
+        os.system(
+            f'timeout 2s ros2 topic pub --once {topic} '
+            f'geometry_msgs/msg/Twist {zero_twist} >/dev/null 2>&1'
+        )
+
+    def _reset_start_refine(self):
+        self.set_parameters([
+            Parameter(self.start_param, Parameter.Type.BOOL, False),
+        ])
+
+    def _reset_cancel_refine(self):
+        self.set_parameters([
+            Parameter(self.cancel_param, Parameter.Type.BOOL, False),
+        ])
+
+    @staticmethod
+    def _clip(value, low, high):
+        return max(low, min(high, value))
+
+    @staticmethod
+    def _apply_min_abs(value, min_abs):
+        min_abs = max(abs(min_abs), 0.0)
+        if value == 0.0 or abs(value) >= min_abs:
+            return value
+        return math.copysign(min_abs, value)
+
+    @staticmethod
+    def _normalize_angle(angle):
+        return math.atan2(math.sin(angle), math.cos(angle))
+
+    @staticmethod
+    def _yaw_from_quaternion(x, y, z, w):
+        siny_cosp = 2.0 * (w * z + x * y)
+        cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
+        return math.atan2(siny_cosp, cosy_cosp)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = FinalPoseRefiner()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node._stop_robot()
+        node._stop_robot_with_ros_cli()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_calibration/agv_pro_calibration/navigate_to_pose_refiner_proxy.py

@@ -0,0 +1,397 @@
+#!/usr/bin/env python3
+"""Transparent final-refinement proxy for Nav2 pose navigation actions."""
+
+import threading
+import time
+from copy import deepcopy
+
+import rclpy
+from action_msgs.msg import GoalStatus
+from geometry_msgs.msg import PoseStamped
+from nav2_msgs.action import NavigateThroughPoses, NavigateToPose
+from rcl_interfaces.msg import Parameter as ParameterMsg
+from rcl_interfaces.msg import ParameterType, ParameterValue
+from rcl_interfaces.srv import SetParameters
+from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from std_msgs.msg import String
+
+
+class NavigateToPoseRefinerProxy(Node):
+    """Forward pose-navigation actions and complete them after final yaw refinement."""
+
+    TERMINAL_REFINER_STATUSES = {
+        'succeeded',
+        'timeout',
+        'failed_tf',
+        'no_goal',
+        'handoff_distance_exceeded',
+        'canceled',
+    }
+    STARTLESS_REFINER_FAILURES = {
+        'failed_tf',
+        'no_goal',
+        'handoff_distance_exceeded',
+    }
+
+    def __init__(self):
+        super().__init__('navigate_to_pose_refiner_proxy')
+        self.public_goal_topic = '/goal_pose'
+        self.refiner_goal_topic = '/final_pose_refiner/goal_pose'
+        self.refiner_status_topic = '/final_pose_refiner/status'
+        self.refiner_param_service = '/final_pose_refiner/set_parameters'
+        self.start_param = 'final_pose_refiner_start'
+        self.cancel_param = 'final_pose_refiner_cancel'
+
+        self.declare_parameter('nav2_server_timeout_sec', 5.0)
+        self.declare_parameter('refiner_service_timeout_sec', 2.0)
+        self.declare_parameter('refiner_wait_timeout_sec', 25.0)
+        self.declare_parameter('require_refinement', True)
+        self.declare_parameter('debug_print', False)
+
+        self.callback_group = ReentrantCallbackGroup()
+        self.goal_pub = self.create_publisher(PoseStamped, self.refiner_goal_topic, 10)
+        self.refiner_status_sub = self.create_subscription(
+            String,
+            self.refiner_status_topic,
+            self._on_refiner_status,
+            10,
+            callback_group=self.callback_group,
+        )
+        self.refiner_param_client = self.create_client(
+            SetParameters,
+            self.refiner_param_service,
+            callback_group=self.callback_group,
+        )
+
+        self._refinement_lock = threading.Lock()
+        self._status_condition = threading.Condition()
+        self._status_sequence = 0
+        self._status_history = []
+        self.routes = []
+        self._add_route(
+            'NavigateToPose',
+            NavigateToPose,
+            '/navigate_to_pose',
+            '/navigate_to_pose_nav2',
+            lambda request: request.pose,
+        )
+        self._add_route(
+            'NavigateThroughPoses',
+            NavigateThroughPoses,
+            '/navigate_through_poses',
+            '/navigate_through_poses_nav2',
+            lambda request: request.poses[-1] if request.poses else None,
+        )
+        self.topic_nav_client = ActionClient(
+            self,
+            NavigateToPose,
+            '/navigate_to_pose',
+            callback_group=self.callback_group,
+        )
+        self.goal_topic_sub = self.create_subscription(
+            PoseStamped,
+            self.public_goal_topic,
+            self._on_goal_pose,
+            10,
+            callback_group=self.callback_group,
+        )
+
+        self.get_logger().info(
+            'Navigation refinement proxy ready for NavigateToPose, NavigateThroughPoses, '
+            'and /goal_pose; public tasks complete after final yaw refinement.'
+        )
+
+    def _add_route(self, label, action_type, public_name, nav2_name, final_pose_getter):
+        route = {
+            'label': label,
+            'action_type': action_type,
+            'public_name': public_name,
+            'nav2_name': nav2_name,
+            'final_pose_getter': final_pose_getter,
+        }
+        route['client'] = ActionClient(
+            self,
+            action_type,
+            nav2_name,
+            callback_group=self.callback_group,
+        )
+        route['server'] = ActionServer(
+            self,
+            action_type,
+            public_name,
+            execute_callback=lambda handle, current=route: self._execute_callback(current, handle),
+            goal_callback=lambda request, current=route: self._goal_callback(current, request),
+            cancel_callback=self._cancel_callback,
+            callback_group=self.callback_group,
+        )
+        self.routes.append(route)
+        self._debug(f'{label} route: {public_name} -> {nav2_name}')
+
+    def _goal_callback(self, route, goal_request):
+        if not route['client'].server_is_ready():
+            self.get_logger().warn(
+                f"{route['label']} Nav2 action server {route['nav2_name']} is not ready; "
+                'rejecting goal.'
+            )
+            return GoalResponse.REJECT
+
+        final_pose = route['final_pose_getter'](goal_request)
+        if final_pose is not None:
+            self._publish_refiner_goal(final_pose)
+        return GoalResponse.ACCEPT
+
+    @staticmethod
+    def _cancel_callback(_goal_handle):
+        return CancelResponse.ACCEPT
+
+    def _on_goal_pose(self, pose):
+        goal = NavigateToPose.Goal()
+        goal.pose = deepcopy(pose)
+        if not self.topic_nav_client.server_is_ready():
+            self.get_logger().error(
+                'Cannot forward /goal_pose: public NavigateToPose is unavailable.'
+            )
+            return
+
+        send_future = self.topic_nav_client.send_goal_async(goal)
+        send_future.add_done_callback(self._on_topic_goal_response)
+
+    def _on_topic_goal_response(self, future):
+        try:
+            goal_handle = future.result()
+        except Exception as exc:
+            self.get_logger().error(f'Failed to forward /goal_pose to NavigateToPose: {exc}')
+            return
+
+        if goal_handle is None or not goal_handle.accepted:
+            self.get_logger().error('/goal_pose navigation goal was rejected.')
+            return
+
+        result_future = goal_handle.get_result_async()
+        result_future.add_done_callback(self._on_topic_goal_result)
+
+    def _on_topic_goal_result(self, future):
+        try:
+            action_result = future.result()
+        except Exception as exc:
+            self.get_logger().error(f'Failed to receive /goal_pose navigation result: {exc}')
+            return
+
+        if action_result.status != GoalStatus.STATUS_SUCCEEDED:
+            self.get_logger().warn(
+                f'/goal_pose navigation ended with action status {action_result.status}.'
+            )
+
+    def _execute_callback(self, route, goal_handle):
+        nav_result = self._forward_to_nav2(route, goal_handle)
+        if nav_result is None:
+            return route['action_type'].Result()
+
+        result, status = nav_result
+        if status == GoalStatus.STATUS_CANCELED:
+            goal_handle.canceled()
+            return result
+        if status != GoalStatus.STATUS_SUCCEEDED:
+            goal_handle.abort()
+            return result
+
+        final_pose = route['final_pose_getter'](goal_handle.request)
+        refine_status = 'succeeded'
+        if final_pose is not None:
+            refine_status = self._refine_final_pose(goal_handle, final_pose)
+
+        if refine_status == 'succeeded':
+            goal_handle.succeed()
+        elif refine_status == 'canceled':
+            goal_handle.canceled()
+        else:
+            self.get_logger().error(
+                f"{route['label']} completed in Nav2 but final refinement ended with "
+                f"status '{refine_status}'."
+            )
+            goal_handle.abort()
+        return result
+
+    def _forward_to_nav2(self, route, goal_handle):
+        timeout = float(self.get_parameter('nav2_server_timeout_sec').value)
+        if not route['client'].wait_for_server(timeout_sec=timeout):
+            self.get_logger().error(f"Nav2 action server {route['nav2_name']} is not available.")
+            goal_handle.abort()
+            return None
+
+        send_future = route['client'].send_goal_async(
+            deepcopy(goal_handle.request),
+            feedback_callback=lambda message: self._relay_feedback(goal_handle, message),
+        )
+        if not self._wait_for_future(send_future, timeout):
+            self.get_logger().error(f"Timed out forwarding {route['label']} goal to Nav2.")
+            goal_handle.abort()
+            return None
+
+        try:
+            nav_goal_handle = send_future.result()
+        except Exception as exc:
+            self.get_logger().error(f"Failed to forward {route['label']} goal to Nav2: {exc}")
+            goal_handle.abort()
+            return None
+
+        if nav_goal_handle is None or not nav_goal_handle.accepted:
+            self.get_logger().error(f"Forwarded {route['label']} goal was rejected by Nav2.")
+            goal_handle.abort()
+            return None
+
+        result_future = nav_goal_handle.get_result_async()
+        while rclpy.ok() and not result_future.done():
+            if goal_handle.is_cancel_requested:
+                self._cancel_nav_goal(nav_goal_handle)
+                goal_handle.canceled()
+                return None
+            time.sleep(0.05)
+
+        if not result_future.done():
+            goal_handle.abort()
+            return None
+
+        try:
+            nav_result = result_future.result()
+        except Exception as exc:
+            self.get_logger().error(f"Failed to get Nav2 {route['label']} result: {exc}")
+            goal_handle.abort()
+            return None
+
+        result = (
+            nav_result.result
+            if nav_result and nav_result.result
+            else route['action_type'].Result()
+        )
+        return result, nav_result.status
+
+    def _refine_final_pose(self, goal_handle, pose):
+        if not self.get_parameter('require_refinement').value:
+            return 'succeeded'
+
+        with self._refinement_lock:
+            if goal_handle.is_cancel_requested:
+                return 'canceled'
+
+            self._publish_refiner_goal(pose)
+            start_sequence = self._status_snapshot()
+            if not self._set_refiner_parameter(self.start_param, True):
+                return 'unavailable'
+
+            wait_timeout = float(self.get_parameter('refiner_wait_timeout_sec').value)
+            deadline = time.monotonic() + max(wait_timeout, 0.0)
+            saw_running = False
+            sequence = start_sequence
+            while rclpy.ok():
+                if goal_handle.is_cancel_requested:
+                    self._set_refiner_parameter(self.cancel_param, True)
+                    return 'canceled'
+
+                updates = self._wait_for_status_updates(sequence, deadline)
+                if updates is None:
+                    self.get_logger().error('Timed out waiting for final pose refinement result.')
+                    self._set_refiner_parameter(self.cancel_param, True)
+                    return 'timeout'
+
+                for sequence, status in updates:
+                    if status == 'running':
+                        saw_running = True
+                    elif status in self.TERMINAL_REFINER_STATUSES:
+                        if saw_running or status in self.STARTLESS_REFINER_FAILURES:
+                            return status
+            return 'canceled'
+
+    def _set_refiner_parameter(self, name, value):
+        timeout = float(self.get_parameter('refiner_service_timeout_sec').value)
+        if not self.refiner_param_client.wait_for_service(timeout_sec=timeout):
+            self.get_logger().error(
+                f'Final pose refiner parameter service {self.refiner_param_service} '
+                'is unavailable.'
+            )
+            return False
+
+        parameter = ParameterMsg()
+        parameter.name = name
+        parameter.value = ParameterValue(type=ParameterType.PARAMETER_BOOL, bool_value=value)
+        request = SetParameters.Request()
+        request.parameters = [parameter]
+        future = self.refiner_param_client.call_async(request)
+        if not self._wait_for_future(future, timeout):
+            self.get_logger().error(f'Timed out setting final pose refiner parameter {name}.')
+            return False
+
+        response = future.result()
+        if response is None or not response.results or not response.results[0].successful:
+            reason = response.results[0].reason if response and response.results else ''
+            self.get_logger().error(f'Failed to set final pose refiner parameter {name}: {reason}')
+            return False
+        return True
+
+    def _on_refiner_status(self, message):
+        with self._status_condition:
+            self._status_sequence += 1
+            self._status_history.append((self._status_sequence, message.data))
+            self._status_history = self._status_history[-32:]
+            self._status_condition.notify_all()
+
+    def _status_snapshot(self):
+        with self._status_condition:
+            return self._status_sequence
+
+    def _wait_for_status_updates(self, sequence, deadline):
+        with self._status_condition:
+            while rclpy.ok():
+                updates = [item for item in self._status_history if item[0] > sequence]
+                if updates:
+                    return updates
+                remaining = deadline - time.monotonic()
+                if remaining <= 0.0:
+                    return None
+                self._status_condition.wait(timeout=min(remaining, 0.1))
+        return None
+
+    def _publish_refiner_goal(self, pose):
+        refiner_goal = deepcopy(pose)
+        refiner_goal.header.stamp = self.get_clock().now().to_msg()
+        self.goal_pub.publish(refiner_goal)
+
+    @staticmethod
+    def _relay_feedback(goal_handle, feedback_message):
+        if goal_handle.is_active:
+            goal_handle.publish_feedback(feedback_message.feedback)
+
+    def _debug(self, message):
+        if self.get_parameter('debug_print').value:
+            self.get_logger().info(message)
+
+    @staticmethod
+    def _wait_for_future(future, timeout_sec):
+        done = threading.Event()
+        future.add_done_callback(lambda _: done.set())
+        return done.wait(timeout_sec)
+
+    def _cancel_nav_goal(self, nav_goal_handle):
+        cancel_future = nav_goal_handle.cancel_goal_async()
+        self._wait_for_future(cancel_future, 2.0)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = NavigateToPoseRefinerProxy()
+    executor = MultiThreadedExecutor(num_threads=6)
+    try:
+        rclpy.spin(node, executor=executor)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_calibration/agv_pro_calibration/odom_linear_calib.py

@@ -0,0 +1,469 @@
+#!/usr/bin/env python3
+"""X-axis odometry scale calibration helper for AGV Pro."""
+
+import math
+import os
+import shlex
+import statistics
+import sys
+import threading
+
+import rclpy
+from geometry_msgs.msg import Twist
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from tf2_ros import Buffer, TransformException, TransformListener
+
+CONTROL_RATE_HZ = 20.0
+MAX_SPEED_LIMIT = 0.30
+TF_TIMEOUT_SEC = 0.5
+STOP_REPEAT_COUNT = 5
+
+
+class OdomLinearCalib(Node):
+    """Run repeated X-axis odom tests and compute the final scale from cached samples."""
+
+    def __init__(self):
+        super().__init__('odom_linear_calib')
+
+        self.declare_parameter('cmd_vel_topic', '/cmd_vel')
+        self.declare_parameter('odom_frame', 'odom')
+        self.declare_parameter('base_frame', 'base_footprint')
+        self.declare_parameter('start_test', False)
+        self.declare_parameter('test_distance', 1.0)
+        self.declare_parameter('speed', 0.10)
+        self.declare_parameter('tolerance', 0.01)
+        self.declare_parameter('odom_linear_scale_correction', 1.0)
+        self.declare_parameter('timeout', 30.0)
+
+        self.cmd_vel_topic = self.get_parameter('cmd_vel_topic').value
+        self.cmd_vel_pub = self.create_publisher(Twist, self.cmd_vel_topic, 10)
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.state = 'idle'
+        self.start_pose = None
+        self.direction_sign = 1.0
+        self.signed_target_distance = 1.0
+        self.target_distance = 1.0
+        self.command_speed = 0.10
+        self.tolerance = 0.01
+        self.odom_linear_scale_correction = 1.0
+        self.timeout = 30.0
+        self.start_time = None
+        self.last_odom_distance = 0.0
+        self.last_log_time = self.get_clock().now()
+
+        self.samples = []
+        self.samples_lock = threading.Lock()
+        self.pending_sample = None
+
+        self.timer = self.create_timer(1.0 / CONTROL_RATE_HZ, self.on_timer)
+        threading.Thread(target=self._stdin_loop, daemon=True).start()
+
+        self.get_logger().info(
+            'odom_linear_calib ready. Set params, set start_test:=true for each run, '
+            'use positive test_distance for forward and negative for backward, '
+            'then enter the measured ground error in cm after the robot stops. '
+            'Enter 0 to finish and print the cached scale summary; enter any text to skip a verification run.'
+        )
+
+    def _stdin_loop(self):
+        while True:
+            line = sys.stdin.readline()
+            if line == '':
+                return
+            self._handle_input_line(line.strip())
+
+    def _handle_input_line(self, text):
+        if not text:
+            self.get_logger().info(
+                'Input ignored. After a successful run enter ground error in cm '
+                '(+over target along motion direction, -short), or enter 0 to finish.'
+            )
+            return
+
+        try:
+            value = float(text)
+        except ValueError:
+            self._skip_pending_sample(text)
+            return
+
+        if value == 0.0 and not text.startswith(('+', '-')):
+            with self.samples_lock:
+                had_pending_sample = self.pending_sample is not None
+                self.pending_sample = None
+                if self.state == 'awaiting_input':
+                    self.state = 'idle'
+            if had_pending_sample:
+                self.get_logger().warn('Pending run was not recorded because finish input 0 was entered.')
+            self._print_summary()
+            return
+
+        self._record_pending_sample(value)
+
+    def on_timer(self):
+        if self.state == 'running':
+            self._run_test_step()
+            return
+
+        if self.state == 'awaiting_input':
+            if self.get_parameter('start_test').value:
+                self.get_logger().warn(
+                    'A finished run is waiting for ground-error input; record it before starting again.'
+                )
+                self._reset_start_test()
+            return
+
+        if self.get_parameter('start_test').value:
+            self._start_test()
+
+    def _start_test(self):
+        config = self._read_test_config()
+        if config is None:
+            self._reset_start_test()
+            return
+
+        pose = self._lookup_pose()
+        if pose is None:
+            self.get_logger().warn('Cannot start test: odom transform is not available.')
+            self._reset_start_test()
+            return
+
+        self.signed_target_distance = config['signed_test_distance']
+        self.target_distance = config['target_distance']
+        self.command_speed = config['speed']
+        self.tolerance = config['tolerance']
+        self.odom_linear_scale_correction = config['odom_linear_scale_correction']
+        self.timeout = config['timeout']
+        self.direction_sign = float(config['direction_sign'])
+        self.start_pose = pose
+        self.start_time = self.get_clock().now()
+        self.last_odom_distance = 0.0
+        self.state = 'running'
+
+        self.get_logger().info(
+            f'Start X odom calibration: direction={int(self.direction_sign)}, '
+            f'signed_target={self.signed_target_distance:.3f} m, '
+            f'target={self.target_distance:.3f} m, speed={self.command_speed:.3f} m/s, '
+            f'odom_linear_scale_correction={self.odom_linear_scale_correction:.6f}'
+        )
+
+    def _run_test_step(self):
+        pose = self._lookup_pose()
+        if pose is None:
+            self._finish_test('failed_tf', publish_warning=True)
+            return
+
+        raw_progress, lateral_drift = self._calculate_progress(pose)
+        corrected_progress = raw_progress * self.odom_linear_scale_correction
+        error = corrected_progress - self.target_distance
+        elapsed = (self.get_clock().now() - self.start_time).nanoseconds / 1e9
+        self.last_odom_distance = raw_progress
+
+        if corrected_progress >= self.target_distance - self.tolerance:
+            self._finish_test('succeeded', raw_progress, corrected_progress, lateral_drift, elapsed)
+            return
+
+        if elapsed > self.timeout:
+            self._finish_test('timeout', raw_progress, corrected_progress, lateral_drift, elapsed)
+            return
+
+        cmd = Twist()
+        cmd.linear.x = self.direction_sign * self.command_speed
+        self.cmd_vel_pub.publish(cmd)
+        self._log_progress(raw_progress, corrected_progress, error, lateral_drift, elapsed)
+
+    def _finish_test(
+        self,
+        status,
+        odom_distance=None,
+        corrected_distance=None,
+        lateral_drift=None,
+        elapsed=None,
+        publish_warning=False,
+    ):
+        self._stop_robot()
+        self._reset_start_test()
+
+        if odom_distance is None:
+            odom_distance = self.last_odom_distance
+        if corrected_distance is None:
+            corrected_distance = odom_distance * self.odom_linear_scale_correction
+        if lateral_drift is None:
+            lateral_drift = 0.0
+        if elapsed is None and self.start_time is not None:
+            elapsed = (self.get_clock().now() - self.start_time).nanoseconds / 1e9
+        if elapsed is None:
+            elapsed = 0.0
+
+        if status == 'succeeded' and odom_distance > 0.0:
+            pending_sample = {
+                'direction': int(self.direction_sign),
+                'signed_target_distance': self.signed_target_distance,
+                'target_distance': self.target_distance,
+                'odom_distance': odom_distance,
+                'corrected_distance': corrected_distance,
+                'lateral_drift': lateral_drift,
+                'elapsed': elapsed,
+                'used_correction': self.odom_linear_scale_correction,
+            }
+            with self.samples_lock:
+                self.pending_sample = pending_sample
+                self.state = 'awaiting_input'
+            self.get_logger().info(
+                'Run is waiting for measured ground error. '
+                'Enter cm error now: +over target along motion direction, -short of target, '
+                '+0/-0 for exact target, 0 to finish, or any text to skip this run.'
+            )
+        else:
+            self.state = 'idle'
+
+        msg = (
+            f'Calibration {status}: odom_distance={odom_distance:.4f} m, '
+            f'corrected_distance={corrected_distance:.4f} m, '
+            f'lateral_drift={lateral_drift:.4f} m, elapsed={elapsed:.2f} s, '
+            f'target={self.target_distance:.4f} m, '
+            f'used_correction={self.odom_linear_scale_correction:.6f}.'
+        )
+        if publish_warning:
+            self.get_logger().warn(msg)
+        else:
+            self.get_logger().info(msg)
+
+    def _read_test_config(self):
+        test_distance = self.get_parameter('test_distance').value
+        speed = abs(self.get_parameter('speed').value)
+        tolerance = max(self.get_parameter('tolerance').value, 0.0)
+        correction = self.get_parameter('odom_linear_scale_correction').value
+        timeout = self.get_parameter('timeout').value
+
+        if test_distance == 0.0:
+            self.get_logger().error(
+                'test_distance must not be 0.0 m. Use a positive value for forward, negative for backward.'
+            )
+            return None
+        if speed <= 0.0:
+            self.get_logger().error('speed must be greater than 0.0 m/s.')
+            return None
+        if timeout <= 0.0:
+            self.get_logger().error('timeout must be greater than 0.0 s.')
+            return None
+        if correction <= 0.0:
+            self.get_logger().error('odom_linear_scale_correction must be greater than 0.0.')
+            return None
+        if speed > MAX_SPEED_LIMIT:
+            self.get_logger().warn(
+                f'speed {speed:.3f} m/s exceeds internal safety limit '
+                f'{MAX_SPEED_LIMIT:.3f} m/s; clipping command speed.'
+            )
+            speed = MAX_SPEED_LIMIT
+
+        direction_sign = 1 if test_distance > 0.0 else -1
+        return {
+            'direction_sign': direction_sign,
+            'signed_test_distance': test_distance,
+            'target_distance': abs(test_distance),
+            'speed': speed,
+            'tolerance': tolerance,
+            'odom_linear_scale_correction': correction,
+            'timeout': timeout,
+        }
+
+    def _lookup_pose(self):
+        odom_frame = self.get_parameter('odom_frame').value
+        base_frame = self.get_parameter('base_frame').value
+        try:
+            trans = self.tf_buffer.lookup_transform(
+                odom_frame,
+                base_frame,
+                rclpy.time.Time(),
+                timeout=Duration(seconds=TF_TIMEOUT_SEC),
+            )
+        except TransformException as exc:
+            self.get_logger().warn(f'TF lookup failed: {exc}')
+            return None
+
+        translation = trans.transform.translation
+        rotation = trans.transform.rotation
+        return (
+            translation.x,
+            translation.y,
+            self._yaw_from_quaternion(rotation.x, rotation.y, rotation.z, rotation.w),
+        )
+
+    def _calculate_progress(self, pose):
+        x, y, _ = pose
+        start_x, start_y, start_yaw = self.start_pose
+        dx = x - start_x
+        dy = y - start_y
+        cos_yaw = math.cos(start_yaw)
+        sin_yaw = math.sin(start_yaw)
+
+        forward_delta = dx * cos_yaw + dy * sin_yaw
+        lateral_drift = -dx * sin_yaw + dy * cos_yaw
+        progress = self.direction_sign * forward_delta
+        return progress, lateral_drift
+
+    def _log_progress(self, raw_progress, corrected_progress, error, lateral_drift, elapsed):
+        now = self.get_clock().now()
+        if (now - self.last_log_time).nanoseconds < 1e9:
+            return
+        self.get_logger().info(
+            f'odom_distance={raw_progress:.3f} m, '
+            f'corrected_distance={corrected_progress:.3f} m, '
+            f'error={error:+.3f} m, lateral_drift={lateral_drift:.3f} m, '
+            f'elapsed={elapsed:.1f} s'
+        )
+        self.last_log_time = now
+
+    def _skip_pending_sample(self, reason):
+        with self.samples_lock:
+            if self.pending_sample is None:
+                self.get_logger().warn(
+                    f'Input "{reason}" ignored. No pending successful run is waiting for input.'
+                )
+                return
+
+            skipped_sample = self.pending_sample
+            self.pending_sample = None
+            self.state = 'idle'
+
+        self.get_logger().info(
+            f'Skipped pending run by input "{reason}": '
+            f'direction={skipped_sample["direction"]:+d}, '
+            f'signed_target={skipped_sample["signed_target_distance"]:.4f} m, '
+            f'odom={skipped_sample["odom_distance"]:.4f} m, '
+            f'corrected={skipped_sample["corrected_distance"]:.4f} m, '
+            f'used_correction={skipped_sample["used_correction"]:.6f}. '
+            'This run will not be used in the final scale summary.'
+        )
+
+    def _record_pending_sample(self, ground_error_cm):
+        with self.samples_lock:
+            if self.pending_sample is None:
+                self.get_logger().warn(
+                    'No pending successful run. Set start_test:=true first, wait for the robot to stop, '
+                    'then enter the measured cm error.'
+                )
+                return
+
+            actual_distance = self.pending_sample['target_distance'] + ground_error_cm / 100.0
+            if actual_distance <= 0.0:
+                self.get_logger().error(
+                    f'Invalid measured result: target + error = {actual_distance:.4f} m. '
+                    'Re-enter the cm error for this pending run.'
+                )
+                return
+
+            sample = dict(self.pending_sample)
+            sample['ground_error_cm'] = ground_error_cm
+            sample['actual_distance'] = actual_distance
+            sample['scale'] = actual_distance / sample['odom_distance']
+            self.samples.append(sample)
+            sample_index = len(self.samples)
+            direction_index = sum(
+                1 for recorded_sample in self.samples
+                if recorded_sample['direction'] == sample['direction']
+            )
+            self.pending_sample = None
+            self.state = 'idle'
+
+        self.get_logger().info(
+            f'Recorded sample #{sample_index} overall, direction {sample["direction"]:+d} #{direction_index}: '
+            f'actual={actual_distance:.4f} m, '
+            f'ground_error={ground_error_cm:+.2f} cm, odom={sample["odom_distance"]:.4f} m, '
+            f'scale={sample["scale"]:.6f}. Set start_test:=true for the next run, or enter 0 to finish.'
+        )
+
+    def _print_summary(self):
+        with self.samples_lock:
+            samples = list(self.samples)
+
+        if not samples:
+            self.get_logger().warn('No successful calibration samples have been recorded yet.')
+            return
+
+        self.get_logger().info('========== X ODOM SCALE SUMMARY ==========')
+        for index, sample in enumerate(samples, start=1):
+            self.get_logger().info(
+                f'#{index:02d} direction={sample["direction"]:+d}, '
+                f'signed_target={sample["signed_target_distance"]:.4f} m, '
+                f'target={sample["target_distance"]:.4f} m, '
+                f'actual={sample["actual_distance"]:.4f} m, '
+                f'ground_error={sample["ground_error_cm"]:+.2f} cm, '
+                f'odom={sample["odom_distance"]:.4f} m, '
+                f'corrected={sample["corrected_distance"]:.4f} m, '
+                f'lateral_drift={sample["lateral_drift"]:.4f} m, '
+                f'used_correction={sample["used_correction"]:.6f}, '
+                f'scale={sample["scale"]:.6f}'
+            )
+
+        self._print_scale_stats('all', samples)
+        for direction in (1, -1):
+            direction_samples = [sample for sample in samples if sample['direction'] == direction]
+            if direction_samples:
+                self._print_scale_stats(f'direction={direction:+d}', direction_samples)
+        self.get_logger().info('Restart this node to clear cached samples.')
+
+    def _print_scale_stats(self, label, samples):
+        scales = [sample['scale'] for sample in samples]
+        mean_scale = statistics.fmean(scales)
+        std_scale = statistics.pstdev(scales) if len(scales) > 1 else 0.0
+        self.get_logger().info(
+            f'{label}: samples={len(scales)}, recommended_odometry.scale_x={mean_scale:.6f}, '
+            f'std={std_scale:.6f}, min={min(scales):.6f}, max={max(scales):.6f}'
+        )
+
+    def _publish_stop(self):
+        try:
+            self.cmd_vel_pub.publish(Twist())
+        except Exception:
+            pass
+
+    def _stop_robot(self):
+        for _ in range(STOP_REPEAT_COUNT):
+            self._publish_stop()
+
+    def _stop_robot_with_ros_cli(self):
+        topic = shlex.quote(self.cmd_vel_topic)
+        zero_twist = (
+            '"{linear: {x: 0.0, y: 0.0, z: 0.0}, '
+            'angular: {x: 0.0, y: 0.0, z: 0.0}}"'
+        )
+        os.system(
+            f'timeout 2s ros2 topic pub --once {topic} '
+            f'geometry_msgs/msg/Twist {zero_twist} >/dev/null 2>&1'
+        )
+
+    def _reset_start_test(self):
+        self.set_parameters([
+            Parameter('start_test', Parameter.Type.BOOL, False),
+        ])
+
+    @staticmethod
+    def _yaw_from_quaternion(x, y, z, w):
+        siny_cosp = 2.0 * (w * z + x * y)
+        cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
+        return math.atan2(siny_cosp, cosy_cosp)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OdomLinearCalib()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node._stop_robot()
+        node._stop_robot_with_ros_cli()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_calibration/agv_pro_calibration/odom_yaw_calib.py

@@ -0,0 +1,466 @@
+#!/usr/bin/env python3
+"""Yaw odometry scale calibration helper for AGV Pro."""
+
+import math
+import os
+import shlex
+import statistics
+import sys
+import threading
+
+import rclpy
+from geometry_msgs.msg import Twist
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from tf2_ros import Buffer, TransformException, TransformListener
+
+CONTROL_RATE_HZ = 20.0
+MAX_ANGULAR_SPEED_LIMIT = 0.50
+TF_TIMEOUT_SEC = 0.5
+STOP_REPEAT_COUNT = 5
+
+
+class OdomYawCalib(Node):
+    """Run repeated yaw odom tests and compute the final scale from cached samples."""
+
+    def __init__(self):
+        super().__init__('odom_yaw_calib')
+
+        self.declare_parameter('cmd_vel_topic', '/cmd_vel')
+        self.declare_parameter('odom_frame', 'odom')
+        self.declare_parameter('base_frame', 'base_footprint')
+        self.declare_parameter('start_test', False)
+        self.declare_parameter('test_angle', 360.0)
+        self.declare_parameter('speed', 0.20)
+        self.declare_parameter('tolerance', 2.0)
+        self.declare_parameter('odom_yaw_scale_correction', 1.0)
+        self.declare_parameter('timeout', 60.0)
+
+        self.cmd_vel_topic = self.get_parameter('cmd_vel_topic').value
+        self.cmd_vel_pub = self.create_publisher(Twist, self.cmd_vel_topic, 10)
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.state = 'idle'
+        self.direction_sign = 1.0
+        self.signed_target_angle_deg = 360.0
+        self.target_angle_deg = 360.0
+        self.target_angle = math.radians(360.0)
+        self.command_speed = 0.20
+        self.tolerance_deg = 2.0
+        self.tolerance = math.radians(2.0)
+        self.odom_yaw_scale_correction = 1.0
+        self.timeout = 60.0
+        self.start_time = None
+        self.prev_yaw = None
+        self.accumulated_yaw = 0.0
+        self.last_odom_angle = 0.0
+        self.last_log_time = self.get_clock().now()
+
+        self.samples = []
+        self.samples_lock = threading.Lock()
+        self.pending_sample = None
+
+        self.timer = self.create_timer(1.0 / CONTROL_RATE_HZ, self.on_timer)
+        threading.Thread(target=self._stdin_loop, daemon=True).start()
+
+        self.get_logger().info(
+            'odom_yaw_calib ready. Set params, set start_test:=true for each run, '
+            'use positive test_angle for positive angular.z and negative for negative angular.z, '
+            'then enter the measured ground yaw error in deg after the robot stops. '
+            'Enter 0 to finish and print the cached scale summary; enter any text to skip a verification run.'
+        )
+
+    def _stdin_loop(self):
+        while True:
+            line = sys.stdin.readline()
+            if line == '':
+                return
+            self._handle_input_line(line.strip())
+
+    def _handle_input_line(self, text):
+        if not text:
+            self.get_logger().info(
+                'Input ignored. After a successful run enter ground yaw error in deg '
+                '(+over target along rotation direction, -short), or enter 0 to finish.'
+            )
+            return
+
+        try:
+            value = float(text)
+        except ValueError:
+            self._skip_pending_sample(text)
+            return
+
+        if value == 0.0 and not text.startswith(('+', '-')):
+            with self.samples_lock:
+                had_pending_sample = self.pending_sample is not None
+                self.pending_sample = None
+                if self.state == 'awaiting_input':
+                    self.state = 'idle'
+            if had_pending_sample:
+                self.get_logger().warn('Pending run was not recorded because finish input 0 was entered.')
+            self._print_summary()
+            return
+
+        self._record_pending_sample(value)
+
+    def on_timer(self):
+        if self.state == 'running':
+            self._run_test_step()
+            return
+
+        if self.state == 'awaiting_input':
+            if self.get_parameter('start_test').value:
+                self.get_logger().warn(
+                    'A finished run is waiting for ground-yaw-error input; record it before starting again.'
+                )
+                self._reset_start_test()
+            return
+
+        if self.get_parameter('start_test').value:
+            self._start_test()
+
+    def _start_test(self):
+        config = self._read_test_config()
+        if config is None:
+            self._reset_start_test()
+            return
+
+        pose = self._lookup_pose()
+        if pose is None:
+            self.get_logger().warn('Cannot start test: odom transform is not available.')
+            self._reset_start_test()
+            return
+
+        self.signed_target_angle_deg = config['signed_test_angle']
+        self.target_angle_deg = config['target_angle']
+        self.target_angle = math.radians(config['target_angle'])
+        self.command_speed = config['speed']
+        self.tolerance_deg = config['tolerance']
+        self.tolerance = math.radians(config['tolerance'])
+        self.odom_yaw_scale_correction = config['odom_yaw_scale_correction']
+        self.timeout = config['timeout']
+        self.direction_sign = float(config['direction_sign'])
+        self.prev_yaw = pose[2]
+        self.accumulated_yaw = 0.0
+        self.start_time = self.get_clock().now()
+        self.last_odom_angle = 0.0
+        self.state = 'running'
+
+        self.get_logger().info(
+            f'Start yaw odom calibration: direction={int(self.direction_sign)}, '
+            f'signed_target={self.signed_target_angle_deg:.1f} deg, '
+            f'target={self.target_angle_deg:.1f} deg, speed={self.command_speed:.3f} rad/s, '
+            f'odom_yaw_scale_correction={self.odom_yaw_scale_correction:.6f}'
+        )
+
+    def _run_test_step(self):
+        pose = self._lookup_pose()
+        if pose is None:
+            self._finish_test('failed_tf', publish_warning=True)
+            return
+
+        raw_progress = self._calculate_yaw_progress(pose)
+        raw_angle = max(raw_progress, 0.0)
+        corrected_angle = raw_angle * self.odom_yaw_scale_correction
+        error = corrected_angle - self.target_angle
+        elapsed = (self.get_clock().now() - self.start_time).nanoseconds / 1e9
+        self.last_odom_angle = raw_angle
+
+        if corrected_angle >= self.target_angle - self.tolerance:
+            self._finish_test('succeeded', raw_angle, corrected_angle, elapsed)
+            return
+
+        if elapsed > self.timeout:
+            self._finish_test('timeout', raw_angle, corrected_angle, elapsed)
+            return
+
+        cmd = Twist()
+        cmd.angular.z = self.direction_sign * self.command_speed
+        self.cmd_vel_pub.publish(cmd)
+        self._log_progress(raw_angle, corrected_angle, error, elapsed)
+
+    def _finish_test(
+        self,
+        status,
+        odom_angle=None,
+        corrected_angle=None,
+        elapsed=None,
+        publish_warning=False,
+    ):
+        self._stop_robot()
+        self._reset_start_test()
+
+        if odom_angle is None:
+            odom_angle = self.last_odom_angle
+        if corrected_angle is None:
+            corrected_angle = odom_angle * self.odom_yaw_scale_correction
+        if elapsed is None and self.start_time is not None:
+            elapsed = (self.get_clock().now() - self.start_time).nanoseconds / 1e9
+        if elapsed is None:
+            elapsed = 0.0
+
+        if status == 'succeeded' and odom_angle > 0.0:
+            pending_sample = {
+                'direction': int(self.direction_sign),
+                'signed_target_angle_deg': self.signed_target_angle_deg,
+                'target_angle_deg': self.target_angle_deg,
+                'odom_angle_deg': math.degrees(odom_angle),
+                'corrected_angle_deg': math.degrees(corrected_angle),
+                'elapsed': elapsed,
+                'used_correction': self.odom_yaw_scale_correction,
+            }
+            with self.samples_lock:
+                self.pending_sample = pending_sample
+                self.state = 'awaiting_input'
+            self.get_logger().info(
+                'Run is waiting for measured ground yaw error. '
+                'Enter deg error now: +over target along rotation direction, -short of target, '
+                '+0/-0 for exact target, 0 to finish, or any text to skip this run.'
+            )
+        else:
+            self.state = 'idle'
+
+        msg = (
+            f'Calibration {status}: odom_angle={math.degrees(odom_angle):.2f} deg, '
+            f'corrected_angle={math.degrees(corrected_angle):.2f} deg, '
+            f'elapsed={elapsed:.2f} s, target={self.target_angle_deg:.2f} deg, '
+            f'used_correction={self.odom_yaw_scale_correction:.6f}.'
+        )
+        if publish_warning:
+            self.get_logger().warn(msg)
+        else:
+            self.get_logger().info(msg)
+
+    def _read_test_config(self):
+        test_angle = self.get_parameter('test_angle').value
+        speed = abs(self.get_parameter('speed').value)
+        tolerance = max(self.get_parameter('tolerance').value, 0.0)
+        correction = self.get_parameter('odom_yaw_scale_correction').value
+        timeout = self.get_parameter('timeout').value
+
+        if test_angle == 0.0:
+            self.get_logger().error(
+                'test_angle must not be 0.0 deg. Use a positive value for one yaw direction, '
+                'negative for the opposite direction.'
+            )
+            return None
+        if speed <= 0.0:
+            self.get_logger().error('speed must be greater than 0.0 rad/s.')
+            return None
+        if timeout <= 0.0:
+            self.get_logger().error('timeout must be greater than 0.0 s.')
+            return None
+        if correction <= 0.0:
+            self.get_logger().error('odom_yaw_scale_correction must be greater than 0.0.')
+            return None
+        if speed > MAX_ANGULAR_SPEED_LIMIT:
+            self.get_logger().warn(
+                f'speed {speed:.3f} rad/s exceeds internal safety limit '
+                f'{MAX_ANGULAR_SPEED_LIMIT:.3f} rad/s; clipping command speed.'
+            )
+            speed = MAX_ANGULAR_SPEED_LIMIT
+
+        direction_sign = 1 if test_angle > 0.0 else -1
+        return {
+            'direction_sign': direction_sign,
+            'signed_test_angle': test_angle,
+            'target_angle': abs(test_angle),
+            'speed': speed,
+            'tolerance': tolerance,
+            'odom_yaw_scale_correction': correction,
+            'timeout': timeout,
+        }
+
+    def _lookup_pose(self):
+        odom_frame = self.get_parameter('odom_frame').value
+        base_frame = self.get_parameter('base_frame').value
+        try:
+            trans = self.tf_buffer.lookup_transform(
+                odom_frame,
+                base_frame,
+                rclpy.time.Time(),
+                timeout=Duration(seconds=TF_TIMEOUT_SEC),
+            )
+        except TransformException as exc:
+            self.get_logger().warn(f'TF lookup failed: {exc}')
+            return None
+
+        rotation = trans.transform.rotation
+        return (
+            trans.transform.translation.x,
+            trans.transform.translation.y,
+            self._yaw_from_quaternion(rotation.x, rotation.y, rotation.z, rotation.w),
+        )
+
+    def _calculate_yaw_progress(self, pose):
+        current_yaw = pose[2]
+        delta = math.atan2(
+            math.sin(current_yaw - self.prev_yaw),
+            math.cos(current_yaw - self.prev_yaw),
+        )
+        self.accumulated_yaw += delta
+        self.prev_yaw = current_yaw
+        return self.direction_sign * self.accumulated_yaw
+
+    def _log_progress(self, raw_angle, corrected_angle, error, elapsed):
+        now = self.get_clock().now()
+        if (now - self.last_log_time).nanoseconds < 1e9:
+            return
+        self.get_logger().info(
+            f'odom_angle={math.degrees(raw_angle):.1f} deg, '
+            f'corrected_angle={math.degrees(corrected_angle):.1f} deg, '
+            f'error={math.degrees(error):+.1f} deg, elapsed={elapsed:.1f} s'
+        )
+        self.last_log_time = now
+
+    def _skip_pending_sample(self, reason):
+        with self.samples_lock:
+            if self.pending_sample is None:
+                self.get_logger().warn(
+                    f'Input "{reason}" ignored. No pending successful run is waiting for input.'
+                )
+                return
+
+            skipped_sample = self.pending_sample
+            self.pending_sample = None
+            self.state = 'idle'
+
+        self.get_logger().info(
+            f'Skipped pending run by input "{reason}": '
+            f'direction={skipped_sample["direction"]:+d}, '
+            f'signed_target={skipped_sample["signed_target_angle_deg"]:.2f} deg, '
+            f'odom={skipped_sample["odom_angle_deg"]:.2f} deg, '
+            f'corrected={skipped_sample["corrected_angle_deg"]:.2f} deg, '
+            f'used_correction={skipped_sample["used_correction"]:.6f}. '
+            'This run will not be used in the final scale summary.'
+        )
+
+    def _record_pending_sample(self, ground_error_deg):
+        with self.samples_lock:
+            if self.pending_sample is None:
+                self.get_logger().warn(
+                    'No pending successful run. Set start_test:=true first, wait for the robot to stop, '
+                    'then enter the measured deg error.'
+                )
+                return
+
+            actual_angle_deg = self.pending_sample['target_angle_deg'] + ground_error_deg
+            if actual_angle_deg <= 0.0:
+                self.get_logger().error(
+                    f'Invalid measured result: target + error = {actual_angle_deg:.2f} deg. '
+                    'Re-enter the deg error for this pending run.'
+                )
+                return
+
+            sample = dict(self.pending_sample)
+            sample['ground_error_deg'] = ground_error_deg
+            sample['actual_angle_deg'] = actual_angle_deg
+            sample['scale'] = math.radians(actual_angle_deg) / math.radians(sample['odom_angle_deg'])
+            self.samples.append(sample)
+            sample_index = len(self.samples)
+            direction_index = sum(
+                1 for recorded_sample in self.samples
+                if recorded_sample['direction'] == sample['direction']
+            )
+            self.pending_sample = None
+            self.state = 'idle'
+
+        self.get_logger().info(
+            f'Recorded sample #{sample_index} overall, direction {sample["direction"]:+d} #{direction_index}: '
+            f'actual={actual_angle_deg:.2f} deg, '
+            f'ground_error={ground_error_deg:+.2f} deg, odom={sample["odom_angle_deg"]:.2f} deg, '
+            f'scale={sample["scale"]:.6f}. Set start_test:=true for the next run, or enter 0 to finish.'
+        )
+
+    def _print_summary(self):
+        with self.samples_lock:
+            samples = list(self.samples)
+
+        if not samples:
+            self.get_logger().warn('No successful calibration samples have been recorded yet.')
+            return
+
+        self.get_logger().info('========== YAW ODOM SCALE SUMMARY ==========')
+        for index, sample in enumerate(samples, start=1):
+            self.get_logger().info(
+                f'#{index:02d} direction={sample["direction"]:+d}, '
+                f'signed_target={sample["signed_target_angle_deg"]:.2f} deg, '
+                f'target={sample["target_angle_deg"]:.2f} deg, '
+                f'actual={sample["actual_angle_deg"]:.2f} deg, '
+                f'ground_error={sample["ground_error_deg"]:+.2f} deg, '
+                f'odom={sample["odom_angle_deg"]:.2f} deg, '
+                f'corrected={sample["corrected_angle_deg"]:.2f} deg, '
+                f'used_correction={sample["used_correction"]:.6f}, '
+                f'scale={sample["scale"]:.6f}'
+            )
+
+        self._print_scale_stats('all', samples)
+        for direction in (1, -1):
+            direction_samples = [sample for sample in samples if sample['direction'] == direction]
+            if direction_samples:
+                self._print_scale_stats(f'direction={direction:+d}', direction_samples)
+        self.get_logger().info('Restart this node to clear cached samples.')
+
+    def _print_scale_stats(self, label, samples):
+        scales = [sample['scale'] for sample in samples]
+        mean_scale = statistics.fmean(scales)
+        std_scale = statistics.pstdev(scales) if len(scales) > 1 else 0.0
+        self.get_logger().info(
+            f'{label}: samples={len(scales)}, recommended_odometry.scale_theta={mean_scale:.6f}, '
+            f'std={std_scale:.6f}, min={min(scales):.6f}, max={max(scales):.6f}'
+        )
+
+    def _publish_stop(self):
+        try:
+            self.cmd_vel_pub.publish(Twist())
+        except Exception:
+            pass
+
+    def _stop_robot(self):
+        for _ in range(STOP_REPEAT_COUNT):
+            self._publish_stop()
+
+    def _stop_robot_with_ros_cli(self):
+        topic = shlex.quote(self.cmd_vel_topic)
+        zero_twist = (
+            '"{linear: {x: 0.0, y: 0.0, z: 0.0}, '
+            'angular: {x: 0.0, y: 0.0, z: 0.0}}"'
+        )
+        os.system(
+            f'timeout 2s ros2 topic pub --once {topic} '
+            f'geometry_msgs/msg/Twist {zero_twist} >/dev/null 2>&1'
+        )
+
+    def _reset_start_test(self):
+        self.set_parameters([
+            Parameter('start_test', Parameter.Type.BOOL, False),
+        ])
+
+    @staticmethod
+    def _yaw_from_quaternion(x, y, z, w):
+        siny_cosp = 2.0 * (w * z + x * y)
+        cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
+        return math.atan2(siny_cosp, cosy_cosp)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OdomYawCalib()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node._stop_robot()
+        node._stop_robot_with_ros_cli()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

agv_pro_calibration/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agv_pro_calibration</name>
+  <version>0.0.0</version>
+  <description>AGV Pro calibration tools for odom, IMU, and TF health check</description>
+  <maintainer email="elephant@todo.todo">elephant</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>std_msgs</depend>
+  <depend>action_msgs</depend>
+  <depend>nav2_msgs</depend>
+  <depend>rcl_interfaces</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

agv_pro_calibration/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/agv_pro_calibration
+[install]
+install_scripts=$base/lib/agv_pro_calibration

agv_pro_calibration/setup.py

@@ -0,0 +1,29 @@
+from setuptools import find_packages, setup
+
+package_name = 'agv_pro_calibration'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='elephant',
+    maintainer_email='elephant@todo.todo',
+    description='AGV Pro calibration tools for odom, IMU, and TF health check',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'odom_linear_calib = agv_pro_calibration.odom_linear_calib:main',
+            'odom_yaw_calib = agv_pro_calibration.odom_yaw_calib:main',
+            'final_pose_refiner = agv_pro_calibration.final_pose_refiner:main',
+            'navigate_to_pose_refiner_proxy = agv_pro_calibration.navigate_to_pose_refiner_proxy:main',
+        ],
+    },
+)

agv_pro_calibration/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

agv_pro_calibration/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

agv_pro_calibration/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

agv_pro_navigation2/launch/navigation2_active.launch.py

@@ -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'),
@@ -45,12 +51,41 @@ def generate_launch_description():
             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.yaml

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

agv_pro_navigation2/map/map1.yaml

@@ -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.yaml

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

@@ -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:

agv_pro_navigation2/scripts/example_nav_to_pose.py

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

@@ -0,0 +1,3 @@
+from pymycobot import MyAGVPro
+m = MyAGVPro('/dev/agvpro_controller')
+m.power_on()

agv_pro_navigation2/scripts/record_waypoints.py

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

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