[rospy] Add option to reset timer when time moved backwards (ros#1083)

* Add option to reset timer when time moved backwards

* refactor logic

clients/rospy/src/rospy/timer.py

@@ -49,15 +49,18 @@ class Rate(object):
     Convenience class for sleeping in a loop at a specified rate
     """
 
-    def __init__(self, hz):
+    def __init__(self, hz, reset=False):
         """
         Constructor.
         @param hz: hz rate to determine sleeping
         @type  hz: int
+        @param reset: if True, timer is reset when rostime moved backward. [default: False]
+        @type  reset: bool
         """
         # #1403
         self.last_time = rospy.rostime.get_rostime()
         self.sleep_dur = rospy.rostime.Duration(0, int(1e9/hz))
+        self._reset = reset
 
     def _remaining(self, curr_time):
         """
@@ -96,7 +99,13 @@ 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:
+            if not self._reset:
+                raise
+            self.last_time = rospy.rostime.get_rostime()
+            return
         self.last_time = self.last_time + self.sleep_dur
 
         # detect time jumping forwards, as well as loops that are
@@ -180,7 +189,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=False):
         """
         Constructor.
         @param period: desired period between callbacks
@@ -189,11 +198,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: if True, timer is reset when rostime moved backward. [default: False]
+        @type  reset: bool
         """
         super(Timer, self).__init__()
         self._period   = period
         self._callback = callback
         self._oneshot  = oneshot
+        self._reset = reset
         self._shutdown = False
         self.setDaemon(True)
         self.start()
@@ -205,7 +217,7 @@ 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=self._reset)
         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: