Route Manipulation Creates an Unordered List of Waypoints

[agamatlab]

The issue:

Running python leaderboard/leaderboard/leaderboard_evaluator.py generates wrongly ordered list of route_items:

route_items = list(self._route_planner.route) # in leaderboard/team_code/tcp_b2d_agent.py

In my particular scenario, the generated list is:

  1. idx0: (x≈0.00, y≈330.61), cmd=4
  2. idx1: (x≈26.10, y≈330.61), cmd=1
  3. idx2: (x≈39.09, y≈317.45), cmd=4
  4. idx3: (x≈39.12, y≈266.45), cmd=4
  5. idx4: (x≈39.15, y≈215.45), cmd=4
  6. idx5: (x≈39.10, y≈299.45), cmd=4  # wrong backward jump

Which casues my run script with npc_agent to fail with 40% route completion:

========= Results of RouteScenario_0 (repetition 0) ------ FAILURE =========

╒═══════════════════════╤═════════════════════╕
│ Start Time            │ 2026-02-28 19:05:52 │
├───────────────────────┼─────────────────────┤
│ End Time              │ 2026-02-28 19:06:08 │
├───────────────────────┼─────────────────────┤
│ System Time           │ 15.02s              │
├───────────────────────┼─────────────────────┤
│ Game Time             │ 87.75s              │
├───────────────────────┼─────────────────────┤
│ Ratio (Game / System) │ 5.844               │
╘═══════════════════════╧═════════════════════╛

╒═══════════════════════╤═════════╤══════════╕
│ Criterion             │ Result  │ Value    │
├───────────────────────┼─────────┼──────────┤
│ RouteCompletionTest   │ FAILURE │ 40.84 %  │
├───────────────────────┼─────────┼──────────┤
│ OutsideRouteLanesTest │ FAILURE │ 0 %      │
├───────────────────────┼─────────┼──────────┤
│ CollisionTest         │ SUCCESS │ 0 times  │
├───────────────────────┼─────────┼──────────┤
│ RunningRedLightTest   │ SUCCESS │ 0 times  │
├───────────────────────┼─────────┼──────────┤
│ RunningStopTest       │ SUCCESS │ 0 times  │
├───────────────────────┼─────────┼──────────┤
│ MinSpeedTest          │ FAILURE │ 309.91 % │
├───────────────────────┼─────────┼──────────┤
│ InRouteTest           │ FAILURE │          │
├───────────────────────┼─────────┼──────────┤
│ AgentBlockedTest      │ SUCCESS │          │
├───────────────────────┼─────────┼──────────┤
│ Timeout               │ SUCCESS │          │
╘═══════════════════════╧═════════╧══════════╛

Update:

After I had closer look at it, I realized that the # Bench2Drive/leaderboard/data/town01_simple.xml file is flawed. 100% can be achieved by changing all x="350" to x="340". The issue happens when the route point doesn't exactly end up on the road. So because

     <position x="350.0" y="300.0" z="0.0"/>

Not on the road, the target position snaps to:

   <position x="350.0" y="200.0" z="0.0"/>

Which also is not on the road, but since it is the last key point and the checks are more loose on the last one, it chooses that as target point. This suggests that the route_manipulation.py script requires more safe guards to avoid ambiguities like that can only be traced using days of debugging.

How you can reproduce it

To test this, I git cloned a fresh copy of this repository. After a small patch that set leaderboard/leaderboard_evaluator.py to run CARLA headful and added basic town 1 route:

# Bench2Drive/leaderboard/data/town01_simple.xml

<routes>
   <route id="0" town="Town01">
      <weathers>
         <weather route_percentage="0" cloudiness="10.0" precipitation="0.0" precipitation_deposits="0.0" wetness="0.0" wind_intensity="10.0" sun_azimuth_angle="-1.0" sun_altitude_angle="90.0" fog_density="2.0"/>
         <weather route_percentage="100" cloudiness="10.0" precipitation="0.0" precipitation_deposits="0.0" wetness="0.0" wind_intensity="10.0" sun_azimuth_angle="-1.0" sun_altitude_angle="15.0" fog_density="2.0"/>
      </weathers>
      <waypoints>
         <position x="300.0" y="330.0" z="0.0"/>
         <position x="350.0" y="300.0" z="0.0"/>
         <position x="350.0" y="200.0" z="0.0"/>
      </waypoints>
      <scenarios>
         <scenario name="Scenario_0" type="Scenario">
            <trigger_point x="125.6" y="330.6" z="0.0" yaw="0"/>
         </scenario>
      </scenarios>
   </route>
