From 7b68cd7078e72fd659421b1a26428c0287e72f0f Mon Sep 17 00:00:00 2001 From: Gabriel Alcantara Date: Thu, 10 Jun 2021 12:37:53 +0200 Subject: [PATCH] Set cmd_dt to zero when detecting negative value Instead of setting curr_cmd to zero, the correct approach is to set cmd_dt to zero and this will then keep the current control, without any abrupt change Co-authored-by: Matt Reynolds --- .../src/ackermann_steering_controller.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index f24291bd7..c57b7f643 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -321,12 +321,11 @@ namespace ackermann_steering_controller{ } // Limit velocities and accelerations: - const double cmd_dt = (period.toSec() > 0) ? period.toSec() : 0.0; - if(cmd_dt < 0) + double cmd_dt(period.toSec()); + if (cmd_dt < 0.0) { ROS_ERROR("Invalid time interval, delta time cannot be negative"); - curr_cmd.lin = 0.0; - curr_cmd.ang = 0.0; + cmd_dt = 0.0; } limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);