[kVinsom]

Summary

Follow up on the remaining roadmap-indexing item in #755 after #817 was merged. PRMPlanner.plan() now builds pgraph's k-d tree once, after the roadmap is complete, to accelerate the two endpoint lookups in query(). Empty roadmaps retain their existing query error. Roadmap construction and A* are unchanged.

Add examples/benchmark_roadmap.py to compare complete queries, endpoint lookups and index construction on real PRM and lattice graphs. It alternates measurement order, checks paths and lattice costs, and reports index-construction failures separately from timings.

Behaviour: nearest-vertex ties may select a different equally close vertex, and therefore a different route, than the previous scan. All sampled non-tied paths agreed exactly. The docstrings explain this and pgraph's coordinate-mutation/index-invalidation rules. No public signatures change.

Measurements and Lattice limitation

Windows, Python 3.12.14, NumPy 2.5.4, SciPy 1.18.1, spatialmath-python 1.1.18, pgraph-python 1.0.0. Medians of five repetitions, 20 fixed queries per repetition, seed 0; planning excluded. These are local measurements, not timing assertions.

PRM vertices	Scan: complete query	Indexed: complete query	Index build	Speedup
1,000	3.713 ms	1.330 ms	0.336 ms	2.79x
2,000	7.063 ms	2.461 ms	0.592 ms	2.87x

Index construction pays for itself within one query in these cases. Reproduce with:

python examples/benchmark_roadmap.py
python examples/benchmark_roadmap.py --npoints 2000
python examples/benchmark_roadmap.py --npoints 300 --iterations 4

Automatic lattice indexing is deliberately deferred. With pgraph 1.0.0, LatticePlanner().plan(iterations=5) already produces angles around -2.4492936e-16; wrapping them rounds to exactly 2*pi, which cKDTree rejects with ValueError: Some input data are greater than the size of the periodic box. The 12-iteration benchmark (1,932 vertices) reproduces this. The script reports index_error and leaves indexed timings unset. A four-iteration lattice (97 vertices) indexes successfully and its paths and costs agree. The lattice plan docstring records why its current scan is retained. This PR does not change pgraph or round lattice poses to hide that failure.

Related issue

Refs #755, particularly #755 (comment). Leaving issue closure to the maintainer because the lattice dependency limitation remains.

Validation

• Planner regression selection: 192 passed, 25 subtests passed (test_graph_planners, test_mobile_planners_a, test_prm_constructor, test_planner_failure_contract, test_planner_cleanup, test_planner_unknown_keywords, test_planner_world_coordinates), with --timeout=50 --timeout_method=thread.
• Four new tests cover indexing only the completed graph, indexed/scan path equivalence, replanning, and empty roadmaps.
• mypy --platform linux: 165 source files passed. Native Windows mypy still reports six existing Unix-signal attribute errors in unchanged symbolic_ik.py.
• Ruff formatting passes for all four changed files; no new lint findings compared with the base (16 pre-existing findings in the three existing files, none in the benchmark).
• git diff --check passes. Full test/platform matrix and Sphinx docs build were not run locally; Sphinx is not installed in this environment.

Checklist

• Checked that no other open PR addresses this work; Performance: RRTPlanner polygon transforms, PoseGraph.optimize nonzero/pad, and where build_kdtree() would help #755 was previously claimed in my comment
• PR title follows Conventional Commits
• Relevant tests pass locally; scope and limits listed above
• Added tests and updated reST docstrings
• New code uses modern type annotations
• Scoped to the remaining Performance: RRTPlanner polygon transforms, PoseGraph.optimize nonzero/pad, and where build_kdtree() would help #755 roadmap-indexing investigation
• Numerical/public behaviour changes described above
• AI assistance disclosed; Codacy is advisory
• Human author has independently reviewed and understood the diff

AI-assisted with OpenAI Codex (GPT-6).

[codacy-production]

Not up to standards ⛔

🔴 Issues 1 medium

Alerts:
⚠ 1 issue (≤ 0 issues of at least minor severity)

Results:
1 new issue

Category	Results
Security	1 medium

View in Codacy

🟢 Metrics 35 complexity · 0 duplication

Metric	Results
Complexity	35
Duplication	0

View in Codacy

_{NEW Get contextual insights on your PRs based on Codacy's metrics, along with PR and Jira context, without leaving GitHub. Enable AI reviewer
TIP This summary will be updated as you push new changes.}

[codecov]

Codecov Report

❌ Patch coverage is 0% with 2 lines in your changes missing coverage. Please review.
✅ Project coverage is 0.00%. Comparing base (31d99a9) to head (e19af8b).

Files with missing lines	Patch %	Lines
src/roboticstoolbox/mobile/PRMPlanner.py	0.00%	2 Missing ⚠️

Additional details and impacted files

@@          Coverage Diff          @@
##            main    #889   +/-   ##
=====================================
  Coverage   0.00%   0.00%           
=====================================
  Files        146     146           
  Lines      17295   17297    +2     
=====================================
- Misses     17295   17297    +2     

☔ View full report in Codecov by Harness.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.