Cache the last-accessed segment in Path::getPathSegment

[hugogo1998]

Description

Path::getPathSegment() currently scans from the beginning for every lookup. During time parameterization, queries usually move forward, so the same segments are visited repeatedly.

This change stores the last segment index and resumes forward queries from there. Backward queries restart at the beginning. The cached index is atomic so concurrent const queries remain safe.

Results

Mean time for a full time-parameterization pass:

Waypoints	Current	This change	Speedup
20	7.83 ms	6.42 ms	1.21x
40	10.59 ms	5.98 ms	1.76x
80	18.59 ms	6.73 ms	2.76x
160	60.71 ms	7.92 ms	7.64x

Testing

Added coverage for forward and backward queries, segment boundaries, copied paths, and concurrent queries.

Built moveit_core and ran the targeted test in moveit/moveit2:humble-ci: 18/18 gtests passed; colcon reported 19 tests, 0 errors, 0 failures, 0 skipped.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend tutorials / documentation — not applicable; no user-facing change
• Document API changes in MIGRATION.md — not applicable; no API change
• Create tests — added targeted cache coverage
• Include a screenshot if changing a GUI — not applicable
• While waiting for the PR to be reviewed, review another open PR

[coderabbitai]

No actionable comments were generated in the recent review. 🎉

ℹ️ Recent review info

⚙️ Run configuration

Configuration used: defaults

Review profile: CHILL

Plan: Pro Plus

Run ID: aa442268-fa66-4405-b205-e9418418de66

📥 Commits

Reviewing files that changed from the base of the PR and between 4a5021b and 9ecb89e.

📒 Files selected for processing (3)

• moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.hpp
• moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
• moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp

Included review availability: Your plan includes up to 3 reviews per rolling hour; 2 remain after this review.

---

📝 Walkthrough

Walkthrough

Path now stores segments in indexed storage and caches the current segment index atomically. Tests cover traversal order, boundaries, copied paths, and concurrent queries.

Changes

Path-segment cache

Layer / File(s)	Summary
Indexed path-segment cache moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.hpp, moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp	Path stores segments in a std::vector and tracks the cached segment with an atomic index. getPathSegment uses indexed traversal and resets the cache for backward queries.
Cache behavior validation moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp	Tests compare cached and uncached configuration, tangent, and curvature queries across traversal orders, segment boundaries, copied paths, and concurrent access.

Estimated code review effort: 3 (Moderate) | ~20 minutes

Merge Risk: ⚪ Minimal · up to 9ecb8

This change only optimizes repeated path-segment lookups while preserving segment selection behavior; targeted tests pass, and no actionable merge-blocking risk remains.

🚥 Pre-merge checks | ✅ 5

✅ Passed checks (5 passed)

Check name	Status	Explanation
Description Check	✅ Passed	Check skipped - CodeRabbit’s high-level summary is enabled.
Title check	✅ Passed	The title clearly and concisely describes the main change: caching the last-accessed segment in Path::getPathSegment.
Docstring Coverage	✅ Passed	No functions found in the changed files to evaluate docstring coverage. Skipping docstring coverage check.
Linked Issues check	✅ Passed	Check skipped because no linked issues were found for this pull request.
Out of Scope Changes check	✅ Passed	Check skipped because no linked issues were found for this pull request.

✨ Finishing Touches

🧪 Generate unit tests (beta)

• Create PR with unit tests

---

_{Comment @coderabbitai help to get the list of available commands.}