Planner Benchmarking

name: planner-benchmarking description: 'Benchmark Nav2 planner performance. Use when measuring planning time, path length, or comparing planner quality.'

Planner Benchmarking

What to Measure

Collecting Data

Planning Time from Logs

The planner server logs planning duration at DEBUG level:

ros2 run nav2_planner planner_server --ros-args --log-level nav2_planner:=DEBUG

Look for log lines like:

[planner_server]: Created plan of length X.XX in Y.YYs

Recording Plans with rosbag2

ros2 bag record /plan /goal_pose /tf /tf_static \
  --output planner_benchmark_bag \
  --max-bag-duration 300

Replay for fair comparison across planners:

# Record goals separately
ros2 bag record /goal_pose --output goals_only

# Replay goals while running different planner configs
ros2 bag play goals_only --topics /goal_pose

Echoing the Plan Topic

ros2 topic echo /plan --once

The /plan topic publishes nav_msgs/msg/Path, containing a header and a poses[] array of geometry_msgs/msg/PoseStamped.

Repeatable Test Scenarios

Fixed Start/Goal Pairs

Define a set of test cases covering different planning challenges:

test_scenarios:
  - name: "open_room_diagonal"
    start: {x: 1.0, y: 1.0, yaw: 0.0}
    goal:  {x: 8.0, y: 6.0, yaw: 1.57}

  - name: "narrow_corridor"
    start: {x: 2.0, y: 5.0, yaw: 0.0}
    goal:  {x: 12.0, y: 5.0, yaw: 0.0}

  - name: "around_obstacle"
    start: {x: 1.0, y: 3.0, yaw: 0.0}
    goal:  {x: 1.0, y: 7.0, yaw: 3.14}

  - name: "tight_turn"
    start: {x: 3.0, y: 2.0, yaw: 1.57}
    goal:  {x: 4.0, y: 3.0, yaw: 0.0}

  - name: "long_range"
    start: {x: 0.5, y: 0.5, yaw: 0.0}
    goal:  {x: 19.0, y: 14.0, yaw: -1.57}

Automated Goal Publisher

#!/usr/bin/env python3
"""Publish a sequence of goals for planner benchmarking."""
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import math
import time
import json

class PlannerBenchmark(Node):
    def __init__(self):
        super().__init__('planner_benchmark')
        self.navigator = BasicNavigator()
        self.plan_pub = self.create_publisher(
            PoseStamped, '/goal_pose', 10
        )
        self.plan_sub = self.create_subscription(
            PoseStamped, '/plan', self.plan_callback, 10
        )
        # Subscribe to the full path
        from nav_msgs.msg import Path
        self.path_sub = self.create_subscription(
            Path, '/plan', self.path_callback, 10
        )
        self.results = []

    def make_pose(self, x, y, yaw):
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.header.stamp = self.get_clock().now().to_msg()
        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.orientation.z = math.sin(yaw / 2.0)
        pose.pose.orientation.w = math.cos(yaw / 2.0)
        return pose

    def path_callback(self, msg):
        """Analyze a received path."""
        if len(msg.poses) < 2:
            return
        metrics = compute_path_metrics(msg)
        self.results.append(metrics)
        self.get_logger().info(
            f"Path: length={metrics['length']:.3f}m, "
            f"smoothness={metrics['smoothness']:.4f}, "
            f"poses={metrics['num_poses']}"
        )

Computing Metrics from nav_msgs/Path

import math
import numpy as np
from nav_msgs.msg import Path

def compute_path_metrics(path_msg: Path) -> dict:
    """Compute benchmarking metrics from a Path message."""
    poses = path_msg.poses
    n = len(poses)
    if n < 2:
        return {'length': 0.0, 'smoothness': 0.0, 'num_poses': n,
                'max_cost': 0, 'reversals': 0}

    # Path length: sum of Euclidean segment distances
    length = 0.0
    segments = []
    for i in range(1, n):
        dx = poses[i].pose.position.x - poses[i-1].pose.position.x
        dy = poses[i].pose.position.y - poses[i-1].pose.position.y
        seg_len = math.hypot(dx, dy)
        length += seg_len
        segments.append((dx, dy, seg_len))

    # Smoothness: sum of absolute angular changes between segments
    smoothness = 0.0
    angular_changes = []
    for i in range(1, len(segments)):
        dx1, dy1, _ = segments[i-1]
        dx2, dy2, _ = segments[i]
        angle1 = math.atan2(dy1, dx1)
        angle2 = math.atan2(dy2, dx2)
        delta = abs(math.atan2(
            math.sin(angle2 - angle1),
            math.cos(angle2 - angle1)
        ))
        smoothness += delta
        angular_changes.append(delta)

    # Direction reversals (dot product sign change)
    reversals = 0
    for i in range(1, len(segments)):
        dot = (segments[i-1][0] * segments[i][0] +
               segments[i-1][1] * segments[i][1])
        if dot < 0 and segments[i-1][2] > 0.01 and segments[i][2] > 0.01:
            reversals += 1

    return {
        'length': length,
        'smoothness': smoothness,
        'max_angular_change': max(angular_changes) if angular_changes else 0.0,
        'num_poses': n,
        'reversals': reversals,
    }

Costmap Cost Along Path

To measure obstacle proximity, sample the costmap at each path pose:

def sample_costmap_along_path(costmap, path_msg):
    """Sample costmap values at each path pose.

    costmap: nav2_simple_commander costmap or OccupancyGrid.
    """
    costs = []
    for pose_stamped in path_msg.poses:
        wx = pose_stamped.pose.position.x
        wy = pose_stamped.pose.position.y
        # Convert world to map coordinates
        mx = int((wx - costmap.metadata.origin.position.x)
                 / costmap.metadata.resolution)
        my = int((wy - costmap.metadata.origin.position.y)
                 / costmap.metadata.resolution)
        if 0 <= mx < costmap.metadata.size_x and 0 <= my < costmap.metadata.size_y:
            idx = my * costmap.metadata.size_x + mx
            costs.append(costmap.data[idx])
    return {
        'max_cost': max(costs) if costs else 0,
        'mean_cost': sum(costs) / len(costs) if costs else 0,
        'total_cost': sum(costs),
        'lethal_cells': sum(1 for c in costs if c >= 253),
    }

Planner Server Parameters Affecting Benchmarks

Results Reporting

Structure results for comparison:

import json

def report_results(planner_name, scenario_name, metrics, planning_time_ms):
    return {
        'planner': planner_name,
        'scenario': scenario_name,
        'planning_time_ms': planning_time_ms,
        'path_length_m': round(metrics['length'], 3),
        'smoothness_rad': round(metrics['smoothness'], 4),
        'max_angular_change_rad': round(metrics['max_angular_change'], 4),
        'num_poses': metrics['num_poses'],
        'reversals': metrics['reversals'],
    }

# Write results to JSON for later analysis
with open('benchmark_results.json', 'w') as f:
    json.dump(all_results, f, indent=2)

Fair Comparison Checklist

1. Same map and costmap configuration across all planners.
2. Same start/goal pairs — use recorded bag files or fixed coordinates.
3. Warm up the planner — discard the first planning request (cold-start overhead from costmap subscription).
4. Multiple runs per scenario — plan each scenario 10+ times and report mean/std.
5. Same hardware and system load — close unnecessary processes, use nice -n -10 for the planner if needed.
6. Record raw data — save Path messages and timing for post-hoc analysis.