</routes>

I finally used the following run script to evaluate npc_agents behavior:

#!/usr/bin/env bash
set -euo pipefail

CARLA_ROOT="${CARLA_ROOT:-/home/aga/carla_build}"
SCENARIO_RUNNER_ROOT="${SCENARIO_RUNNER_ROOT:-/home/aga/testBench2Drive/scenario_runner}"
LEADERBOARD_ROOT="${LEADERBOARD_ROOT:-/home/aga/testBench2Drive/leaderboard}"
PYTHON_BIN="${PYTHON_BIN:-/home/aga/miniconda3/envs/carla37/bin/python}"

ROUTES_FILE="${ROUTES_FILE:-/home/aga/carla_build/Bench2Drive/leaderboard/data/town01_simple.xml}"
ROUTES_SUBSET="${ROUTES_SUBSET:-0}"
CHECKPOINT="${CHECKPOINT:-/home/aga/testBench2Drive/runs/town01_simple_npc_$(date +%Y%m%d_%H%M%S).json}"
AGENT_FILE="${AGENT_FILE:-/home/aga/testBench2Drive/leaderboard/leaderboard/autoagents/npc_agent.py}"
PORT="${PORT:-30000}"
TM_PORT="${TM_PORT:-50000}"
GPU_RANK="${GPU_RANK:-0}"

mkdir -p "$(dirname "$CHECKPOINT")"

export CARLA_ROOT
export SCENARIO_RUNNER_ROOT
export LEADERBOARD_ROOT
export PYTHONPATH="${PYTHONPATH:-}:${CARLA_ROOT}/PythonAPI:${CARLA_ROOT}/PythonAPI/carla:${CARLA_ROOT}/PythonAPI/carla/dist/carla-0.9.15-py3.7-linux-x86_64.egg:/home/aga/testBench2Drive/leaderboard:/home/aga/testBench2Drive/scenario_runner"

echo "Running evaluator with:"
echo "  ROUTES_FILE=$ROUTES_FILE"
echo "  ROUTES_SUBSET=$ROUTES_SUBSET"
echo "  CHECKPOINT=$CHECKPOINT"
echo "  PORT=$PORT TM_PORT=$TM_PORT GPU_RANK=$GPU_RANK"

"$PYTHON_BIN" "${LEADERBOARD_ROOT}/leaderboard/leaderboard_evaluator.py" \
  --routes="${ROUTES_FILE}" \
  --routes-subset="${ROUTES_SUBSET}" \
  --repetitions=1 \
  --track=SENSORS \
  --checkpoint="${CHECKPOINT}" \
  --agent="${AGENT_FILE}" \
  --agent-config= \
  --debug=0 \
  --port="${PORT}" \
  --traffic-manager-port="${TM_PORT}" \
  --gpu-rank="${GPU_RANK}"

echo "Done. Result: ${CHECKPOINT}"

How to fix it:

Applied the following .patch to leaderboard/leaderboard/utils/route_manipulation.py for a minimal fix:

@@ -11,11 +11,16 @@
 """
 
 import math
+import os
 import xml.etree.ElementTree as ET
 from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
 from agents.navigation.global_route_planner import GlobalRoutePlanner
 from agents.navigation.local_planner import RoadOption
 
+ROUTE_INTERP_DEBUG = int(os.environ.get("ROUTE_INTERP_DEBUG", "0"))
+ROUTE_INTERP_JUMP_WARN_M = float(os.environ.get("ROUTE_INTERP_JUMP_WARN_M", "20.0"))
+ROUTE_INTERP_STITCH_MAX_DIST_M = float(os.environ.get("ROUTE_INTERP_STITCH_MAX_DIST_M", "8.0"))
+
 
 def _location_to_gps(lat_ref, lon_ref, location):
     """
@@ -150,14 +155,47 @@
     route = []
     gps_route = []
 
