From 1aa97a9f1da03fc73e531ce93815ffde6a655ac9 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Thu, 12 Dec 2024 08:21:33 +0900 Subject: [PATCH] Revert "fix(controller): revival of dry steering (#7903) (#1405)" This reverts commit e7b27956d90f91dae5ca3fbf298cc623044afcb6. --- .../src/mpc.cpp | 5 +- .../pid_longitudinal_controller.hpp | 2 - .../src/pid_longitudinal_controller.cpp | 63 ++++++++----------- .../test/test_controller_node.cpp | 27 ++------ 4 files changed, 33 insertions(+), 64 deletions(-) diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index c42072a186f4d..66a4336ea9eb0 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -765,11 +765,10 @@ 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; + // when the vehicle is stopped, no steering rate limit. 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); + return VectorXd::Zero(m_param.prediction_horizon); } // calculate steering rate limit 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 61c730eb257e7..9db49a42c87e6 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 @@ -233,8 +233,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 4acb73b1d1b60..87ffcc5b5cc0f 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -603,6 +603,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; @@ -661,18 +676,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); } @@ -715,42 +727,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); return changeState(ControlState::DRIVE); 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 48ffab52e2b15..e0dc5ede906a9 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); }