added watchdog for servoing

Author: sjavdani

src/prpy/base/mico.py

@@ -32,6 +32,8 @@
 from manipulator import Manipulator
 from std_msgs.msg import Float64
 import rospy
+from ..util import Watchdog
+from functools import partial
 
 class Mico(Manipulator):
     def __init__(self, sim,
@@ -69,11 +71,13 @@ def __init__(self, sim,
                 self.GetName(), '', self.GetArmIndices(), 0, True)
             self.servo_simulator = ServoSimulator(self, rate=20,
                                                   watchdog_timeout=0.1)
-            
         else:
-        #if not simulation, create publishers for each joint
+            #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) for topic_name in self.velocity_topic_names]
+            self.velocity_publishers = [rospy.Publisher(topic_name, Float64, queue_size=1) for topic_name in self.velocity_topic_names]
+            
+            #create watchdog to send zero velocity
+            self.servo_watchdog = Watchdog(timeout_duration=0.3, handler=lambda: self.SendVelocitiesToMico([0.,0.,0.,0.,0.,0.,]))
 
     def CloneBindings(self, parent):
         super(Mico, self).CloneBindings(parent)
@@ -106,9 +110,11 @@ def Servo(self, velocities):
             self.GetRobot().GetController().Reset(0)
             self.servo_simulator.SetVelocity(velocities)
         else:
-            for velocity_publisher,velocity in zip(self.velocity_publishers, velocities):
-                velocity_publisher.publish(velocity)
-            #raise NotImplementedError('Servo is not implemented.') 
-
+            self.SendVelocitiesToMico(velocities)
+            #reset watchdog timer
+            self.servo_watchdog.reset()
 
 
+    def SendVelocitiesToMico(self, velocities):
+        for velocity_publisher,velocity in zip(self.velocity_publishers, velocities):
+            velocity_publisher.publish(velocity)

src/prpy/util.py

@@ -497,6 +497,21 @@ def stop(self):
     def get_duration(self):
         return self.end - self.start
 
+class Watchdog(object):
+    def __init__(self, timeout_duration, handler):
+        self.timeout_duration = timeout_duration
+        self.handler = handler
+        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()
+        
+    def stop(self):
+        self.timer.cancel()
+
 
 def quadraticPlusJointLimitObjective(dq, J, dx, q, q_min, q_max, delta_joint_penalty=5e-1, lambda_dqdist=0.01, *args):
     '''