fix(simulator, controller): fix inverse pitch calculation (backport a…

…utowarefoundation#5199) (#1025) fix(simulator, controller): fix inverse pitch calculation (autowarefoundation#5199) Signed-off-by: Mamoru Sobue <[email protected]> Signed-off-by: Takamasa Horibe <[email protected]> Co-authored-by: Mamoru Sobue <[email protected]> Co-authored-by: Takamasa Horibe <[email protected]>
tier4 · Nov 20, 2023 · 0b731b4 · 0b731b4
1 parent c19995a
commit 0b731b4
Show file tree

Hide file tree

Showing 6 changed files with 508 additions and 2 deletions.
diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md
@@ -75,6 +75,10 @@ For instance, if the vehicle is attempting to start with an acceleration of `1.0
 
 A suitable example of a vehicle system for the slope compensation function is one in which the output acceleration from the longitudinal_controller is converted into target accel/brake pedal input without any feedbacks. In this case, the output acceleration is just used as a feedforward term to calculate the target pedal, and hence the issue mentioned above does not arise.
 
+Note: The angle of the slope is defined as positive for an uphill slope, while the pitch angle of the ego pose is defined as negative when facing upward. They have an opposite definition.
+
+![slope_definition](./media/slope_definition.drawio.svg)
+
 #### PID control
 
 For deviations that cannot be handled by FeedForward control, such as model errors, PID control is used to construct a feedback system.

diff --git a/control/pid_longitudinal_controller/media/slope_definition.drawio.svg b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg
diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -450,7 +450,9 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
     current_pose, m_trajectory, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold);
 
   // pitch
-  const double raw_pitch = longitudinal_utils::getPitchByPose(current_pose.orientation);
+  // NOTE: getPitchByTraj() calculates the pitch angle as defined in
+  // ../media/slope_definition.drawio.svg while getPitchByPose() is not, so `raw_pitch` is reversed
+  const double raw_pitch = (-1.0) * longitudinal_utils::getPitchByPose(current_pose.orientation);
   const double traj_pitch =
     longitudinal_utils::getPitchByTraj(m_trajectory, control_data.nearest_idx, m_wheel_base);
   control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch);

diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md
@@ -89,6 +89,12 @@ _Note_: The steering/velocity/acceleration dynamics is modeled by a first order
 Since the vehicle outputs `odom`->`base_link` tf, this simulator outputs the tf with the same frame_id configuration.
 In the simple_planning_simulator.launch.py, the node that outputs the `map`->`odom` tf, that usually estimated by the localization module (e.g. NDT), will be launched as well. Since the tf output by this simulator module is an ideal value, `odom`->`map` will always be 0.
 
+### (Caveat) Pitch calculation
+
+Ego vehicle pitch angle is calculated in the following manner.
+
+![pitch calculation](./media/pitch-calculation.drawio.svg)
+
 ## Error detection and handling
 
 The only validation on inputs being done is testing for a valid vehicle model type.