feat(start_planner): output collided stop path

[danielsanchezaran]

Description

Sometimes, when the ego vehicle is surrounded by other vehicles/objects, the start planner is unable to find a pull out path that does not collide with other objects and instead outputs a stop path (a path with 2 points and 0 speed). However, if the surrounding vehicles move, or the ego vehicle is moved manually, as in a driver moves the vehicle to a place were it should be able to take off from, the start planner does not output a new path and, instead, the start and goal poses must be set again so the start planner can output a valid path.

With this PR, if the start planner does not find a non-collided path, it outputs the "best collided path" with a stop point on the ego's current position, and periodically checks if the stop point can be removed (either because the ego was moved manually by a driver or because the surrounding obstacles disappeared), which can be very beneficial when running Autoware on real vehicles.

Examples:

1)The start planner outputs a geometric path, but, since it is surrounded by other vehicles a stop point is inserted, once the other vehicles are removed, the ego can move:

https://github.com/autowarefoundation/autoware.universe/assets/25967964/5e6c7966-0b84-46f2-9211-607290408e84

2. The ego outputs a collided path with stop point when it is surrounded by other objects:

• First, I move the ego position (as if a driver moved the vehicle manually) to a new point in the path where the ego can continue -> the path turns green and the ego can proceed
• Second, same situation, but I eliminate the surrounding vehicles and the ego can proceed.

https://github.com/autowarefoundation/autoware.universe/assets/25967964/d85031d1-7a9e-4efd-99b2-97aeaa8568ea

Limitations: Paths with backward movements are not fully supported: Since the output paths with backward movement are "divided in several parts" it is difficult at the moment to check in which part of the path the ego vehicle is in order to re-check for collisions with static objects, the result is that, if the driver takes manually and moves the ego to a different position, the "active path" will still be the backward path and the ego wont move.

Related links

Related Ticket: TIER IV INTERNAL LINK Conversation: TIER IV INTERNAL LINK

Tests performed

PSim Evaluator: TIER IVs INTERNAL LINK

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

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

• [x] I've confirmed the contribution guidelines.
• [x] 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.

[kyoichi-sugahara]

highly appreciate this improvement

periodically checks if the stop point can be removed (either because the ego was moved manually by a driver or because the surrounding obstacles disappeared), which can be very beneficial when running Autoware on real vehicles.

[codecov[bot]]

Codecov Report

Attention: Patch coverage is 4.95050% with 96 lines in your changes are missing coverage. Please review.

Project coverage is 14.88%. Comparing base (35642a6) to head (c1a4a1d). Report is 48 commits behind head on main.

:exclamation: Current head c1a4a1d differs from pull request most recent head 32061dd. Consider uploading reports for the commit 32061dd to get more accurate results

Files	Patch %	Lines
..._start_planner_module/src/start_planner_module.cpp	0.00%	53 Missing :warning:
...ils/include/motion_utils/trajectory/trajectory.hpp	13.51%	1 Missing and 31 partials :warning:
.../motion_utils/src/trajectory/path_with_lane_id.cpp	0.00%	0 Missing and 2 partials :warning:
...tion_utils/test/src/trajectory/test_trajectory.cpp	0.00%	0 Missing and 2 partials :warning:
...th_start_planner_module/src/geometric_pull_out.cpp	0.00%	2 Missing :warning:
...r_path_start_planner_module/src/shift_pull_out.cpp	0.00%	2 Missing :warning:
...ng/behavior_path_start_planner_module/src/util.cpp	0.00%	2 Missing :warning:
...path_start_planner_module/start_planner_module.hpp	0.00%	1 Missing :warning:

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6738      +/-   ##
==========================================
- Coverage   14.93%   14.88%   -0.06%     
==========================================
  Files        1944     1950       +6     
  Lines      134061   135270    +1209     
  Branches    39891    40385     +494     
==========================================
+ Hits        20027    20134     +107     
- Misses      91735    92634     +899     
- Partials    22299    22502     +203     

Flag	Coverage Δ		*Carryforward flag
differential	18.25% <4.95%> (?)
total	14.91% <ø> (-0.02%)	:arrow_down:	Carriedforward from de21d1a

*This pull request uses carry forward flags. Click here to find out more.

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

[danielsanchezaran]

I am not happy with the current state of this PR and I believe there can be some problems related to chattering, so I am moving it back to a draft. @kyoichi-sugahara

[kyoichi-sugahara]

@danielsanchezaran Agree! thanks