fix(avoidance): fix a bug regarding the nearest search

[yuki-takagi-66]

Description

This PR fix a bug regarding the nearest search in the function extendBackwardLength()
This function requires two pose which are appropriately near to the current ego pose.
The first one is denoted by orig_ego_idx and shown in the figure as the blue arrow, the other one is dented by prev_ego_idx and shown in the figure as the red arrow.
The code so far failure to find prev_ego_idx appropriately and this PR fix this bug.

This PR is re-opened as PR from https://github.com/autowarefoundation/autoware.universe/pull/5574 by the author's mistaken force push

Tests performed

psim tests and the tier4 internal test were performed in success.

Effects on system behavior

This PR enables to the system generates avoidance path appropriately on the curved reference paths

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.