diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 418b3f7d27220..9d25322c793e0 100644 --- 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/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 9c01f7bc26c4b..5011af8ca15a2 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/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); /** diff --git a/control/pid_longitudinal_controller/media/slope_definition.drawio.svg b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg new file mode 100644 index 0000000000000..b5c2032208041 --- /dev/null +++ b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Slope is defined as positive for an uphill +
+
+
+ + Slope is defined as positive for an uphi... + + + + + + + + + + + + + + + +
+
+
+ (Pitch of ego is defined as negative when facing upward) +
+
+
+ + (Pitch of ego is defined as negative when facing upwar... + + + + + + +
+
+
+ x +
+
+
+ + x + + + + + + +
+
+
+ y +
+
+
+ + y + + + + + + +
+
+
+ z +
+
+
+ + z + + + + + + +
+
+
+ d_z +
+
+
+ + d_z + + + + + + +
+
+
+ d_xy +
+
+
+ + d_xy + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 9791a1f0b5e3e..6b5e5c92bd1f5 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/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 diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index a943aa05be5ff..1970267f20772 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/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); diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 5c7698180f82b..d4be98560d396 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/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)); }