From e0b192e55841fac0b0799d12c64c7bd023b22378 Mon Sep 17 00:00:00 2001 From: yuki-takagi-66 Date: Fri, 20 Dec 2024 13:55:00 +0900 Subject: [PATCH 1/4] po Signed-off-by: yuki-takagi-66 --- .../pid_longitudinal_controller.hpp | 7 ++- .../src/pid_longitudinal_controller.cpp | 61 ++++++++++++++++--- 2 files changed, 57 insertions(+), 11 deletions(-) diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 8796664947a5a..1d03d1b339af6 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -204,7 +204,12 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double m_max_acc_cmd_diff; // slope compensation - enum class SlopeSource { RAW_PITCH = 0, TRAJECTORY_PITCH, TRAJECTORY_ADAPTIVE }; + enum class SlopeSource { + RAW_PITCH = 0, + TRAJECTORY_PITCH, + TRAJECTORY_ADAPTIVE, + TRAJECTORY_GOAL_ADAPTIVE + }; SlopeSource m_slope_source{SlopeSource::RAW_PITCH}; double m_adaptive_trajectory_velocity_th; std::shared_ptr m_lpf_pitch{nullptr}; diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index be969e5a4af10..e5829633d79d2 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -28,6 +28,36 @@ #include #include +#define debug(var) \ + do { \ + std::cerr << #var << ": "; \ + view(var); \ + } while (0) +template +void view(T e) +{ + std::cerr << e << std::endl; +} +template +void view(const std::vector & v) +{ + for (const auto & e : v) { + std::cerr << e << " "; + } + std::cerr << std::endl; +} +template +void view(const std::vector > & vv) +{ + for (const auto & v : vv) { + view(v); + } +} +#define line() \ + { \ + std::cerr << __LINE__ << std::endl; \ + } + namespace autoware::motion::control::pid_longitudinal_controller { PidLongitudinalController::PidLongitudinalController( @@ -196,6 +226,8 @@ PidLongitudinalController::PidLongitudinalController( m_slope_source = SlopeSource::TRAJECTORY_PITCH; } else if (slope_source == "trajectory_adaptive") { m_slope_source = SlopeSource::TRAJECTORY_ADAPTIVE; + } else if (slope_source == "trajectory_goal_adaptive") { + m_slope_source = SlopeSource::TRAJECTORY_GOAL_ADAPTIVE; } else { RCLCPP_ERROR(logger_, "Slope source is not valid. Using raw_pitch option as default"); m_slope_source = SlopeSource::RAW_PITCH; @@ -529,21 +561,30 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // 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); + m_lpf_pitch->filter(raw_pitch); const double traj_pitch = longitudinal_utils::getPitchByTraj( control_data.interpolated_traj, control_data.target_idx, m_wheel_base); if (m_slope_source == SlopeSource::RAW_PITCH) { - control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); + control_data.slope_angle = m_lpf_pitch->getValue(); } else if (m_slope_source == SlopeSource::TRAJECTORY_PITCH) { control_data.slope_angle = traj_pitch; - } else if (m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE) { + } else if ( + m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE || + m_slope_source == SlopeSource::TRAJECTORY_GOAL_ADAPTIVE) { // if velocity is high, use target idx for slope, otherwise, use raw_pitch - if (control_data.current_motion.vel > m_adaptive_trajectory_velocity_th) { - control_data.slope_angle = traj_pitch; - m_lpf_pitch->filter(raw_pitch); - } else { - control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); - } + const bool is_vel_slow = control_data.current_motion.vel < m_adaptive_trajectory_velocity_th && + m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE; + + const double goal_dist = autoware::motion_utils::calcSignedArcLength( + control_data.interpolated_traj.points, current_pose.position, + control_data.interpolated_traj.points.size() - 1); + const bool is_close_to_trajectory_end = + goal_dist < m_wheel_base && m_slope_source == SlopeSource::TRAJECTORY_GOAL_ADAPTIVE; + + control_data.slope_angle = + (is_close_to_trajectory_end || is_vel_slow) ? m_lpf_pitch->getValue() : traj_pitch; + if (m_previous_slope_angle.has_value()) { constexpr double gravity_const = 9.8; control_data.slope_angle = std::clamp( @@ -551,13 +592,13 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData m_previous_slope_angle.value() + m_min_jerk * control_data.dt / gravity_const, m_previous_slope_angle.value() + m_max_jerk * control_data.dt / gravity_const); } + m_previous_slope_angle = control_data.slope_angle; } else { RCLCPP_ERROR_THROTTLE( logger_, *clock_, 3000, "Slope source is not valid. Using raw_pitch option as default"); - control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); + control_data.slope_angle = m_lpf_pitch->getValue(); } - m_previous_slope_angle = control_data.slope_angle; updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch, m_lpf_pitch->getValue()); return control_data; From 1734ed6cd90411f8eefc7114f2a6e4166e9e8c39 Mon Sep 17 00:00:00 2001 From: yuki-takagi-66 Date: Fri, 20 Dec 2024 18:09:40 +0900 Subject: [PATCH 2/4] po Signed-off-by: yuki-takagi-66 --- .../src/pid_longitudinal_controller.cpp | 30 ------------------- 1 file changed, 30 deletions(-) diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index e5829633d79d2..ec95ce656fa6f 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -28,36 +28,6 @@ #include #include -#define debug(var) \ - do { \ - std::cerr << #var << ": "; \ - view(var); \ - } while (0) -template -void view(T e) -{ - std::cerr << e << std::endl; -} -template -void view(const std::vector & v) -{ - for (const auto & e : v) { - std::cerr << e << " "; - } - std::cerr << std::endl; -} -template -void view(const std::vector > & vv) -{ - for (const auto & v : vv) { - view(v); - } -} -#define line() \ - { \ - std::cerr << __LINE__ << std::endl; \ - } - namespace autoware::motion::control::pid_longitudinal_controller { PidLongitudinalController::PidLongitudinalController( From 6ec5a9a26979a68c16db03eb9319587e977e25b5 Mon Sep 17 00:00:00 2001 From: yuki-takagi-66 Date: Fri, 20 Dec 2024 18:25:45 +0900 Subject: [PATCH 3/4] po Signed-off-by: yuki-takagi-66 --- .../config/autoware_pid_longitudinal_controller.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml index bd905cf9be0f1..ec6a6f11b437d 100644 --- a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml +++ b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml @@ -74,7 +74,7 @@ # slope compensation lpf_pitch_gain: 0.95 - slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive + slope_source: "trajectory_goal_adaptive" # raw_pitch, trajectory_pitch, trajectory_adaptive or trajectory_goal_adaptive adaptive_trajectory_velocity_th: 1.0 max_pitch_rad: 0.1 min_pitch_rad: -0.1 From ae3ceff4fbde32bf8a31099544f203905bf35736 Mon Sep 17 00:00:00 2001 From: yuki-takagi-66 Date: Fri, 20 Dec 2024 18:47:51 +0900 Subject: [PATCH 4/4] po Signed-off-by: yuki-takagi-66 --- control/autoware_pid_longitudinal_controller/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/control/autoware_pid_longitudinal_controller/README.md b/control/autoware_pid_longitudinal_controller/README.md index b147b3b795391..aba0f36f04d65 100644 --- a/control/autoware_pid_longitudinal_controller/README.md +++ b/control/autoware_pid_longitudinal_controller/README.md @@ -68,6 +68,8 @@ There are two sources of the slope information, which can be switched by a param - Cons: z-coordinates of high-precision map is needed. - Cons: Does not support free space planning (for now) +We also offer the options to switch between these, depending on driving conditions. + **Notation:** This function works correctly only in a vehicle system that does not have acceleration feedback in the low-level control system. This compensation adds gravity correction to the target acceleration, resulting in an output value that is no longer equal to the target acceleration that the autonomous driving system desires. Therefore, it conflicts with the role of the acceleration feedback in the low-level controller.