+    prev_last_xy = None
     for i in range(len(waypoints_trajectory) - 1):
 
         waypoint = waypoints_trajectory[i]
         waypoint_next = waypoints_trajectory[i + 1]
         interpolated_trace = grp.trace_route(waypoint, waypoint_next)
+
+        # Safeguard: stitch segment boundaries to avoid large discontinuities.
+        if interpolated_trace and prev_last_xy is not None:
+            distances = []
+            for k, (wp, _) in enumerate(interpolated_trace):
+                loc = wp.transform.location
+                d = math.hypot(loc.x - prev_last_xy[0], loc.y - prev_last_xy[1])
+                distances.append((d, k))
+            min_d, min_k = min(distances, key=lambda x: x[0])
+            if min_d <= ROUTE_INTERP_STITCH_MAX_DIST_M and min_k > 0:
+                if ROUTE_INTERP_DEBUG:
+                    print(f"[interp] stitch seg={i} trim_prefix={min_k} min_join_dist={min_d:.2f}m", flush=True)
+                interpolated_trace = interpolated_trace[min_k:]
+
+        if ROUTE_INTERP_DEBUG and interpolated_trace:
+            first_wp = interpolated_trace[0][0].transform.location
+            last_wp = interpolated_trace[-1][0].transform.location
+            if prev_last_xy is not None:
+                join_jump = math.hypot(first_wp.x - prev_last_xy[0], first_wp.y - prev_last_xy[1])
+                if join_jump > ROUTE_INTERP_JUMP_WARN_M:
+                    print(
+                        f"[interp] discontinuity seg={i-1}->{i} jump={join_jump:.2f}m "
+                        f"prev_end=({prev_last_xy[0]:.2f},{prev_last_xy[1]:.2f}) "
+                        f"new_start=({first_wp.x:.2f},{first_wp.y:.2f})",
+                        flush=True,
+                    )
+
         for wp, connection in interpolated_trace:
             route.append((wp.transform, connection))
             gps_coord = _location_to_gps(lat_ref, lon_ref, wp.transform.location)
             gps_route.append((gps_coord, connection))
+        if interpolated_trace:
+            prev_last_xy = (
+                interpolated_trace[-1][0].transform.location.x,
+                interpolated_trace[-1][0].transform.location.y,
+            )
 
     return gps_route, route

Resulting in:

========= Results of RouteScenario_0 (repetition 0) ------ FAILURE =========

╒═══════════════════════╤═════════════════════╕
│ Start Time            │ 2026-03-02 14:05:53 │
├───────────────────────┼─────────────────────┤
│ End Time              │ 2026-03-02 14:06:07 │
├───────────────────────┼─────────────────────┤
│ System Time           │ 14.02s              │
├───────────────────────┼─────────────────────┤
│ Game Time             │ 85.4s               │
├───────────────────────┼─────────────────────┤
│ Ratio (Game / System) │ 6.093               │
╘═══════════════════════╧═════════════════════╛

╒═══════════════════════╤═════════╤══════════╕
│ Criterion             │ Result  │ Value    │
├───────────────────────┼─────────┼──────────┤
│ RouteCompletionTest   │ SUCCESS │ 100 %    │
├───────────────────────┼─────────┼──────────┤
│ OutsideRouteLanesTest │ SUCCESS │ 0 %      │
├───────────────────────┼─────────┼──────────┤
│ CollisionTest         │ SUCCESS │ 0 times  │
├───────────────────────┼─────────┼──────────┤
│ RunningRedLightTest   │ SUCCESS │ 0 times  │
├───────────────────────┼─────────┼──────────┤
│ RunningStopTest       │ SUCCESS │ 0 times  │
├───────────────────────┼─────────┼──────────┤
│ MinSpeedTest          │ FAILURE │ 368.71 % │
├───────────────────────┼─────────┼──────────┤
│ InRouteTest           │ SUCCESS │          │
├───────────────────────┼─────────┼──────────┤
│ AgentBlockedTest      │ SUCCESS │          │
├───────────────────────┼─────────┼──────────┤
│ Timeout               │ SUCCESS │          │
╘═══════════════════════╧═════════╧══════════╛