From 7706c3ba065c99db4d830d44ed1456c72363af62 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 27 Jun 2024 17:37:06 +0900 Subject: [PATCH 1/5] delete a fucntion block. More appropriate function can be achieved by the parameter settings. Signed-off-by: Yuki Takagi --- .../src/vehicle_cmd_gate.cpp | 17 ----------------- 1 file changed, 17 deletions(-) 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..9d831e6bc08b0 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -618,23 +618,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); } From ce6953ec4e2b5b60881c022bece6b7dfbaa81803 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Fri, 19 Jul 2024 13:40:29 +0900 Subject: [PATCH 2/5] add notation to readme Signed-off-by: Yuki Takagi --- control/autoware_vehicle_cmd_gate/README.md | 4 ++++ .../config/vehicle_cmd_gate.param.yaml | 16 ++++++++-------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/control/autoware_vehicle_cmd_gate/README.md b/control/autoware_vehicle_cmd_gate/README.md index 2d6f5c5f949af..e46db3c06cfeb 100644 --- a/control/autoware_vehicle_cmd_gate/README.md +++ b/control/autoware_vehicle_cmd_gate/README.md @@ -78,6 +78,10 @@ The limitation values are calculated based on the 1D interpolation of the limita Notation: this filter is not designed to enhance ride comfort. Its main purpose is to detect and remove abnormal values in the control outputs during the final stages of Autoware. If this filter is frequently active, it implies the control module may need tuning. If you're aiming to smoothen the signal via a low-pass filter or similar techniques, that should be handled in the control module. When the filter is activated, the topic `~/is_filter_activated` is published. +Notation 2: If you use vehicles in which the driving force is controlled by the accelerator/brake pedal, the jerk limit, denoting the pedal rate limit, must be sufficiently relaxed at low speeds. +Otherwise, quick pedal changes at start/stop will not be possible, resulting in slow starts and creep down on hills. +This functionality for starting/stopping was embedded in the source code but was removed because it was complex and could be achieved by parameters. + ## Assumptions / Known limits The parameter `check_external_emergency_heartbeat` (true by default) enables an emergency stop request from external modules. 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..a0c8fd7752717 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] # The first element is required for quick pedal changes when stopping and starting on a slope. + 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] From 11475e10b445d7ebcaaefb2430ecd4cb2bb4508f Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Fri, 19 Jul 2024 19:48:22 +0900 Subject: [PATCH 3/5] update comment Signed-off-by: Yuki Takagi --- .../config/vehicle_cmd_gate.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 a0c8fd7752717..74affea696893 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 @@ -20,7 +20,7 @@ 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] # The first element is required for quick pedal changes when stopping and starting on a slope. + lon_jerk_lim: [80.0, 5.0, 5.0, 4.0] # The first element is required for quick pedal changes when stopping and starting. 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] From 745374dd12588c04a49e4f116c244f4ea2572171 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Mon, 22 Jul 2024 13:17:16 +0900 Subject: [PATCH 4/5] delete unused var Signed-off-by: Yuki Takagi --- control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 2 -- 1 file changed, 2 deletions(-) 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 9d831e6bc08b0..c2e10d8f077b0 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -606,8 +606,6 @@ Control VehicleCmdGate::filterControlCommand(const Control & in) const double dt = getDt(); 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); From 4c6d79c51458268d8c881d47393bc8a1a478ecb0 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Mon, 22 Jul 2024 13:39:59 +0900 Subject: [PATCH 5/5] restore Signed-off-by: Yuki Takagi --- control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 1 + 1 file changed, 1 insertion(+) 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 c2e10d8f077b0..dd6e2f0f54aea 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -606,6 +606,7 @@ Control VehicleCmdGate::filterControlCommand(const Control & in) const double dt = getDt(); const auto mode = current_operation_mode_; const auto current_status_cmd = getActualStatusAsCommand(); + const auto ego_is_stopped = vehicle_stop_checker_->isVehicleStopped(stop_check_duration_); filter_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); filter_on_transition_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x);