feat(predicted_path_checker): check predicted trajectory to avoid collisions planning can not handle

[brkay54]

Description

Discussion: autowarefoundation/autoware#2859
Issue: #2252

In current implementation of Autoware, control module is not responsible for collision checking, except
obstacle_collision_checker package (It can stop the vehicle just for emergency situations.). Planning module is making
stop planning and collision checking using the reference trajectory.

However, vehicles actual path would be apart from reference trajectory in some cases such as starting driving with high
lateral error and at sharp turns.

In this scenarios, planning may not handle the collision in some part of the path because it does collision checking just
on reference trajectory. I think this is a critical safety issue, because vehicle will be open to collision in some cases.

Because vehicle’s actual path is strictly depend on the controller, we decided to add new package (predicted_path_checker) to check obstacle collision on predicted path and stop the vehicle if the collision was not handled by the planning module.

This node will make the vehicle stop if the current situation is emergency (if there is an obstacle in brake distance.) or collision can not be handled by planning module.

Flow Chart:

Before this PR merged, following PRs should be merged before:
#3437
#3435
tier4/tier4_autoware_msgs#82

Proposed Solutions:

Please feel free to share your ideas on discussion.

Related links

Tests performed

Checked the vehicle state if there is an obstacle in front of the vehicle by using planning_simulator. It worked as I wanted.

Notes for reviewers

While testing the package, you should also checkout these PRs:

#3790
autowarefoundation/autoware_launch#367
tier4/tier4_autoware_msgs#88

To test the package, it is enough to run the launch file inside of the package.

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.

[codecov]

Codecov Report

Attention: 672 lines in your changes are missing coverage. Please review.

Comparison is base (fe9ad68) 14.87% compared to head (1f2f0aa) 14.79%.
Report is 9 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2528      +/-   ##
==========================================
- Coverage   14.87%   14.79%   -0.09%     
==========================================
  Files        1629     1635       +6     
  Lines      112780   113452     +672     
  Branches    34900    34900              
==========================================
  Hits        16780    16780              
- Misses      77158    77830     +672     
  Partials    18842    18842              

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	14.87% <ø> (ø)		Carriedforward from fe9ad68

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

Files	Coverage Δ
...er/include/predicted_path_checker/debug_marker.hpp	0.00% <0.00%> (ø)
...clude/predicted_path_checker/collision_checker.hpp	0.00% <0.00%> (ø)
.../predicted_path_checker_node/collision_checker.cpp	0.00% <0.00%> (ø)
...r/src/predicted_path_checker_node/debug_marker.cpp	0.00% <0.00%> (ø)
..._checker/src/predicted_path_checker_node/utils.cpp	0.00% <0.00%> (ø)
..._path_checker_node/predicted_path_checker_node.cpp	0.00% <0.00%> (ø)

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[mitsudome-r]

We had another call to discuss this. In conclusion:

• Instead of adding new Pause state, we will add request_source field in SetPause.srv
• Pause Interface node (vehicle_command_gate) will keep the record of requested source and only release pause whenever all the source requests pause: false

[brkay54]

Hello, sorry for the delay. I designed node with using point cloud for collision checking. However I needed to make some changes in design. Currently, I changed design for using predicted objects, this week I will update the PR for this.

[TakaHoribe]

LGTM!

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.

[takayuki5168]

@brkay54 I added a label of run-build-and-test-differential to run CI.

[TakaHoribe]

@brkay54 Can we merge this PR?

[brkay54]

@TakaHoribe Sorry for the late response, I am planning to work on this, Just i need to fix a small bug. After that, I am gonna merge it in this week.

[brkay54]

So sorry for the delay.

@mehmetdogru If it is okay for you, can we merge it?

[brkay54]

Sorry for the long time for this PR. Thank you for your review @TakaHoribe I am merging this package. Also, if you have time, would you help me merge the launch file in autowarefoundation/autoware_launch#385 ? FYI @mehmetdogru

[xmfcx]

Probably should also add enable_predicted_path_checker as a launch arg here.