Skip to content

Commit

Permalink
Merge branch 'main' into refactor/distortion_corrector_node
Browse files Browse the repository at this point in the history
  • Loading branch information
vividf authored Jun 26, 2024
2 parents 798b82b + 69258bd commit 803b158
Show file tree
Hide file tree
Showing 8 changed files with 31 additions and 37 deletions.
2 changes: 1 addition & 1 deletion .github/workflows/clang-tidy-differential.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ jobs:
- name: Get modified files
id: get-modified-files
run: |
echo "changed_files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' || true)" >> $GITHUB_OUTPUT
echo "changed_files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | tr '\n' ' ' || true)" >> $GITHUB_OUTPUT
shell: bash

- name: Run clang-tidy
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -271,7 +271,7 @@ class MPC
*/
std::pair<bool, VectorXd> executeOptimization(
const MPCMatrix & mpc_matrix, const VectorXd & x0, const double prediction_dt,
const MPCTrajectory & trajectory, const double current_velocity);
const MPCTrajectory & trajectory);

/**
* @brief Resample the trajectory with the MPC resampling time.
Expand Down Expand Up @@ -386,8 +386,7 @@ class MPC
* @param reference_trajectory The reference trajectory.
* @param current_velocity current velocity of ego.
*/
VectorXd calcSteerRateLimitOnTrajectory(
const MPCTrajectory & trajectory, const double current_velocity) const;
VectorXd calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const;

//!< @brief logging with warn and return false
template <typename... Args>
Expand Down
20 changes: 5 additions & 15 deletions control/autoware_mpc_lateral_controller/src/mpc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -76,9 +76,8 @@ bool MPC::calculateMPC(
const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);

// solve Optimization problem
const auto [success_opt, Uex] = executeOptimization(
mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
current_kinematics.twist.twist.linear.x);
const auto [success_opt, Uex] =
executeOptimization(mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory);
if (!success_opt) {
return fail_warn_throttle("optimization failed. Stop MPC.");
}
Expand Down Expand Up @@ -544,8 +543,7 @@ MPCMatrix MPC::generateMPCMatrix(
* [ -au_lim * dt ] < [uN-uN-1] < [ au_lim * dt ] (*N... DIM_U)
*/
std::pair<bool, VectorXd> MPC::executeOptimization(
const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj,
const double current_velocity)
const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj)
{
VectorXd Uex;

Expand Down Expand Up @@ -578,7 +576,7 @@ std::pair<bool, VectorXd> MPC::executeOptimization(
VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim); // max steering angle

// steering angle rate limit
VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity);
VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj);
VectorXd ubA = steer_rate_limits * prediction_dt;
VectorXd lbA = -steer_rate_limits * prediction_dt;
ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period;
Expand Down Expand Up @@ -730,8 +728,7 @@ double MPC::calcDesiredSteeringRate(
return steer_rate;
}

