From e7b27956d90f91dae5ca3fbf298cc623044afcb6 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 10 Jul 2024 11:31:07 +0900 Subject: [PATCH] fix(controller): revival of dry steering (#7903) (#1405) * Revert "fix(autoware_mpc_lateral_controller): delete the zero speed constraint (#7673)" This reverts commit 69258bd92cb8a0ff8320df9b2302db72975e027f. * dry steering * add comments * add minor fix and modify unit test for dry steering --------- Signed-off-by: Takayuki Murooka --- .../src/mpc.cpp | 5 +- .../pid_longitudinal_controller.hpp | 2 + .../src/pid_longitudinal_controller.cpp | 63 +++++++++++-------- .../test/test_controller_node.cpp | 27 ++++++-- 4 files changed, 64 insertions(+), 33 deletions(-) diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 1f3327bf28f2e..79748f23e7aa0 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -765,10 +765,11 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory( return reference.back(); }; - // when the vehicle is stopped, no steering rate limit. + // 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 VectorXd::Zero(m_param.prediction_horizon); + return steer_rate_lim * VectorXd::Ones(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 9291d538b022f..39c75964dbc72 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,6 +233,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 78c7ccf832514..09f4a90e14898 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -597,21 +597,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 && @@ -670,15 +655,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); } @@ -721,19 +709,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); + } }