fix(pid_longitudinal_control): slope definition

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/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

@@ -54,7 +54,7 @@ double calcStopDistance(
 /**
  * @brief calculate pitch angle from estimated current pose
  */
-double getPitchByPose(const Quaternion & quaternion);
+double getSlopeByPose(const Quaternion & quaternion);
 
 /**
  * @brief calculate pitch angle from trajectory on map
@@ -63,7 +63,7 @@ double getPitchByPose(const Quaternion & quaternion);
  * @param [in] closest_idx nearest index to current vehicle position
  * @param [in] wheel_base length of wheel base
  */
-double getPitchByTraj(
+double getSlopeByTraj(
   const Trajectory & trajectory, const size_t closest_idx, const double wheel_base);
 
 /**

control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

@@ -73,17 +73,25 @@ double calcStopDistance(
   return signed_length_on_traj;
 }
 
-double getPitchByPose(const Quaternion & quaternion_msg)
+// Calculate pitch angle from ego_pose information.
+// The angle of the slope is defined as positive for an uphill slope.
+double getSlopeByPose(const Quaternion & quaternion_msg)
 {
   double roll, pitch, yaw;
   tf2::Quaternion quaternion;
   tf2::fromMsg(quaternion_msg, quaternion);
   tf2::Matrix3x3{quaternion}.getRPY(roll, pitch, yaw);
 
-  return pitch;
+  // The pitch angle of the posture is defined as positive when facing downward.
+  // Thus, the definition is the opposite of the slope.
+  const auto slope = -pitch;
+
+  return slope;
 }
 
-double getPitchByTraj(
+// Calculate pitch angle from trajectory information.
+// The angle of the slope is defined as positive for an uphill slope.
+double getSlopeByTraj(
   const Trajectory & trajectory, const size_t nearest_idx, const double wheel_base)
 {
   // cannot calculate pitch

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -457,9 +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);
+  const double raw_pitch = longitudinal_utils::getSlopeByPose(current_pose.orientation);
   const double traj_pitch =
-    longitudinal_utils::getPitchByTraj(m_trajectory, control_data.nearest_idx, m_wheel_base);
+    longitudinal_utils::getSlopeByTraj(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);
   updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch);
 

control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp

@@ -102,16 +102,20 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance)
   EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), 3.0);
 }
 
-TEST(TestLongitudinalControllerUtils, getPitchByPose)
+TEST(TestLongitudinalControllerUtils, getSlopeByPose)
 {
+  // NOTE: the slope is defined as positive for an uphill slope, while the pitch is defined as
+  // positive when facing downward. They are opposite.
   tf2::Quaternion quaternion_tf;
   quaternion_tf.setRPY(0.0, 0.0, 0.0);
-  EXPECT_EQ(longitudinal_utils::getPitchByPose(tf2::toMsg(quaternion_tf)), 0.0);
+  EXPECT_EQ(longitudinal_utils::getSlopeByPose(tf2::toMsg(quaternion_tf)), 0.0);
   quaternion_tf.setRPY(0.0, 1.0, 0.0);
-  EXPECT_EQ(longitudinal_utils::getPitchByPose(tf2::toMsg(quaternion_tf)), 1.0);
+  EXPECT_EQ(longitudinal_utils::getSlopeByPose(tf2::toMsg(quaternion_tf)), -1.0);
+  quaternion_tf.setRPY(0.0, 0.1, 0.0);
+  EXPECT_EQ(longitudinal_utils::getSlopeByPose(tf2::toMsg(quaternion_tf)), -0.1);
 }
 
-TEST(TestLongitudinalControllerUtils, getPitchByTraj)
+TEST(TestLongitudinalControllerUtils, getSlopeByTraj)
 {
   using autoware_auto_planning_msgs::msg::Trajectory;
   using autoware_auto_planning_msgs::msg::TrajectoryPoint;
@@ -144,17 +148,17 @@ TEST(TestLongitudinalControllerUtils, getPitchByTraj)
   traj.points.push_back(point);
   size_t closest_idx = 0;
   EXPECT_DOUBLE_EQ(
-    std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), M_PI_4);
+    std::abs(longitudinal_utils::getSlopeByTraj(traj, closest_idx, wheel_base)), M_PI_4);
   closest_idx = 1;
   EXPECT_DOUBLE_EQ(
-    std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)), M_PI_4);
+    std::abs(longitudinal_utils::getSlopeByTraj(traj, closest_idx, wheel_base)), M_PI_4);
   closest_idx = 2;
   EXPECT_DOUBLE_EQ(
-    std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)),
+    std::abs(longitudinal_utils::getSlopeByTraj(traj, closest_idx, wheel_base)),
     std::atan2(0.5, 1));
   closest_idx = 3;
   EXPECT_DOUBLE_EQ(
-    std::abs(longitudinal_utils::getPitchByTraj(traj, closest_idx, wheel_base)),
+    std::abs(longitudinal_utils::getSlopeByTraj(traj, closest_idx, wheel_base)),
     std::atan2(0.5, 1));
 }