Merge pull request #258 from personalrobotics/feature/mico_servo_pub_…

…each_joint

Implement Servo() on MicoArm.

src/prpy/base/mico.py

@@ -30,6 +30,10 @@
 
 import openravepy
 from manipulator import Manipulator
+from std_msgs.msg import Float64
+import rospy
+import threading
+from ..util import Watchdog
 
 class Mico(Manipulator):
     def __init__(self, sim,
@@ -67,6 +71,14 @@ 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
+            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=self.SendVelocitiesToMico, args=[[0.,0.,0.,0.,0.,0.,]])
 
     def CloneBindings(self, parent):
         super(Mico, self).CloneBindings(parent)
@@ -99,4 +111,12 @@ def Servo(self, velocities):
             self.GetRobot().GetController().Reset(0)
             self.servo_simulator.SetVelocity(velocities)
         else:
-            raise NotImplementedError('Servo is not implemented.') 
+            self.SendVelocitiesToMico(velocities)
+            #reset watchdog timer
+            self.servo_watchdog.reset()
+
+
+    def SendVelocitiesToMico(self, velocities):
+        with self.velocity_publisher_lock:
+            for velocity_publisher,velocity in zip(self.velocity_publishers, velocities):
+                velocity_publisher.publish(velocity)

src/prpy/util.py

@@ -497,6 +497,74 @@ def stop(self):
     def get_duration(self):
         return self.end - self.start
 
+class Watchdog(object):
+    '''
+    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.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()
+
+    def reset(self):
+        '''
+        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:
+            if self.canceled or not self.thread_checking_time.is_alive():
+                self.thread_checking_time = threading.Thread(target=self._check_timer_loop)
+                self.thread_checking_time.start()
+
+            self.start_time = time.time()
+            self.canceled = False
+
+    def stop(self):
+        '''
+        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)
+              with self.timer_thread_lock:
+                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):
     '''