Skip to content

Commit

Permalink
delete a fucntion block. More appropriate function can be achieved by…
Browse files Browse the repository at this point in the history
… the parameter settings.

Signed-off-by: Yuki Takagi <[email protected]>
  • Loading branch information
yuki-takagi-66 committed Jun 27, 2024
1 parent a6b5f27 commit 544bd64
Showing 1 changed file with 0 additions and 17 deletions.
17 changes: 0 additions & 17 deletions control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -613,23 +613,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);
}

Expand Down

0 comments on commit 544bd64

Please sign in to comment.