[huxiaoyi-ovo]

Summary

InformedRRTStar.find_near_nodes() builds squared distances and maps each matching distance back to an index using d_list.index(distance). That lookup always returns the first occurrence.

When two distinct nodes are equally far from a new sample, the method can return [0, 0] instead of [0, 1]. The second candidate is never considered for parent selection or rewiring.

A deterministic example has two obstacle-free parent candidates at (-1, 0) and (1, 0) with costs 9 and 1, and a new node at (0, 0). The old lookup duplicates the first parent, producing cost 10; preserving the actual indices selects the second parent at cost 2.

Fix

Use enumerate(d_list) to retain each vertex's original index. The existing neighbor-radius calculation, geometry, and collision checks remain unchanged.

Regression tests

• Distinct equidistant neighbors are both retained and the lower-cost parent is chosen.
• Separate vertices with identical coordinates retain distinct indices, while a distant vertex remains excluded.

The change is limited to the Informed RRT* example and its tests, with no new dependencies.

Related: #1443 fixes a similar index-identity bug in the separate PathPlanning/RRTStar/rrt_star.py implementation; this PR covers PathPlanning/InformedRRTStar/informed_rrt_star.py.

Independent cross-platform CI validation has been started.

AI disclosure: ChatGPT assisted with analyzing the bug and preparing the fix and regression tests.