diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 663b8662e..003efc08e 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -321,7 +321,11 @@ namespace ackermann_steering_controller{ } // Limit velocities and accelerations: - const double cmd_dt(period.toSec()); + const double cmd_dt = (period.toSec() > 0) ? period.toSec() : 0.0; + if(period.toSec() < 0) + { + ROS_ERROR("Invalid time interval, delta time cannot be negative"); + } limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt); limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt); diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 018bd177c..dc5892503 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -470,7 +470,11 @@ namespace diff_drive_controller{ } // Limit velocities and accelerations: - const double cmd_dt(period.toSec()); + const double cmd_dt = (period.toSec() > 0) ? period.toSec() : 0.0; + if(period.toSec() > 0 < 0) + { + ROS_ERROR("Invalid time interval, delta time cannot be negative"); + } limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt); limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt); diff --git a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp index 0c5dbd91e..b5b27e9b5 100644 --- a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp +++ b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp @@ -362,7 +362,11 @@ namespace four_wheel_steering_controller{ curr_cmd_4ws.rear_steering = 0.0; } - const double cmd_dt(period.toSec()); + const double cmd_dt = (period.toSec() > 0) ? period.toSec() : 0.0; + if(period.toSec() < 0) + { + ROS_ERROR("Invalid time interval, delta time cannot be negative"); + } const double angular_speed = odometry_.getAngular(); const double steering_track = track_-2*wheel_steering_y_offset_;