feat(planning_validator): add planning validator package

[TakaHoribe]

Description

For the Issue: #2605

This PR adds a new planning_validator pkg.

motivation

• Invalid trajectory (e.g. contains infinite values, shape is broken, etc) should be detected in the planning module and handled in an appropriate way.
• the planning_validator will be put at the last stage in the planning module to check the validity of the planning output.

Design

• check some metrics
• when an invalid part is found, the plannig_validator will do some of the following list based on the parameter:
  • publish a Diagnostic msg
  • publish last validated trajectory / publish the invalid trajectory as it is / publish nothing

Related links

autoware_launch: https://github.com/tier4/autoware_launch/pull/675
(for TIER IV internal tracker: https://tier4.atlassian.net/browse/T4PB-18620)

Tests performed

Now some metrics can be seen here.

planning_vaidator_normal_4-2022-09-24_15.07.49.mp4

---

Invalid case check:
it is confirmed this PR works for the invalid trajectory. This is the case when the trajectory shape is broken when generating lane change trajectory. (For TIER IV INTERNAL, it can be found in this ticket. )

planning_validator_test-2023-01-02_12.03.12.mp4

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.

[codecov]

Codecov Report

Base: 11.37% // Head: 29.44% // Increases project coverage by +18.06% 🎉

Coverage data is based on head (bf847ac) compared to base (0305906).
Patch coverage: 45.84% of modified lines in pull request are covered.

Additional details and impacted files

@@             Coverage Diff             @@
##             main    #1947       +/-   ##
===========================================
+ Coverage   11.37%   29.44%   +18.06%     
===========================================
  Files        1277       12     -1265     
  Lines       89317      917    -88400     
  Branches    23653      277    -23376     
===========================================
- Hits        10160      270     -9890     
+ Misses      68386      442    -67944     
+ Partials    10771      205    -10566     

Flag	Coverage Δ
differential	29.44% <45.84%> (?)
total	?

Impacted Files	Coverage Δ
...jectory_publisher/invalid_trajectory_publisher.cpp	0.00% <0.00%> (ø)
...idator/test/src/test_planning_validator_pubsub.cpp	35.29% <35.29%> (ø)
...idator/test/src/test_planning_validator_helper.cpp	37.93% <37.93%> (ø)
...ning/planning_validator/src/planning_validator.cpp	45.60% <45.60%> (ø)
...tor/test/src/test_planning_validator_functions.cpp	50.94% <50.94%> (ø)
planning/planning_validator/src/utils.cpp	54.90% <54.90%> (ø)
planning/planning_validator/src/debug_marker.cpp	65.00% <65.00%> (ø)
planning/planning_validator/test/src/test_main.cpp	66.66% <66.66%> (ø)
...idator/include/planning_validator/debug_marker.hpp	100.00% <100.00%> (ø)
.../_planning_debug_tools_s.ep.rosidl_typesupport_c.c
... and 1273 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[taikitanaka3]

@TakaHoribe
TODO in description is done by planning_error_monitor or maybe by planning evaluator.

[TakaHoribe]

@taikitanaka3 right. This package is based on the planning_error_monitor and his all features are (will be) inherited to this package.

[TakaHoribe]

@TakaHoribe

[TakaHoribe]

@TakaHoribe

[xmfcx]

In addition, the validation of a generated path is also be done by iteratively simulating the control stack on the given trajectory and seeing how well it can follow it by calculating the error.

invalid trajectory (e.g. contains infinite values, shape is broken, etc) should be detected in the planning module

Is this package going to check for just these?

Maybe it'd be better if we can list the features that this node can validate.

[taikitanaka3]

@TakaHoribe
In my opinion, this validator package should use trajectory from obstacle cruise and output trajectory for smoother, because smootheris necessary to calculate desired motion (eg. stop, cruise, accelerate) using previous trajectory.

[TakaHoribe]

it'd be better if we can list the features that this node can validate.

@xmfcx Thank you for the good advice. The current implementation checks the following items:

Ported from planning_error_monitor

• invalid field (e.g. Inf, Nan)
• trajectory points interval (invalid if the distance of trajectory point is too large, like 100m)
• curvature (invalid if the trajectory has too sharp turns that is not feasible for the given vehicle kinematics)

Implemented in this PR

• relative angle in trajectory points (invalid if the yaw angle changes too fast in the sequence of trajectory points)
• lateral acceleration (invalid if the expected lateral acceleration/deceleration is too large)
• longitudinal acceleration/deceleration (invalid if the acceleration/deceleration is too large)
• steering angle (expected value estimated from curvature)
• steering angle rate (expected value estimated from curvature)
• velocity deviation (invalid if the planning velocity is too far from the ego velocity)
• distance deviation (invalid if the ego is too far from the trajectory)

I'll list them in the readme as well. Any comments are welcome.

[TakaHoribe]

In my opinion, this validator package should use trajectory from obstacle cruise and output trajectory for smoother, because smootheris necessary to calculate desired motion (eg. stop, cruise, accelerate) using previous trajectory.

@taikitanaka3 Sorry I don't get your point. Does your idea solve the original motivation: "we don't want to send the invalid trajectory to the controller"?

[taikitanaka3]

@TakaHoribe
partly yes, but my concern is Is it possible to drive using previous trajectory without smoother all the time.
-> afeter discussion with horibesan this pkg is just to validate trajectory and not to guarantee behavior when trajectory is invalid.

[wep21]

typo?

[TakaHoribe]

wow

[TakaHoribe]

thank you
#2707