Skip to content

Commit

Permalink
ci(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Oct 21, 2022
1 parent 894cdfa commit af3f271
Show file tree
Hide file tree
Showing 2 changed files with 6 additions and 7 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
float64_t slope_angle{0.0};
float64_t dt{0.0};
float64_t trans_deviation{0.0}; // translation deviation between nearest point and current_pose
float64_t rot_deviation{0.0}; // rotation deviation between nearest point and current_pose
float64_t rot_deviation{0.0}; // rotation deviation between nearest point and current_pose
};
ControlData m_control_data;
rclcpp::Node * node_;
Expand Down
11 changes: 5 additions & 6 deletions control/trajectory_follower/src/pid_longitudinal_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -384,7 +384,8 @@ boost::optional<LongitudinalOutput> PidLongitudinalController::run()
if (m_enable_large_tracking_error_emergency) {
m_control_state = ControlState::EMERGENCY; // update control state
}
const Motion raw_ctrl_cmd = calcEmergencyCtrlCmd(m_control_data.dt); // calculate control command
const Motion raw_ctrl_cmd =
calcEmergencyCtrlCmd(m_control_data.dt); // calculate control command
m_prev_raw_ctrl_cmd = raw_ctrl_cmd;
const auto cmd_msg =
createCtrlCmdMsg(raw_ctrl_cmd, m_control_data.current_motion.vel); // create control command
Expand Down Expand Up @@ -437,7 +438,7 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
const bool is_dist_deviation_large =
m_state_transition_params.emergency_state_traj_trans_dev < control_data.trans_deviation;
control_data.rot_deviation = std::abs(tier4_autoware_utils::normalizeRadian(
tf2::getYaw(nearest_point.orientation) - tf2::getYaw(current_pose.orientation)));
tf2::getYaw(nearest_point.orientation) - tf2::getYaw(current_pose.orientation)));
const bool is_yaw_deviation_large =
m_state_transition_params.emergency_state_traj_rot_dev < control_data.rot_deviation;

Expand Down Expand Up @@ -962,13 +963,11 @@ void PidLongitudinalController::checkControlState(
msg = "emergency occurred due to ";
}

if(m_state_transition_params.emergency_state_traj_trans_dev < m_control_data.trans_deviation)
{
if (m_state_transition_params.emergency_state_traj_trans_dev < m_control_data.trans_deviation) {
msg += "translation deviation";
}

if(m_state_transition_params.emergency_state_traj_rot_dev < m_control_data.rot_deviation)
{
if (m_state_transition_params.emergency_state_traj_rot_dev < m_control_data.rot_deviation) {
msg += "rotation deviation";
}

Expand Down

0 comments on commit af3f271

Please sign in to comment.