guard dt negative case and break the controller

ros-controls · May 17, 2021 · d967780 · d967780
1 parent 9bbdedd
commit d967780
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 0 deletions.
diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/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 --git a/diff_drive_controller/src/diff_drive_controller.cpp b/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);

diff --git a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp
@@ -362,6 +362,12 @@ 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.lin = 0.0;
+      curr_cmd.ang = 0.0;
+    }
 
     const double angular_speed = odometry_.getAngular();
     const double steering_track = track_-2*wheel_steering_y_offset_;