guard dt negative case and break the controller

ros-controls · May 12, 2021 · 9752e25 · 9752e25
1 parent 9bbdedd
commit 9752e25
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp
@@ -461,6 +461,13 @@ namespace diff_drive_controller{
     Commands curr_cmd = *(command_.readFromRT());
     const double dt = (time - curr_cmd.stamp).toSec();
 
+    if(dt < 0)
+    {
+      ROS_ERROR("Invalid time interval, delta time cannot be negative");
+      curr_cmd.lin = 0.0;
+      curr_cmd.ang = 0.0;
+    }
+
     // Brake if cmd_vel has timeout:
     if (dt > cmd_vel_timeout_)
     {