From 615d61c21fd87e38da9db9f67eef601614f45080 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Sun, 11 Dec 2022 21:56:33 +0300 Subject: [PATCH] fix(trajectory_follower_nodes): mpc_follower does not send proper converged data under low steering rate limit Signed-off-by: Berkay Karaman dasds asd --- .../longitudinal_controller_base.hpp | 9 +++ .../mpc_lateral_controller.hpp | 5 +- .../pid_longitudinal_controller.hpp | 10 --- .../include/trajectory_follower/sync_data.hpp | 4 + .../src/mpc_lateral_controller.cpp | 75 +++++++++---------- .../src/pid_longitudinal_controller.cpp | 2 + 6 files changed, 54 insertions(+), 51 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp index 6eb4d13119275..4ac0a0f19153c 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp @@ -31,6 +31,7 @@ namespace control { namespace trajectory_follower { + struct LongitudinalOutput { autoware_auto_control_msgs::msg::LongitudinalCommand control_cmd; @@ -43,6 +44,14 @@ class LongitudinalControllerBase virtual void setInputData(InputData const & input_data) = 0; void sync(LateralSyncData const & lateral_sync_data) { lateral_sync_data_ = lateral_sync_data; } + std::string toStr(const ControlState s) + { + if (s == ControlState::DRIVE) return "DRIVE"; + if (s == ControlState::STOPPING) return "STOPPING"; + if (s == ControlState::STOPPED) return "STOPPED"; + if (s == ControlState::EMERGENCY) return "EMERGENCY"; + return "UNDEFINED"; + }; protected: LateralSyncData lateral_sync_data_; }; diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index d38312505fed7..2d6f224bd8bb1 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -187,7 +187,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController /** * @brief check ego car is in stopped state */ - bool isStoppedState() const; + bool isStoppedState(); /** * @brief check if the trajectory has valid value @@ -196,7 +196,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController bool isTrajectoryShapeChanged() const; - bool isSteerConverged(const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const; + bool isSteerConverged( + const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd, const bool is_mpc_solved); rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; 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 f91c09ecf3527..9cd6d6c181b08 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp @@ -98,17 +98,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal // vehicle info double m_wheel_base; - // control state - enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY }; ControlState m_control_state{ControlState::STOPPED}; - std::string toStr(const ControlState s) - { - if (s == ControlState::DRIVE) return "DRIVE"; - if (s == ControlState::STOPPING) return "STOPPING"; - if (s == ControlState::STOPPED) return "STOPPED"; - if (s == ControlState::EMERGENCY) return "EMERGENCY"; - return "UNDEFINED"; - }; // control period double m_longitudinal_ctrl_period; diff --git a/control/trajectory_follower/include/trajectory_follower/sync_data.hpp b/control/trajectory_follower/include/trajectory_follower/sync_data.hpp index 48224a5498a9c..13c0a7cd9f66a 100644 --- a/control/trajectory_follower/include/trajectory_follower/sync_data.hpp +++ b/control/trajectory_follower/include/trajectory_follower/sync_data.hpp @@ -23,6 +23,9 @@ namespace control { namespace trajectory_follower { +// control state +enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY }; + struct LateralSyncData { bool is_steer_converged{false}; @@ -31,6 +34,7 @@ struct LateralSyncData struct LongitudinalSyncData { bool is_velocity_converged{false}; + ControlState state_of_longitudinal_controller{ControlState::STOPPED}; }; } // namespace trajectory_follower diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index a90fa0fd61da6..119d7b13d4a99 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -186,6 +186,23 @@ boost::optional MpcLateralController::run() m_is_ctrl_cmd_prev_initialized = true; } + if (isStoppedState()) { + // If the difference between current steering and desired steering is large while vehicle in + // stopped state, the mpc may not solve the problem under low steering angle rate limits. And it + // sends the previous control command. We want to make sure that vehicle sends the desired + // steering angle before it starts to moving. + m_mpc.m_steer_rate_lim = std::numeric_limits::max(); + // Reset input buffer + for (auto & value : m_mpc.m_input_buffer) { + value = m_ctrl_cmd_prev.steering_tire_angle; + } + // Use previous command value as previous raw steer command + m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; + } else { + m_mpc.m_steer_rate_lim = + tier4_autoware_utils::deg2rad(node_->get_parameter("steer_rate_lim_dps").as_double()); + } + const bool is_mpc_solved = m_mpc.calculateMPC( *m_current_steering_ptr, m_current_kinematic_state_ptr->twist.twist.linear.x, m_current_kinematic_state_ptr->pose.pose, ctrl_cmd, predicted_traj, debug_values); @@ -193,23 +210,13 @@ boost::optional MpcLateralController::run() publishPredictedTraj(predicted_traj); publishDebugValues(debug_values); - const auto createLateralOutput = [this](const auto & cmd) { + const auto createLateralOutput = [this](const auto & cmd, const bool is_mpc_solved) { LateralOutput output; output.control_cmd = createCtrlCmdMsg(cmd); - output.sync_data.is_steer_converged = isSteerConverged(cmd); + output.sync_data.is_steer_converged = isSteerConverged(cmd, is_mpc_solved); return boost::optional(output); }; - if (isStoppedState()) { - // Reset input buffer - for (auto & value : m_mpc.m_input_buffer) { - value = m_ctrl_cmd_prev.steering_tire_angle; - } - // Use previous command value as previous raw steer command - m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; - return createLateralOutput(m_ctrl_cmd_prev); - } - if (!is_mpc_solved) { RCLCPP_WARN_SKIPFIRST_THROTTLE( node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, @@ -217,8 +224,9 @@ boost::optional MpcLateralController::run() ctrl_cmd = getStopControlCommand(); } + const auto output = createLateralOutput(ctrl_cmd, is_mpc_solved); m_ctrl_cmd_prev = ctrl_cmd; - return createLateralOutput(ctrl_cmd); + return output; } void MpcLateralController::setInputData(InputData const & input_data) @@ -229,7 +237,7 @@ void MpcLateralController::setInputData(InputData const & input_data) } bool MpcLateralController::isSteerConverged( - const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const + const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd, const bool is_mpc_solved) { // wait for a while to propagate the trajectory shape to the output command when the trajectory // shape is changed. @@ -237,9 +245,16 @@ bool MpcLateralController::isSteerConverged( return false; } + const auto isCmdChanging = [this](const auto & cmd) { + constexpr double min_steer_rate_treshold = 0.001; // [rad/s] + const double steer_cmd_diff = std::abs(cmd.steering_tire_angle - m_ctrl_cmd_prev.steering_tire_angle); + const double steer_cmd_rate = steer_cmd_diff / m_mpc.m_ctrl_period; + return steer_cmd_rate > min_steer_rate_treshold; // need to consider which thresh is reasonable + }; + const bool is_converged = - std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < - static_cast(m_converged_steer_rad); + (std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < + static_cast(m_converged_steer_rad)) && !isCmdChanging(cmd) && is_mpc_solved; return is_converged; } @@ -338,33 +353,15 @@ MpcLateralController::getInitialControlCommand() const return cmd; } -bool MpcLateralController::isStoppedState() const +bool MpcLateralController::isStoppedState() { - // If the nearest index is not found, return false - if (m_current_trajectory_ptr->points.empty()) { - return false; - } - - // Note: This function used to take into account the distance to the stop line - // for the stop state judgement. However, it has been removed since the steering - // control was turned off when approaching/exceeding the stop line on a curve or - // emergency stop situation and it caused large tracking error. - const size_t nearest = motion_utils::findFirstNearestIndexWithSoftConstraints( - m_current_trajectory_ptr->points, m_current_kinematic_state_ptr->pose.pose, - m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); - - const double current_vel = m_current_kinematic_state_ptr->twist.twist.linear.x; - const double target_vel = - m_current_trajectory_ptr->points.at(static_cast(nearest)).longitudinal_velocity_mps; - const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command - if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) { - return false; // not stopState: keep control + if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd, true)) { + return false; } - if ( - std::fabs(current_vel) < m_stop_state_entry_ego_speed && - std::fabs(target_vel) < m_stop_state_entry_target_speed) { + longitudinal_sync_data_.state_of_longitudinal_controller == ControlState::EMERGENCY || + longitudinal_sync_data_.state_of_longitudinal_controller == ControlState::STOPPED) { return true; } else { return false; diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index bde2139bcaa78..94df85c6f6825 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -385,6 +385,7 @@ boost::optional PidLongitudinalController::run() publishDebugData(raw_ctrl_cmd, control_data); // publish debug data LongitudinalOutput output; output.control_cmd = cmd_msg; + output.sync_data.state_of_longitudinal_controller = m_control_state; return output; } @@ -398,6 +399,7 @@ boost::optional PidLongitudinalController::run() const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd, control_data.current_motion.vel); LongitudinalOutput output; output.control_cmd = cmd_msg; + output.sync_data.state_of_longitudinal_controller = m_control_state; // publish debug data publishDebugData(ctrl_cmd, control_data);