From 9752e257f88d56b9423431bcca4674550fbabc0b Mon Sep 17 00:00:00 2001
From: Gabriel Alcantara Costa Silva <gacs@blue-ocean-robotics.com>
Date: Mon, 10 May 2021 15:13:58 +0200
Subject: [PATCH] guard dt negative case and break the controller

---
 diff_drive_controller/src/diff_drive_controller.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp
index 60bf846fa..01a598cda 100644
--- 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_)
     {