refactor(pid_longitudinal_controller): use common elevetation angle calculation

[takayuki5168]

Description

use tier4_autoware_utils::calcElevationAngle instead of an internally defined function.

NOTE:
The sign of slope_angle will be opposite to the existing one.
This is because the sign of calcElevationAngle in pid_longitudinal_controller and tier4_autoware_utils are different due to how the dz is calculated. "calcElevationAngle in pid_longitudinal_controller" is wrong.

• calcElevationAngle in pid_longitudinal_controller (wrong)
  

  autoware.universe/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

  Lines 119 to 129 in 1911464

  	double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to)
  	{
  	const double dx = p_from.pose.position.x - p_to.pose.position.x;
  	const double dy = p_from.pose.position.y - p_to.pose.position.y;
  	const double dz = p_from.pose.position.z - p_to.pose.position.z;
  	const double dxy = std::max(std::hypot(dx, dy), std::numeric_limits<double>::epsilon());
  	const double pitch = std::atan2(dz, dxy);
  	return pitch;
  	}

• calcElevationAngle in tier4_autoware_utils (correct)
  

  autoware.universe/common/tier4_autoware_utils/src/geometry/geometry.cpp

  Lines 97 to 103 in 1911464

  	double calcElevationAngle(
  	const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to)
  	{
  	const double dz = p_to.z - p_from.z;
  	const double dist_2d = calcDistance2d(p_from, p_to);
  	return std::atan2(dz, dist_2d);
  	}

The sign of acceleration calculated by slope_angle will not change. Therefore, the behavior of slope compensation will not change.
This is handled by the following change of this PR.

Tests performed

unit test
psim

Effects on system behavior

nothing

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.

[codecov]

Codecov Report

Patch coverage: 71.42% and project coverage change: -0.02% ⚠️

Comparison is base (9402bb5) 15.77% compared to head (5271bfb) 15.76%.
Report is 102 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5090      +/-   ##
==========================================
- Coverage   15.77%   15.76%   -0.02%     
==========================================
  Files        1584     1584              
  Lines      109568   109542      -26     
  Branches    33728    33722       -6     
==========================================
- Hits        17286    17265      -21     
  Misses      73656    73656              
+ Partials    18626    18621       -5     

Flag	Coverage Δ		*Carryforward flag
differential	41.62% <71.42%> (?)
total	15.75% <ø> (-0.02%)	⬇️	Carriedforward from 2267824

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

Files Changed	Coverage Δ
...dinal_controller/longitudinal_controller_utils.hpp	35.00% <ø> (ø)
...nal_controller/src/pid_longitudinal_controller.cpp	39.71% <0.00%> (ø)
...roller/test/test_longitudinal_controller_utils.cpp	51.20% <ø> (-1.75%)	⬇️
...l_controller/src/longitudinal_controller_utils.cpp	80.32% <83.33%> (-2.75%)	⬇️

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

[TakaHoribe]

LGTM.

Note: This includes the fix for the definition inconsistency of pitch_from_ego and pitch_from_trajectory.