From 1b568a42b4f7a6c4994899cc7fcf723981cfba3d Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi Date: Thu, 12 Dec 2024 18:18:58 +0900 Subject: [PATCH] Revert "fix(controller): revival of dry steering (#7903)" This reverts commit b43cffb3be7059640ed30b51e17e834a1ad3227d. --- .../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, 38 insertions(+), 79 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 36a79cc95728e..f498d97ba4c2f 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 @@ -272,7 +272,7 @@ class MPC */ std::pair 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. @@ -390,8 +390,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 diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 0f350dc40ad0e..1fcfcfad3f67e 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -78,9 +78,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."); } @@ -564,8 +563,7 @@ 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 double current_velocity) + const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj) { VectorXd Uex; @@ -598,7 +596,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, 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; @@ -750,8 +748,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 reference, limits; @@ -785,13 +782,6 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory( 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 1d4192d51d98d..6eb3b4d6558ad 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 @@ -238,8 +238,6 @@ 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 e3cdc4505c037..e12333755974f 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -622,6 +622,21 @@ 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; @@ -680,18 +695,15 @@ 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) { + 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. return changeState(ControlState::STOPPED); } @@ -734,42 +746,19 @@ 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); m_lpf_acc_error->reset(0.0); 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 71813c8a5c5d8..95ea4a0936837 100644 --- a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp +++ b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp @@ -589,28 +589,11 @@ 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); - { // 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); - } - - { // 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); + 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); }