From 07faf545073268474b66dd9b312f849f75af0452 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 25 Sep 2023 23:36:47 +0900 Subject: [PATCH 1/2] fix sl(pid_longitudinal_control): slope definition Signed-off-by: Takamasa Horibe --- .../longitudinal_controller_utils.hpp | 4 ++-- .../src/longitudinal_controller_utils.cpp | 14 ++++++++++--- .../src/pid_longitudinal_controller.cpp | 4 ++-- .../test_longitudinal_controller_utils.cpp | 20 +++++++++++-------- 4 files changed, 27 insertions(+), 15 deletions(-) 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/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)); } From 70975dab3db10cbdd2ceb35784ba47bc7a5e1a61 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 25 Sep 2023 23:52:33 +0900 Subject: [PATCH 2/2] add slope definition in readme Signed-off-by: Takamasa Horibe --- control/pid_longitudinal_controller/README.md | 4 + .../media/slope_definition.drawio.svg | 165 ++++++++++++++++++ 2 files changed, 169 insertions(+) create mode 100644 control/pid_longitudinal_controller/media/slope_definition.drawio.svg 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/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 + + +