fix(obstacle_stop_planner): fix yaw calculation in acc

[tkimura4]

Signed-off-by: tomoya.kimura tomoya.kimura@tier4.jp

Related Issue(required)

Description(required)

• Bug fix of adaptive cruise controller in obstacle_stop_planner.

1. Change the point used to calculate trajectory-yaw.

In acc, target object velocity v is calculated as folloing.

v = object_velocity * cos(object_yaw - trajectory_yaw)

Then, the yaw of trajectory is calculated using collision point(blue arrow).
Thereby, the difference of yaw of object (green arrow) and between trajectory is inaccurately calculated.

In this PR, the nearest point to object (red arrow) is used for calculating yaw of trajectory.

2. re-calculate yaw in the acc function

Previously, the orientation of point was used to calculate the trajectory yaw.
However, the orientation may be inaccurate.

In this PR, the yaw of trajectory point is recalculated in the ACC node, and orientation information of trajectory point is not used..

Review Procedure(required)

test cases as below.

In this cases, v ( = object_velocity * cos(object_yaw - trajectory_yaw) ) of the crossing object should be 0 or less than 0.
However, v was sometimes calculated as positive due to the above bug, and then the mode transitions to ACC and obstacle stop did not work without merging this PR.

Confirm that the bug is fixed by this PR.
(Confirm that the obstacle stop works properly)

Related PR(optional)

Pre-Review Checklist for the PR Author

PR Author should check the checkboxes below when creating the PR.

• Read commit-guidelines
• Assign PR to reviewer

If you are adding new package following items are required:

• Documentation with description of the package is available
• A sample launch file and parameter file are available if the package contains executable nodes

Checklist for the PR Reviewer

Reviewers should check the checkboxes below before approval.

• Commits are properly organized and messages are according to the guideline
• PR title describes the changes

Post-Review Checklist for the PR Author

PR Author should check the checkboxes below before merging.

• All open points are addressed and tracked via issues or tickets

CI Checks

• Build and test for PR / build-and-test-pr: Required to pass before the merge.
• Build and test for PR / clang-tidy-pr: NOT required to pass before the merge. It is up to the reviewer(s). Found false positives? See the [guidelines][clang-tidy-guidelines].
• Check spelling: NOT required to pass before the merge. It is up to the reviewer(s). See here if you want to add some words to the spell check dictionary.

[codecov]

Codecov Report

Merging #406 (fffcb68) into tier4/proposal (ca45e4a) will decrease coverage by 10.04%.
The diff coverage is 0.00%.

@@                Coverage Diff                 @@
##           tier4/proposal    #406       +/-   ##
==================================================
- Coverage           10.04%   0.00%   -10.05%     
==================================================
  Files                 705       4      -701     
  Lines               49295    1096    -48199     
  Branches             6245       0     -6245     
==================================================
- Hits                 4952       0     -4952     
+ Misses              40285    1096    -39189     
+ Partials             4058       0     -4058     

Impacted Files	Coverage Δ
...tacle_stop_planner/src/adaptive_cruise_control.cpp	0.00% <0.00%> (ø)
...ion/include/fault_injection/diagnostic_storage.hpp
...src/outlier_filter/ring_outlier_filter_nodelet.cpp
...follower/src/qp_solver/qp_solver_unconstr_fast.cpp
...ehavior_path_planner/src/behavior_tree_manager.cpp
common/signal_processing/src/lowpass_filter_1d.cpp
...llower/test/test_longitudinal_controller_utils.cpp
..._error_monitor/src/planning_error_monitor_node.cpp
...erception/shape_estimation/lib/corrector/utils.cpp
...nning/behavior_path_planner/src/path_utilities.cpp
... and 692 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update ca45e4a...fffcb68. Read the comment docs.

[TakaHoribe]

Memo. There are 2 modifications here.

To calculate the longitudinal speed of the target vehicle along the trajectory, the yaw angle between the blue and green arrows was used. However, this calculation was not correct on a curve trajectory as in the picture. In this PR, the calculation is fixed by using the yaw angle calculation between the blue and red arrows.

The calculation of the trajectory yaw angle has also been fixed. In the previous implementation, the yaw angle in the trajectory (trajectory.points.pose.orientation) was used, but it is sometimes noisy and inappropriate to use for the purpose above (longitudinal speed calculation). So the trajectory yaw angle is calculated by atan(dx/dy) with a certain distance (5m by default).

[TakaHoribe]

Confirmed the bug has been fixed in my environment.