From ea52b3aea4c17bcb5addda9449c5d4f0214171d7 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 27 Jun 2024 17:37:06 +0900 Subject: [PATCH] delete a fucntion block. More appropriate function can be achieved by the parameter settings. Signed-off-by: Yuki Takagi --- .../config/vehicle_cmd_gate.param.yaml | 16 ++++++++-------- .../src/vehicle_cmd_gate.cpp | 18 ------------------ 2 files changed, 8 insertions(+), 26 deletions(-) diff --git a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 54c87f45b6a96..f79d0b27425b1 100644 --- a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -16,14 +16,14 @@ stop_check_duration: 1.0 nominal: vel_lim: 25.0 - reference_speed_points: [20.0, 30.0] - steer_lim: [1.0, 0.8] - steer_rate_lim: [1.0, 0.8] - lon_acc_lim: [5.0, 4.0] - lon_jerk_lim: [5.0, 4.0] - lat_acc_lim: [5.0, 4.0] - lat_jerk_lim: [7.0, 6.0] - actual_steer_diff_lim: [1.0, 0.8] + reference_speed_points: [0.1, 0.3, 20.0, 30.0] + steer_lim: [1.0, 1.0, 1.0, 0.8] + steer_rate_lim: [1.0, 1.0, 1.0, 0.8] + lon_acc_lim: [5.0, 5.0, 5.0, 4.0] + lon_jerk_lim: [80.0, 5.0, 5.0, 4.0] + lat_acc_lim: [5.0, 5.0, 5.0, 4.0] + lat_jerk_lim: [7.0, 7.0, 7.0, 6.0] + actual_steer_diff_lim: [1.0, 1.0, 1.0, 0.8] on_transition: vel_lim: 50.0 reference_speed_points: [20.0, 30.0] diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 87e79f59bc356..dd6e2f0f54aea 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -607,7 +607,6 @@ Control VehicleCmdGate::filterControlCommand(const Control & in) const auto mode = current_operation_mode_; const auto current_status_cmd = getActualStatusAsCommand(); const auto ego_is_stopped = vehicle_stop_checker_->isVehicleStopped(stop_check_duration_); - const auto input_cmd_is_stopping = in.longitudinal.acceleration < 0.0; filter_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); filter_on_transition_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); @@ -618,23 +617,6 @@ Control VehicleCmdGate::filterControlCommand(const Control & in) if (mode.is_in_transition) { filter_on_transition_.filterAll(dt, current_steer_, out, is_filter_activated); } else { - // When ego is stopped and the input command is not stopping, - // use the higher of actual vehicle longitudinal state - // and previous longitudinal command for the filtering - // this is to prevent the jerk limits being applied and causing - // a delay when restarting the vehicle. - - if (ego_is_stopped && !input_cmd_is_stopping) { - auto prev_cmd = filter_.getPrevCmd(); - prev_cmd.longitudinal.acceleration = - std::max(prev_cmd.longitudinal.acceleration, current_status_cmd.longitudinal.acceleration); - // consider reverse driving - prev_cmd.longitudinal.velocity = std::fabs(prev_cmd.longitudinal.velocity) > - std::fabs(current_status_cmd.longitudinal.velocity) - ? prev_cmd.longitudinal.velocity - : current_status_cmd.longitudinal.velocity; - filter_.setPrevCmd(prev_cmd); - } filter_.filterAll(dt, current_steer_, out, is_filter_activated); }