guard dt negative case and break the controller

Author: Gabriel Alcantara Costa Silva

ackermann_steering_controller/src/ackermann_steering_controller.cpp

@@ -322,6 +322,12 @@ namespace ackermann_steering_controller{
 
     // Limit velocities and accelerations:
     const double cmd_dt(period.toSec());
+    if(cmd_dt < 0)
+    {
+      ROS_ERROR("Invalid time interval, delta time cannot be negative");
+      curr_cmd.lin = 0.0;
+      curr_cmd.ang = 0.0;
+    }
 
     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_drive_controller/src/diff_drive_controller.cpp

@@ -470,6 +470,12 @@ namespace diff_drive_controller{
 
     // Limit velocities and accelerations:
     const double cmd_dt(period.toSec());
+    if(cmd_dt < 0)
+    {
+      ROS_ERROR("Invalid time interval, delta time cannot be negative");
+      curr_cmd.lin = 0.0;
+      curr_cmd.ang = 0.0;
+    }
 
     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);

four_wheel_steering_controller/src/four_wheel_steering_controller.cpp

@@ -362,6 +362,16 @@ namespace four_wheel_steering_controller{
     }
 
     const double cmd_dt(period.toSec());
+    if(cmd_dt < 0)
+    {
+      ROS_ERROR("Invalid time interval, delta time cannot be negative");
+      curr_cmd_twist.lin_x = 0.0;
+      curr_cmd_twist.lin_y = 0.0;
+      curr_cmd_twist.ang = 0.0;
+      curr_cmd_4ws.lin = 0.0;
+      curr_cmd_4ws.front_steering = 0.0;
+      curr_cmd_4ws.rear_steering = 0.0;
+    }
 
     const double angular_speed = odometry_.getAngular();
     const double steering_track = track_-2*wheel_steering_y_offset_;