From af3f271222dc1a48e846610beb226a45dba8e8f7 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 21 Oct 2022 06:18:33 +0000 Subject: [PATCH] ci(pre-commit): autofix --- .../pid_longitudinal_controller.hpp | 2 +- .../src/pid_longitudinal_controller.cpp | 11 +++++------ 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp index 393ef447624ed..598d252723a42 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp @@ -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_; diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index 42cfa255e4974..ff0d8087d2488 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -384,7 +384,8 @@ boost::optional 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 @@ -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; @@ -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"; }