fix(obstacle_avoidance_planner, interpolation): improve slerp and fix yaw

[angry-crab]

Signed-off-by: Xinyu Wang xinyu.wang@tier4.jp

Description

#1499 Fix yaw interpolation and improve performance.
Since the current yaw interpolation is not stable, I just switched to the implementation of fillYawInTrajectoryPoint.
This PR also somehow fixes #966 by correcting yaw afterwards.

Before

[component_container_mt-15] [INFO] [1661409128.691758302] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 3.928 [ms]
[component_container_mt-15] [INFO] [1661409128.756615001] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 2.922 [ms]
[component_container_mt-15] [INFO] [1661409128.865204451] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 5.034 [ms]
[component_container_mt-15] [INFO] [1661409128.964733989] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 5.834 [ms]
[component_container_mt-15] [INFO] [1661409129.110849547] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 4.85 [ms]
[component_container_mt-15] [INFO] [1661409129.163539277] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 3.794 [ms]
[component_container_mt-15] [INFO] [1661409129.501548288] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 25.244 [ms]
[component_container_mt-15] [INFO] [1661409129.527538449] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 22.677 [ms]
[component_container_mt-15] [INFO] [1661409129.552173786] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 7.98 [ms]

After

[component_container_mt-15] [INFO] [1661412920.206313216] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 0.227 [ms]
[component_container_mt-15] [INFO] [1661412920.284322723] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 0.283 [ms]
[component_container_mt-15] [INFO] [1661412920.393833142] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 0.227 [ms]
[component_container_mt-15] [INFO] [1661412920.480718442] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 0.292 [ms]
[component_container_mt-15] [INFO] [1661412920.583334207] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 0.225 [ms]
[component_container_mt-15] [INFO] [1661412920.681510785] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 0.27 [ms]
[component_container_mt-15] [INFO] [1661412920.796739779] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 0.239 [ms]
[component_container_mt-15] [INFO] [1661412920.897798668] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 0.245 [ms]
[component_container_mt-15] [INFO] [1661412920.985440207] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 0.281 [ms]
[component_container_mt-15] [INFO] [1661412921.080549138] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 0.303 [ms]
[component_container_mt-15] [INFO] [1661412921.241484633] [obstacle_avoidance_planner.time]:     generateFineTrajectoryPoints:= 0.236 [ms]

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.

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.

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

[takayuki5168]

@angry-crab Do you know why there is large calculation cost difference between before and after? Both you set skip_optimization false?

[codecov]

Codecov Report

Merging #1687 (d67b21d) into main (728875e) will increase coverage by 0.01%.
The diff coverage is 17.39%.

@@            Coverage Diff             @@
##             main    #1687      +/-   ##
==========================================
+ Coverage   10.39%   10.40%   +0.01%     
==========================================
  Files        1218     1218              
  Lines       87391    87547     +156     
  Branches    20228    20251      +23     
==========================================
+ Hits         9082     9108      +26     
- Misses      68856    68980     +124     
- Partials     9453     9459       +6     

Flag	Coverage Δ		*Carryforward flag
differential	11.53% <0.00%> (?)
total	10.37% <28.57%> (ø)		Carriedforward from 728875e

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

Impacted Files	Coverage Δ
...e/interpolation/spline_interpolation_points_2d.hpp	90.00% <ø> (+6.66%)	⬆️
...anner/include/obstacle_avoidance_planner/utils.hpp	0.00% <0.00%> (ø)
planning/obstacle_avoidance_planner/src/utils.cpp	0.00% <0.00%> (ø)
...terpolation/src/spline_interpolation_points_2d.cpp	58.69% <57.14%> (+5.36%)	⬆️

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

[angry-crab]

@angry-crab Do you know why there is large calculation cost difference between before and after? Both you set skip_optimization ?

It has nothing to do with skip_optimization. fillYawInTrajectoryPoint performed another interpolation using the previously interpolated result, which contained a large amount of points. The second interpolation was unnecessary.

[shulanbushangshu]

@angry-crab @takayuki5168
Yes,I test the cost of the function "generateFineTrajectoryPoints".
When the length of path is default value(max:300m),the cost is shown as below:

As the comment said,the calculation is heavy.

The StepPolygon of obstacle_stop_planner is shown as below:

[shulanbushangshu]

@angry-crab
I test the changed code .It works well:
The time cost:

The result of path:

@takayuki5168
In the "fillYawInTrajectoryPoint",the most time is cost in the code("slerpYawFromPoints") as below:
std::vector yaw_vec;
for (size_t i = 0; i < points.size(); ++i) {
const double yaw = interpolator.getSplineInterpolatedYaw(i, 0.0);
yaw_vec.push_back(yaw);
}
return yaw_vec;

The code uses too much loop.
In the function"getSplineInterpolatedYaw",it can get all "diff_x"、"diff_y"in once.But"getSplineInterpolatedYaw" calculates "diff_x\diff_y" one by one.

[takayuki5168]

LGTM