From 2267824f87f088a61f6407755ed5ea215b708eb7 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 24 Sep 2023 00:56:14 +0900 Subject: [PATCH 1/2] refactor(pid_longitudinal_controller): use common elevetation angle calculation Signed-off-by: Takayuki Murooka --- .../longitudinal_controller_utils.hpp | 5 ---- .../src/longitudinal_controller_utils.cpp | 21 +++++---------- .../test_longitudinal_controller_utils.cpp | 27 ------------------- 3 files changed, 6 insertions(+), 47 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 8685d6264993b..9c01f7bc26c4b 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 @@ -66,11 +66,6 @@ double getPitchByPose(const Quaternion & quaternion); double getPitchByTraj( const Trajectory & trajectory, const size_t closest_idx, const double wheel_base); -/** - * @brief calculate elevation angle - */ -double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to); - /** * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and * acceleration for delayed time diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 70cdbcaf17bd2..41bbb48ff5d08 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -96,7 +96,8 @@ double getPitchByTraj( trajectory.points.at(nearest_idx), trajectory.points.at(i)); if (dist > wheel_base) { // calculate pitch from trajectory between rear wheel (nearest) and front center (i) - return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i)); + return tier4_autoware_utils::calcElevationAngle( + trajectory.points.at(nearest_idx).pose.position, trajectory.points.at(i).pose.position); } } @@ -108,24 +109,14 @@ double getPitchByTraj( if (dist > wheel_base) { // calculate pitch from trajectory // between wheelbase behind the end of trajectory (i) and the end of trajectory (back) - return calcElevationAngle(trajectory.points.at(i), trajectory.points.back()); + return tier4_autoware_utils::calcElevationAngle( + trajectory.points.at(i).pose.position, trajectory.points.back().pose.position); } } // calculate pitch from trajectory between the beginning and end of trajectory - return calcElevationAngle(trajectory.points.at(0), trajectory.points.back()); -} - -double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to) -{ - const double dx = p_from.pose.position.x - p_to.pose.position.x; - const double dy = p_from.pose.position.y - p_to.pose.position.y; - const double dz = p_from.pose.position.z - p_to.pose.position.z; - - const double dxy = std::max(std::hypot(dx, dy), std::numeric_limits::epsilon()); - const double pitch = std::atan2(dz, dxy); - - return pitch; + return tier4_autoware_utils::calcElevationAngle( + trajectory.points.at(0).pose.position, trajectory.points.back().pose.position); } Pose calcPoseAfterTimeDelay( 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 d9fc404ce6abe..5c7698180f82b 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -158,33 +158,6 @@ TEST(TestLongitudinalControllerUtils, getPitchByTraj) std::atan2(0.5, 1)); } -TEST(TestLongitudinalControllerUtils, calcElevationAngle) -{ - using autoware_auto_planning_msgs::msg::TrajectoryPoint; - TrajectoryPoint p_from; - p_from.pose.position.x = 0.0; - p_from.pose.position.y = 0.0; - TrajectoryPoint p_to; - p_to.pose.position.x = 1.0; - p_to.pose.position.y = 0.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), 0.0); - p_to.pose.position.x = 1.0; - p_to.pose.position.z = 1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_4); - p_to.pose.position.x = -1.0; - p_to.pose.position.z = 1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_4); - p_to.pose.position.x = 0.0; - p_to.pose.position.z = 1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_2); - p_to.pose.position.x = 1.0; - p_to.pose.position.z = -1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4); - p_to.pose.position.x = -1.0; - p_to.pose.position.z = -1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4); -} - TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) { using geometry_msgs::msg::Pose; From 5271bfbec8e7f33979694eb036fed16a33a355e5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 24 Sep 2023 09:59:08 +0900 Subject: [PATCH 2/2] update Signed-off-by: Takayuki Murooka --- .../src/pid_longitudinal_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 6d3489811b195..985a22d26f9f4 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -817,7 +817,7 @@ double PidLongitudinalController::applySlopeCompensation( const double pitch_limited = std::min(std::max(pitch, m_min_pitch_rad), m_max_pitch_rad); // Acceleration command is always positive independent of direction (= shift) when car is running - double sign = (shift == Shift::Forward) ? -1 : (shift == Shift::Reverse ? 1 : 0); + double sign = (shift == Shift::Forward) ? 1.0 : (shift == Shift::Reverse ? -1.0 : 0); double compensated_acc = input_acc + sign * 9.81 * std::sin(pitch_limited); return compensated_acc; }