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'