Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Revert "fix(controller): revival of dry steering (#7903) (#1405)" #1690

Draft
wants to merge 1 commit into
base: beta/x2_gen2/v0.29.0
Choose a base branch
from
Draft

Revert "fix(controller): revival of dry steering (#7903) (#1405)" #1690

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 2 additions & 3 deletions control/autoware_mpc_lateral_controller/src/mpc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 0 additions & 2 deletions ...l_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -233,8 +233,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
// debug values
DebugValues m_debug_values;

std::optional<bool> m_prev_keep_stopped_condition{std::nullopt};

std::shared_ptr<rclcpp::Time> m_last_running_time{std::make_shared<rclcpp::Time>(clock_->now())};

// Diagnostic
Expand Down
63 changes: 26 additions & 37 deletions control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -661,18 +676,15 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
m_under_control_starting_time =
is_under_control ? std::make_shared<rclcpp::Time>(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);
}

Expand Down Expand Up @@ -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);
Expand Down
27 changes: 5 additions & 22 deletions control/autoware_trajectory_follower_node/test/test_controller_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Loading