fix(simulator, controller): fix inverse pitch calculation

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.

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -457,7 +457,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);

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.