Handle goal node at index zero in RRT Dubins

[huxiaoyi-ovo]

Summary

• treat goal index 0 as a valid search result in RRTDubins
• add a deterministic regression test where the start already satisfies the goal pose

Problem

search_best_goal_node() returns a node index or None. The planner currently checks the result with if last_index:, so a valid result of 0 is treated as false and the planner incorrectly reports that no path was found.

This occurs naturally when the start node itself already satisfies the goal position and yaw thresholds.

Fix

Use an explicit is not None check for the returned node index.

Tests

Added test_start_already_satisfies_goal with max_iter=0, which makes node index 0 the valid goal result and verifies that planning returns a path.