Path Tracking Metrics

name: path-tracking-metrics description: 'Measure path tracking performance. Use when computing cross-track error, heading error, or evaluating controller quality.'

Path Tracking Metrics — Measuring Navigation Quality

Key Metrics

1. Cross-Track Error (CTE)

Lateral distance from the robot's actual position to the nearest point on the planned path. This is the primary measure of path-following fidelity.

2. Heading Error

Angle between the robot's actual heading and the tangent of the planned path at the nearest point. Measured in radians.

3. Path Length Ratio

actual_distance_traveled / planned_path_length. Ratio of 1.0 means the robot followed the exact planned path. Greater than 1.0 means the robot traveled further (shortcuts, oscillations, detours).

4. Time to Completion

Wall-clock time from receiving the goal to reaching it. Compare against theoretical minimum (path_length / desired_linear_vel) to assess efficiency.

5. Recovery Event Count

Number of times recovery behaviors triggered during a single navigation goal. Ideal: 0. If consistently > 0, the controller or costmap needs tuning.

6. Average Velocity

Mean linear velocity along the path. Compare to desired_linear_vel / vx_max to assess whether regulation or obstacles are reducing speed.

Data Collection

Required Topics

Recording with rosbag2

ros2 bag record -o nav_metrics_bag \
  /odom /plan /local_plan /cmd_vel /tf /tf_static \
  --max-bag-duration 300

Record during a typical patrol run. Include /tf and /tf_static for later replay.

Analysis Script

#!/usr/bin/env python3
"""Compute path tracking metrics from a rosbag2 recording."""

import numpy as np
from rosbags.rosbag2 import Reader
from rosbags.serde import deserialize_cdr
import math


def nearest_point_on_path(robot_pos, path_points):
    """Find nearest point on path and return distance and index."""
    min_dist = float('inf')
    min_idx = 0
    for i, pp in enumerate(path_points):
        d = math.hypot(robot_pos[0] - pp[0], robot_pos[1] - pp[1])
        if d < min_dist:
            min_dist = d
            min_idx = i
    return min_dist, min_idx


def path_tangent_angle(path_points, idx):
    """Compute path tangent angle at index."""
    if idx >= len(path_points) - 1:
        idx = len(path_points) - 2
    dx = path_points[idx + 1][0] - path_points[idx][0]
    dy = path_points[idx + 1][1] - path_points[idx][1]
    return math.atan2(dy, dx)


def yaw_from_quaternion(q):
    """Extract yaw from quaternion (x, y, z, w)."""
    siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
    cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
    return math.atan2(siny_cosp, cosy_cosp)


def analyze_bag(bag_path):
    cte_values = []
    heading_errors = []
    velocities = []
    path_points = None
    odom_positions = []

    with Reader(bag_path) as reader:
        for connection, timestamp, rawdata in reader.messages():
            if connection.topic == '/plan':
                msg = deserialize_cdr(rawdata, connection.msgtype)
                path_points = [
                    (p.pose.position.x, p.pose.position.y)
                    for p in msg.poses
                ]

            elif connection.topic == '/odom' and path_points:
                msg = deserialize_cdr(rawdata, connection.msgtype)
                pos = msg.pose.pose.position
                orient = msg.pose.pose.orientation
                robot_pos = (pos.x, pos.y)
                odom_positions.append(robot_pos)

                # Cross-track error
                cte, idx = nearest_point_on_path(robot_pos, path_points)
                cte_values.append(cte)

                # Heading error
                yaw = yaw_from_quaternion([
                    orient.x, orient.y, orient.z, orient.w])
                tangent = path_tangent_angle(path_points, idx)
                h_err = abs(math.atan2(
                    math.sin(yaw - tangent), math.cos(yaw - tangent)))
                heading_errors.append(h_err)

                # Velocity
                vel = msg.twist.twist.linear.x
                velocities.append(vel)

    if not cte_values:
        print("No data collected. Check bag contents.")
        return

    # Path length ratio
    actual_dist = sum(
        math.hypot(
            odom_positions[i + 1][0] - odom_positions[i][0],
            odom_positions[i + 1][1] - odom_positions[i][1])
        for i in range(len(odom_positions) - 1)
    )
    planned_dist = sum(
        math.hypot(
            path_points[i + 1][0] - path_points[i][0],
            path_points[i + 1][1] - path_points[i][1])
        for i in range(len(path_points) - 1)
    ) if path_points else 1.0

    print(f"=== Path Tracking Metrics ===")
    print(f"Samples: {len(cte_values)}")
    print(f"Cross-Track Error:")
    print(f"  Mean: {np.mean(cte_values):.3f} m")
    print(f"  Max:  {np.max(cte_values):.3f} m")
    print(f"  Std:  {np.std(cte_values):.3f} m")
    print(f"Heading Error:")
    print(f"  Mean: {np.mean(heading_errors):.3f} rad "
          f"({np.degrees(np.mean(heading_errors)):.1f}°)")
    print(f"  Max:  {np.max(heading_errors):.3f} rad")
    print(f"Path Length Ratio: {actual_dist / planned_dist:.3f}")
    print(f"Average Velocity: {np.mean(velocities):.3f} m/s")


if __name__ == '__main__':
    import sys
    analyze_bag(sys.argv[1] if len(sys.argv) > 1 else 'nav_metrics_bag')

Using the Script

# Install dependencies
pip install rosbags numpy

# Run analysis
python3 analyze_nav_metrics.py nav_metrics_bag/

Interpreting Results for Tuning

Continuous Monitoring

For production patrol robots, log metrics per-goal and aggregate over time:

# In your patrol node, subscribe to /odom and /plan
# Compute per-goal CTE and log to file
# Alert if mean CTE exceeds threshold for N consecutive goals

This enables early detection of:

• Costmap drift (SLAM map vs reality).
• Mechanical issues (wheel slip → high CTE).
• Environment changes (new furniture → frequent recoveries).