VectorXd MPC::calcSteerRateLimitOnTrajectory(
const MPCTrajectory & trajectory, const double current_velocity) const
VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const
{
const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) {
std::vector<double> reference, limits;
Expand Down Expand Up @@ -765,13 +762,6 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory(
return reference.back();
};

// when the vehicle is stopped, no steering rate limit.
constexpr double steer_rate_lim = 5.0;
const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01;
if (is_vehicle_stopped) {
return steer_rate_lim * VectorXd::Ones(m_param.prediction_horizon);
}

// calculate steering rate limit
VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon);
for (int i = 0; i < m_param.prediction_horizon; ++i) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
{
public:
/// \param node Reference to the node used only for the component and parameter initialization.
explicit PidLongitudinalController(rclcpp::Node & node);
explicit PidLongitudinalController(
rclcpp::Node & node, std::shared_ptr<diagnostic_updater::Updater> diag_updater);

private:
struct Motion
Expand Down Expand Up @@ -236,8 +237,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
std::shared_ptr<rclcpp::Time> m_last_running_time{std::make_shared<rclcpp::Time>(clock_->now())};

// Diagnostic

diagnostic_updater::Updater diagnostic_updater_;
std::shared_ptr<diagnostic_updater::Updater>
diag_updater_{}; // Diagnostic updater for publishing diagnostic data.
struct DiagnosticData
{
double trans_deviation{0.0}; // translation deviation between nearest point and current_pose
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,14 +27,16 @@

namespace autoware::motion::control::pid_longitudinal_controller
{
PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
PidLongitudinalController::PidLongitudinalController(
rclcpp::Node & node, std::shared_ptr<diagnostic_updater::Updater> diag_updater)
: node_parameters_(node.get_node_parameters_interface()),
clock_(node.get_clock()),
logger_(node.get_logger().get_child("longitudinal_controller")),
diagnostic_updater_(&node)
logger_(node.get_logger().get_child("longitudinal_controller"))
{
using std::placeholders::_1;

diag_updater_ = diag_updater;

// parameters timer
m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double();

Expand Down Expand Up @@ -432,7 +434,7 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run(
publishDebugData(ctrl_cmd, control_data);

// diagnostic
diagnostic_updater_.force_update();
diag_updater_->force_update();

return output;
}
Expand Down Expand Up @@ -1150,8 +1152,8 @@ void PidLongitudinalController::updateDebugVelAcc(const ControlData & control_da

void PidLongitudinalController::setupDiagnosticUpdater()
{
diagnostic_updater_.setHardwareID("autoware_pid_longitudinal_controller");
diagnostic_updater_.add("control_state", this, &PidLongitudinalController::checkControlState);
diag_updater_->setHardwareID("autoware_pid_longitudinal_controller");
diag_updater_->add("control_state", this, &PidLongitudinalController::checkControlState);
}

void PidLongitudinalController::checkControlState(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
switch (longitudinal_controller_mode) {
case LongitudinalControllerMode::PID: {
longitudinal_controller_ =
std::make_shared<pid_longitudinal_controller::PidLongitudinalController>(*this);
std::make_shared<pid_longitudinal_controller::PidLongitudinalController>(
*this, diag_updater_);
break;
}
default:
Expand Down
16 changes: 8 additions & 8 deletions perception/lidar_centerpoint/test/test_postprocess_kernel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -94,12 +94,12 @@ TEST_F(PostprocessKernelTest, SingleDetectionTest)
constexpr float detection_x = 70.f;
constexpr float detection_y = -38.4f;
constexpr float detection_z = 1.0;
constexpr float detection_log_w = std::log(7.0);
constexpr float detection_log_l = std::log(1.0);
constexpr float detection_log_h = std::log(2.0);
const float detection_log_w = std::log(7.0);
const float detection_log_l = std::log(1.0);
const float detection_log_h = std::log(2.0);
constexpr float detection_yaw = M_PI_4;
constexpr float detection_yaw_sin = std::sin(detection_yaw);
constexpr float detection_yaw_cos = std::sin(detection_yaw);
const float detection_yaw_sin = std::sin(detection_yaw);
const float detection_yaw_cos = std::sin(detection_yaw);
constexpr float detection_vel_x = 5.0;
constexpr float detection_vel_y = -5.0;

Expand Down Expand Up @@ -240,9 +240,9 @@ TEST_F(PostprocessKernelTest, CircleNMSTest)
constexpr float detection_x = 70.f;
constexpr float detection_y = -38.4f;
constexpr float detection_z = 1.0;
constexpr float detection_log_w = std::log(7.0);
constexpr float detection_log_l = std::log(1.0);
constexpr float detection_log_h = std::log(2.0);
const float detection_log_w = std::log(7.0);
const float detection_log_l = std::log(1.0);
const float detection_log_h = std::log(2.0);
constexpr float detection_yaw1_sin = 0.0;
constexpr float detection_yaw1_cos = 1.0;
constexpr float detection_yaw2_sin = 1.0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
"default": "4.0"
},
"path_generation_method": {
"type": "string",
"enum": ["shift_line_base", "optimization_base", "both"],
"description": "Path generation method.",
"default": "shift_line_base"
Expand Down

0 comments on commit 803b158

Please sign in to comment.