Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(pid_longitudinal_controller): update trajectory_adaptive; add debug_values, adopt rate limit fillter (#9656) #1704

Open
wants to merge 1 commit into
base: beta/x2_gen2/v0.29.0
Choose a base branch
from
Open

feat(pid_longitudinal_controller): update trajectory_adaptive; add debug_values, adopt rate limit fillter (#9656) #1704

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions ...pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,9 @@ class DebugValues
NEAREST_VEL = 4,
NEAREST_ACC = 5,
SHIFT = 6,
PITCH_LPF_RAD = 7,
PITCH_USING_RAD = 7,
PITCH_RAW_RAD = 8,
PITCH_LPF_DEG = 9,
PITCH_USING_DEG = 9,
PITCH_RAW_DEG = 10,
ERROR_VEL = 11,
ERROR_VEL_FILTERED = 12,
Expand All @@ -58,6 +58,8 @@ class DebugValues
STOP_DIST = 28,
FF_SCALE = 29,
ACC_CMD_FF = 30,
PITCH_LPF_RAD = 31,
PITCH_LPF_DEG = 32,
SIZE // this is the number of enum elements
};

Expand Down
12 changes: 8 additions & 4 deletions ...l_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -211,6 +211,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
std::shared_ptr<LowpassFilter1d> m_lpf_pitch{nullptr};
double m_max_pitch_rad;
double m_min_pitch_rad;
std::optional<double> m_previous_slope_angle{std::nullopt};

// ego nearest index search
double m_ego_nearest_dist_threshold;
Expand Down Expand Up @@ -393,11 +394,14 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro

/**
* @brief update variables for debugging about pitch
* @param [in] pitch current pitch of the vehicle (filtered)
* @param [in] traj_pitch current trajectory pitch
* @param [in] raw_pitch current raw pitch of the vehicle (unfiltered)
* @param [in] pitch_using
* @param [in] traj_pitch
* @param [in] localization_pitch
* @param [in] localization_pitch_lpf
*/
void updatePitchDebugValues(const double pitch, const double traj_pitch, const double raw_pitch);
void updatePitchDebugValues(
const double pitch_using, const double traj_pitch, const double localization_pitch,
const double localization_pitch_lpf);

/**
* @brief update variables for velocity and acceleration
Expand Down
23 changes: 17 additions & 6 deletions control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -556,13 +556,21 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
} else {
control_data.slope_angle = m_lpf_pitch->filter(raw_pitch);
}
if (m_previous_slope_angle.has_value()) {
constexpr double gravity_const = 9.8;
control_data.slope_angle = std::clamp(
control_data.slope_angle,
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);
}
} 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);
}

updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch);
m_previous_slope_angle = control_data.slope_angle;
updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch, m_lpf_pitch->getValue());

return control_data;
}
Expand Down Expand Up @@ -1139,13 +1147,16 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
}

void PidLongitudinalController::updatePitchDebugValues(
const double pitch, const double traj_pitch, const double raw_pitch)
const double pitch_using, const double traj_pitch, const double localization_pitch,
const double localization_pitch_lpf)
{
const double to_degrees = (180.0 / static_cast<double>(M_PI));
m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, pitch);
m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, pitch * to_degrees);
m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, raw_pitch);
m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, raw_pitch * to_degrees);
m_debug_values.setValues(DebugValues::TYPE::PITCH_USING_RAD, pitch_using);
m_debug_values.setValues(DebugValues::TYPE::PITCH_USING_DEG, pitch_using * to_degrees);
m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, localization_pitch_lpf);
m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, localization_pitch_lpf * to_degrees);
m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, localization_pitch);
m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, localization_pitch * to_degrees);
m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_RAD, traj_pitch);
m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees);
}
Expand Down
Loading