fix(behavior_path_planner): set shuolder goal after loop

[kosuke55]

Signed-off-by: kosuke55 kosuke.tnp@gmail.com

Description

Fixed a problem with pull over mosule not working when goal is placed on shoulder lane after loop. This is just a workaround.
The problem is that the arc length cannot be calculated correctly, so this workaround will be considered in the lanelet function.

Related links

https://star4.slack.com/archives/CEV8XMJBV/p1659608990931289?thread_ts=1659523164.728609&cid=CEV8XMJBV

Tests performed

Notes for reviewers

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.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

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.
• The PR is ready for merge.

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

[tkimura4]

** This is just a workaround. **

Before:
The behavior_path_planner threw errors and does not output the path in a following case.

After
The behavior_path_planner outputs the path properly.

[tkimura4]

LGTM

[kosuke55]

Here is the main measure.
autowarefoundation#1571