modified watchdog to spawn fewer threads. added locks for thread safety

Author: sjavdani

src/prpy/base/mico.py

@@ -32,6 +32,7 @@
 from manipulator import Manipulator
 from std_msgs.msg import Float64
 import rospy
+import threading
 from ..util import Watchdog
 
 class Mico(Manipulator):
@@ -74,9 +75,10 @@ def __init__(self, sim,
             #if not simulation, create publishers for each joint
             self.velocity_topic_names = ['vel_j'+str(i)+'_controller/command' for i in range(1,7)]
             self.velocity_publishers = [rospy.Publisher(topic_name, Float64, queue_size=1) for topic_name in self.velocity_topic_names]
+            self.velocity_publisher_lock = threading.Lock()
 
             #create watchdog to send zero velocity
-            self.servo_watchdog = Watchdog(timeout_duration=0.25, handler=lambda: self.SendVelocitiesToMico([0.,0.,0.,0.,0.,0.,]))
+            self.servo_watchdog = Watchdog(timeout_duration=0.25, handler=self.SendVelocitiesToMico, args=[[0.,0.,0.,0.,0.,0.,]])
 
     def CloneBindings(self, parent):
         super(Mico, self).CloneBindings(parent)
@@ -115,5 +117,6 @@ def Servo(self, velocities):
 
 
     def SendVelocitiesToMico(self, velocities):
-        for velocity_publisher,velocity in zip(self.velocity_publishers, velocities):
-            velocity_publisher.publish(velocity)
+        with self.velocity_publisher_lock:
+            for velocity_publisher,velocity in zip(self.velocity_publishers, velocities):
+                velocity_publisher.publish(velocity)

src/prpy/util.py

@@ -498,19 +498,76 @@ def get_duration(self):
         return self.end - self.start
 
 class Watchdog(object):
-    def __init__(self, timeout_duration, handler):
+    '''
+    Calls specified function after duration, unless reset/stopped beforehand
+
+    @param timeout_duration how long to wait before calling handler
+    @param handler function to call after timeout_duration
+    @param args for handler
+    @param kwargs for handler
+    '''
+    def __init__(self, timeout_duration, handler, args=(), kwargs={}):
         self.timeout_duration = timeout_duration
         self.handler = handler
-        self.timer = threading.Timer(self.timeout_duration, self.handler)
-        self.timer.start()
+        self.handler_args = args
+        self.handler_kwargs = kwargs
+
+        self.thread_checking_time = threading.Thread(target=self._check_timer_loop)
+        self.timer_thread_lock = threading.Lock()
+        self.start_time = time.time()
+        self.canceled = False
+
+        self.thread_checking_time.start()
+        #self.timer = threading.Timer(self.timeout_duration, self.handler)
+        #self.timer.start()
 
     def reset(self):
-        self.timer.cancel()
-        self.timer = threading.Timer(self.timeout_duration, self.handler)
-        self.timer.start()
+        '''
+        Resets the timer
+
+        Causes the handler function to be called after the next 
+        timeout duration is reached
+
+        Also restarts the timer thread if it has existed
+        '''
+        with self.timer_thread_lock:
+            self.start_time = time.time()
+            self.canceled = False
+        if not self.thread_checking_time.is_alive():
+            self.thread_checking_time = threading.Thread(target=self._check_timer_loop)
+            self.thread_checking_time.start()
+
+        #self.timer.cancel()
+        #self.timer = threading.Timer(self.timeout_duration, self.handler)
+        #self.timer.start()
 
     def stop(self):
-        self.timer.cancel()
+        '''
+        Stop the watchdog, so it will not call handler
+        '''
+        with self.timer_thread_lock:
+          self.canceled = True
+
+    def _check_timer_loop(self):
+      '''
+      Internal function for timer thread to loop
+
+      If elapsed time has passed, calls the handler function
+      Exists if watchdog was canceled, or handler was called
+      '''
+      while True:
+          with self.timer_thread_lock:
+              if self.canceled:
+                  break
+              elapsed_time = time.time() - self.start_time
+          if elapsed_time > self.timeout_duration:
+              self.handler(*self.handler_args, **self.handler_kwargs)
+              self.canceled = True
+              break
+          else:
+              time.sleep(self.timeout_duration - elapsed_time)
+
+
 
 
 def quadraticPlusJointLimitObjective(dq, J, dx, q, q_min, q_max, delta_joint_penalty=5e-1, lambda_dqdist=0.01, *args):