From 96b36f75cdb1aab37a276e69f0243efe50dced7c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 10 Jul 2024 10:53:32 +0900 Subject: [PATCH] fix(controller): revival of dry steering (#7903) * Revert "fix(autoware_mpc_lateral_controller): delete the zero speed constraint (#7673)" This reverts commit 69258bd92cb8a0ff8320df9b2302db72975e027f. * dry steering Signed-off-by: Takayuki Murooka * add comments Signed-off-by: Takayuki Murooka * add minor fix and modify unit test for dry steering Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../autoware/mpc_lateral_controller/mpc.hpp | 5 +- .../src/mpc.cpp | 20 ++++-- .../pid_longitudinal_controller.hpp | 2 + .../src/pid_longitudinal_controller.cpp | 63 +++++++++++-------- .../test/test_controller_node.cpp | 27 ++++++-- 5 files changed, 79 insertions(+), 38 deletions(-) diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 902790f260e5e..058eb45bfaaff 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -271,7 +271,7 @@ class MPC */ std::pair executeOptimization( const MPCMatrix & mpc_matrix, const VectorXd & x0, const double prediction_dt, - const MPCTrajectory & trajectory); + const MPCTrajectory & trajectory, const double current_velocity); /** * @brief Resample the trajectory with the MPC resampling time. @@ -386,7 +386,8 @@ class MPC * @param reference_trajectory The reference trajectory. * @param current_velocity current velocity of ego. */ - VectorXd calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const; + VectorXd calcSteerRateLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const; //!< @brief logging with warn and return false template diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index ea97e9e6d5f39..dfc4d82541bf8 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -76,8 +76,9 @@ 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); + const auto [success_opt, Uex] = executeOptimization( + mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory, + current_kinematics.twist.twist.linear.x); if (!success_opt) { return fail_warn_throttle("optimization failed. Stop MPC."); } @@ -543,7 +544,8 @@ MPCMatrix MPC::generateMPCMatrix( * [ -au_lim * dt ] < [uN-uN-1] < [ au_lim * dt ] (*N... DIM_U) */ std::pair MPC::executeOptimization( - const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj) + const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj, + const double current_velocity) { VectorXd Uex; @@ -576,7 +578,7 @@ std::pair 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); + VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity); 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; @@ -728,7 +730,8 @@ double MPC::calcDesiredSteeringRate( return steer_rate; } -VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const +VectorXd MPC::calcSteerRateLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const { const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) { std::vector reference, limits; @@ -762,6 +765,13 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) c return reference.back(); }; + // When the vehicle is stopped, a large steer rate limit is used for the dry steering. + constexpr double steer_rate_lim = 100.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) { diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 5805ef7db9f86..0fd85b5822924 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -234,6 +234,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // debug values DebugValues m_debug_values; + std::optional m_prev_keep_stopped_condition{std::nullopt}; + std::shared_ptr m_last_running_time{std::make_shared(clock_->now())}; // Diagnostic diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 93496c85c741b..a0118092413a9 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -599,21 +599,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity static constexpr double vel_epsilon = 1e-3; - // Let vehicle start after the steering is converged for control accuracy - const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon && - m_enable_keep_stopped_until_steer_convergence && - !lateral_sync_data_.is_steer_converged; - if (keep_stopped_condition) { - auto marker = createDefaultMarker( - "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - marker.pose = autoware::universe_utils::calcOffsetPose( - m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang, - m_vehicle_width / 2 + 2.0, 1.5); - marker.text = "steering not\nconverged"; - m_pub_stop_reason_marker->publish(marker); - } - const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel && @@ -672,15 +657,18 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_under_control_starting_time = is_under_control ? std::make_shared(clock_->now()) : nullptr; } + + if (m_control_state != ControlState::STOPPED) { + m_prev_keep_stopped_condition = std::nullopt; + } + // transit state // in DRIVE state if (m_control_state == ControlState::DRIVE) { if (emergency_condition) { return changeState(ControlState::EMERGENCY); } - if (!is_under_control && stopped_condition && keep_stopped_condition) { - // NOTE: When the ego is stopped on manual driving, since the driving state may transit to - // autonomous, keep_stopped_condition should be checked. + if (!is_under_control && stopped_condition) { return changeState(ControlState::STOPPED); } @@ -723,19 +711,42 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // in STOPPED state if (m_control_state == ControlState::STOPPED) { - // -- debug print -- + // debug print if (has_nonzero_target_vel && !departure_condition_from_stopped) { debug_msg_once("target speed > 0, but departure condition is not met. Keep STOPPED."); } - if (has_nonzero_target_vel && keep_stopped_condition) { - debug_msg_once("target speed > 0, but keep stop condition is met. Keep STOPPED."); - } - // --------------- - if (keep_stopped_condition) { - return changeState(ControlState::STOPPED); - } if (departure_condition_from_stopped) { + // Let vehicle start after the steering is converged for dry steering + const bool current_keep_stopped_condition = + std::fabs(current_vel) < vel_epsilon && !lateral_sync_data_.is_steer_converged; + // NOTE: Dry steering is considered unnecessary when the steering is converged twice in a + // row. This is because lateral_sync_data_.is_steer_converged is not the current but + // the previous value due to the order controllers' run and sync functions. + const bool keep_stopped_condition = + !m_prev_keep_stopped_condition || + (current_keep_stopped_condition || *m_prev_keep_stopped_condition); + m_prev_keep_stopped_condition = current_keep_stopped_condition; + if (m_enable_keep_stopped_until_steer_convergence && keep_stopped_condition) { + // debug print + if (has_nonzero_target_vel) { + debug_msg_once("target speed > 0, but keep stop condition is met. Keep STOPPED."); + } + + // publish debug marker + auto marker = createDefaultMarker( + "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose = autoware::universe_utils::calcOffsetPose( + m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang, + m_vehicle_width / 2 + 2.0, 1.5); + marker.text = "steering not\nconverged"; + m_pub_stop_reason_marker->publish(marker); + + // keep STOPPED + return; + } + m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); // prevent the car from taking a long time to start to move diff --git a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp index e0dc5ede906a9..48ffab52e2b15 100644 --- a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp +++ b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp @@ -589,11 +589,28 @@ TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) traj.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); - tester.traj_pub->publish(traj); - test_utils::waitForMessage(tester.node, this, tester.received_control_command); + { // Check if the ego can keep stopped when the steering is not converged. + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); - ASSERT_TRUE(tester.received_control_command); - // Keep stopped state when the lateral control is not converged. - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); + ASSERT_TRUE(tester.received_control_command); + // Keep stopped state when the lateral control is not converged. + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); + } + + { // Check if the ego can keep stopped after the following sequence + // 1. not converged -> 2. converged -> 3. not converged + tester.publish_steer_angle(0.0); + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + tester.publish_steer_angle(steering_tire_angle); + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + // Keep stopped state when the lateral control is not converged. + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); + } }