GreedyLatticePlanner emergency-stops near route end when remaining path is shorter than maneuver_distance

[YousefBabaa]

Near the end of a short global route, the local lattice planner can generate zero edges and trigger an emergency stop even though there is no collision and no boundary violation.

Observed with:

c26_maneuver_distance: 10
c26_planning_horizon: 1

The vehicle stopped about 8.2 m before the selected global goal. Debug showed:

lattice_edges changed from 2 to 0
selected_collision = False
selected_boundary_violation = False
selected local velocity became a ramp to zero

Cause appears to be in avlite/avlite/c20_planning/c27_lattice.py around line 90:

s1_ = s1_ + maneuver_distance
if s1_ > self.global_trajectory.path_s[-2]:
    break

When the remaining route is shorter than maneuver_distance, no next lattice node is created. Then avlite/avlite/c20_planning/c26_local_lattice_planners.py around line 466 hits the no-edge emergency-stop branch.

Suggested fix(not tested): clamp the final sample to the end of the path instead of immediately breaking.

Example idea in c27_lattice.py:

path_end_s = self.global_trajectory.path_s[-2]

for l in range(1, self.planning_horizon + 1):
    s_next = s1_ + maneuver_distance

    if s_next > path_end_s:
        s_next = path_end_s

    if s_next <= s1_:
        break

    s1_ = s_next

    # create the normal centerline/sample nodes here

    if s1_ >= path_end_s:
        break

That would let the planner create a shorter final lattice edge to the route end instead of producing zero edges and emergency-stopping early.