From d7dcfff54e611ec763240fcbad4af47cd18610e7 Mon Sep 17 00:00:00 2001
From: Kentaro Wada <www.kentaro.wada@gmail.com>
Date: Mon, 26 Jun 2017 23:26:04 +0900
Subject: [PATCH] Add option to reset timer when time moved backwards

---
 clients/rospy/src/rospy/timer.py | 23 +++++++++++++++++++----
 1 file changed, 19 insertions(+), 4 deletions(-)

diff --git a/clients/rospy/src/rospy/timer.py b/clients/rospy/src/rospy/timer.py
index 8856ef6f6c..87623c28cd 100644
--- a/clients/rospy/src/rospy/timer.py
+++ b/clients/rospy/src/rospy/timer.py
@@ -49,12 +49,15 @@ class Rate(object):
     Convenience class for sleeping in a loop at a specified rate
     """
     
-    def __init__(self, hz):
+    def __init__(self, hz, reset_when_time_moved_backwards=False):
         """
         Constructor.
         @param hz: hz rate to determine sleeping
         @type  hz: int
+        @param reset_when_time_moved_backwards: if True, timer is reset when rostime moved backward. [default: False]
+        @type  reset_when_time_moved_backwards: bool
         """
+        self._reset_when_time_moved_backwards = reset_when_time_moved_backwards
         # #1403
         self.last_time = rospy.rostime.get_rostime()
         self.sleep_dur = rospy.rostime.Duration(0, int(1e9/hz))
@@ -96,7 +99,15 @@ def sleep(self):
         backwards
         """
         curr_time = rospy.rostime.get_rostime()
-        sleep(self._remaining(curr_time))
+        try:
+            sleep(self._remaining(curr_time))
+        except rospy.exceptions.ROSTimeMovedBackwardsException as e:
+            if self._reset_when_time_moved_backwards:
+                rospy.logwarn('Time jumped backward, resetting timers.')
+                self.last_time = rospy.rostime.get_rostime()
+                return
+            else:
+                raise rospy.exceptions.ROSTimeMovedBackwardsException(e)
         self.last_time = self.last_time + self.sleep_dur
 
         # detect time jumping forwards, as well as loops that are
@@ -180,7 +191,7 @@ class Timer(threading.Thread):
     Convenience class for calling a callback at a specified rate
     """
 
-    def __init__(self, period, callback, oneshot=False):
+    def __init__(self, period, callback, oneshot=False, reset_when_time_moved_backwards=False):
         """
         Constructor.
         @param period: desired period between callbacks
@@ -189,11 +200,14 @@ def __init__(self, period, callback, oneshot=False):
         @type  callback: function taking rospy.TimerEvent
         @param oneshot: if True, fire only once, otherwise fire continuously until shutdown is called [default: False]
         @type  oneshot: bool
+        @param reset_when_time_moved_backwards: if True, timer is reset when rostime moved backward. [default: False]
+        @type  reset_when_time_moved_backwards: bool
         """
         super(Timer, self).__init__()
         self._period   = period
         self._callback = callback
         self._oneshot  = oneshot
+        self._reset_when_time_moved_backwards = reset_when_time_moved_backwards
         self._shutdown = False
         self.setDaemon(True)
         self.start()
@@ -205,7 +219,8 @@ def shutdown(self):
         self._shutdown = True
         
     def run(self):
-        r = Rate(1.0 / self._period.to_sec())
+        r = Rate(1.0 / self._period.to_sec(),
+                 reset_when_time_moved_backwards=self._reset_when_time_moved_backwards)
         current_expected = rospy.rostime.get_rostime() + self._period
         last_expected, last_real, last_duration = None, None, None
         while not rospy.core.is_shutdown() and not self._shutdown: