pi_head_tracking_3d_part2: pi_head_tracking_3d_part2: tf_head

00001 #!/usr/bin/env python
00002 
00003 """
00004     tf_head_tracker.py - Version 1.0 2011-08-01
00005     
00006     Move the head to track a PointStamped target on the /target_point topic.
00007     
00008     Created for the Pi Robot Project: http://www.pirobot.org
00009     Copyright (c) 2011 Patrick Goebel.  All rights reserved.
00010 
00011     This program is free software; you can redistribute it and/or modify
00012     it under the terms of the GNU General Public License as published by
00013     the Free Software Foundation; either version 2 of the License, or
00014     (at your option) any later version.
00015     
00016     This program is distributed in the hope that it will be useful,
00017     but WITHOUT ANY WARRANTY; without even the implied warranty of
00018     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019     GNU General Public License for more details at:
00020     
00021     http://www.gnu.org/licenses/gpl.html
00022 """
00023 
00024 import roslib; roslib.load_manifest('pi_head_tracking_3d_part2')
00025 import rospy
00026 import tf
00027 from std_msgs.msg import Float64
00028 from dynamixel_controllers.srv import *
00029 from geometry_msgs.msg import PointStamped, Point
00030 from sensor_msgs.msg import JointState
00031 from math import radians, sqrt
00032 import sys
00033 
00034 """ A speed of exactly 0 has a special meaning for Dynamixel servos--namely, "move as fast as you can".
00035     This can have some very undesirable consequences since it is the complete opposite of what 0 normally
00036     means.  So we define a very small speed value to represent zero speed.
00037 """
00038 ZERO_SPEED = 0.0001
00039 
00040 class tfTracker():
00041     def __init__(self):
00042         rospy.init_node('tf_head_tracker')
00043         
00044         rospy.on_shutdown(self.shutdown)
00045         
00046         """ How fast should we update the servos? """
00047         self.rate = rospy.get_param('~rate', 10)
00048         r = rospy.Rate(self.rate)
00049         
00050         """ Joint speeds are given in radians per second """
00051         self.default_joint_speed = rospy.get_param('~default_joint_speed', 0.3)
00052         self.max_joint_speed = rospy.get_param('~max_joint_speed', 0.5)
00053         
00054         """ How far ahead or behind the target (in radians) should we aim for? """
00055         self.lead_target_angle = rospy.get_param('~lead_target_angle', 0.5)
00056         
00057         """ How long (in seconds) should we permit the target to be lost before re-centering the servos? """
00058         self.target_timeout = 3.0
00059         self.target_lost = False
00060         self.servos_centered = False
00061 
00062         """ Remap these in the launch file or command line if necessary """
00063         self.camera_link = 'kinect_link'
00064         self.head_pan_joint = 'head_pan_joint'
00065         self.head_tilt_joint = 'head_tilt_joint'
00066         self.head_pan_link = 'head_pan_link'
00067         self.head_tilt_link = 'head_tilt_link'
00068         
00069         self.dynamixels = rospy.get_param('dynamixels', '')
00070         
00071         """ The pan/tilt thresholds indicate how far (in meters) the ROI needs to be off-center
00072             before we make a movement. """
00073         self.pan_threshold = int(rospy.get_param('~pan_threshold', 0.01))
00074         self.tilt_threshold = int(rospy.get_param('~tilt_threshold', 0.01))
00075         
00076         """ The k_pan and k_tilt parameter determine how responsive the servo movements are.
00077             If these are set too high, oscillation can result. """
00078         self.k_pan = rospy.get_param('~k_pan', 1.5)
00079         self.k_tilt = rospy.get_param('~k_tilt', 1.5)
00080         
00081         """ Set limits on how far we can pan or tilt """
00082         self.max_pan = rospy.get_param('~max_pan', radians(145))
00083         self.min_pan = rospy.get_param('~min_pan', radians(-145))
00084         self.max_tilt = rospy.get_param('~max_tilt', radians(90))
00085         self.min_tilt = rospy.get_param('~min_tilt', radians(-90))
00086         
00087         self.servo_speed = dict()
00088         self.servo_position = dict()
00089         self.torque_enable = dict()
00090 
00091         """ Connect to the set_speed and torque_enable services for each servo.
00092             Also define a position publisher for each servo. """
00093         for name in sorted(self.dynamixels):
00094             try:
00095                 controller = name.replace("_joint", "") + "_controller"
00096 
00097                 # The set_speed services
00098                 set_speed_service = '/' + controller + '/set_speed'
00099                 rospy.wait_for_service(set_speed_service)  
00100                 self.servo_speed[name] = rospy.ServiceProxy(set_speed_service, SetSpeed, persistent=True)
00101                 
00102                 # Initialize the servo speed to the default_joint_speed
00103                 self.servo_speed[name](self.default_joint_speed)
00104                 
00105                 # Torque enable/disable control for each servo
00106                 torque_service = '/' + controller + '/torque_enable'
00107                 rospy.wait_for_service(torque_service) 
00108                 self.torque_enable[name] = rospy.ServiceProxy(torque_service, TorqueEnable)
00109                 
00110                 # Start each servo in the disabled state so we can move them by hand
00111                 self.torque_enable[name](False)
00112 
00113                 # The position controllers
00114                 self.servo_position[name] = rospy.Publisher('/' + controller + '/command', Float64)
00115             except:
00116                 rospy.loginfo("Can't contact servo services!")
00117         
00118         rospy.loginfo("TF Tracker node started. Centering servos...")
00119         
00120         self.pan_position = 0
00121         self.tilt_position = 0
00122         self.pan_speed = ZERO_SPEED
00123         self.tilt_speed = ZERO_SPEED
00124         
00125         self.last_tilt_speed = 0
00126         self.last_pan_speed = 0
00127 
00128         """ Use a counter to detect when we have lost the target """
00129         self.tracking_seq = 0
00130         self.last_tracking_seq = -1
00131         self.target_lost_count = 0
00132         self.max_target_lost_count = self.rate * 5
00133     
00134         """ Center the pan and tilt servos at the start. """
00135         self.center_head_servos()
00136         
00137         """ Initialize tf listener """
00138         self.tf = tf.TransformListener()
00139         
00140         """ Make sure we can see the camera and head pan links """
00141         self.tf.waitForTransform(self.camera_link, self.head_pan_link, rospy.Time(), rospy.Duration(5.0))
00142         
00143         """ Wait also for the joint_states topic so we can track our own joint positions """
00144         rospy.wait_for_message('joint_states', JointState)
00145         self.joint_state = JointState()
00146         rospy.Subscriber('joint_states', JointState, self.update_joint_state)
00147         
00148         """ Subscribe to the target point topic """
00149         rospy.Subscriber('target_point', PointStamped, self.update_head_position)
00150                 
00151         while not rospy.is_shutdown():
00152             if self.last_tracking_seq == self.tracking_seq:
00153                 self.pan_speed = ZERO_SPEED
00154                 self.tilt_speed = ZERO_SPEED
00155                 self.target_lost_count += 1
00156             else:
00157                 self.last_tracking_seq = self.tracking_seq
00158                 self.target_lost_count = 0
00159                     
00160             if self.target_lost_count > self.max_target_lost_count:
00161                 self.center_head_servos()
00162 
00163             else:               
00164                 try:
00165                     """ Only update the pan speed if it differs from the last value """
00166                     if self.last_pan_speed != self.pan_speed:
00167                         self.servo_speed[self.head_pan_joint](self.pan_speed)
00168                         self.last_pan_speed = self.pan_speed
00169                     self.servo_position[self.head_pan_joint].publish(self.pan_position)
00170                 except:
00171                     """ If we run into any exceptions, mometarily stop the head movement by setting
00172                     the target pan position to the current position. """
00173                     try:
00174                         current_pan_position = self.joint_state.position[self.joint_state.name.index(self.head_pan_joint)]
00175                         self.servo_position[self.head_pan_joint].publish(current_pan_position)
00176                         rospy.loginfo("Servo SetSpeed Exception!")
00177                         rospy.loginfo(sys.exc_info())
00178                     except:
00179                         pass
00180                                  
00181                 try:
00182                     """ Only update the tilt speed if it differs from the last value """
00183                     if self.last_tilt_speed != self.tilt_speed:
00184                         self.servo_speed[self.head_tilt_joint](self.tilt_speed)
00185                         self.last_tilt_speed = self.tilt_speed
00186                     self.servo_position[self.head_tilt_joint].publish(self.tilt_position)
00187                 except:
00188                     """ If we run into any exceptions, mometarily stop the head movement by setting
00189                     the target tilt position to the current position. """
00190                     try:
00191                         current_tilt_position = self.joint_state.position[self.joint_state.name.index(self.head_tilt_joint)]
00192                         self.servo_position[self.head_tilt_joint].publish(current_tilt_position)
00193                         rospy.loginfo("Servo SetSpeed Exception!")
00194                         rospy.loginfo(sys.exc_info())
00195                     except:
00196                         pass
00197                                     
00198             r.sleep()
00199             
00200     def center_head_servos(self):
00201         try:
00202             self.servo_speed[self.head_pan_joint](self.default_joint_speed)
00203             self.servo_speed[self.head_tilt_joint](self.default_joint_speed)
00204             for i in range(3):
00205                 self.servo_position[self.head_pan_joint].publish(0)
00206                 self.servo_position[self.head_tilt_joint].publish(0)
00207                 rospy.sleep(1)
00208         except:
00209             pass
00210         
00211     def update_joint_state(self, msg):
00212         self.joint_state = msg
00213     
00214     def update_head_position(self, target):
00215         """ We increment a tracking counter upon receiving a target message so we can use the counter to
00216             determine when we have lost the target. """
00217         self.tracking_seq += 1
00218         
00219         """ Project the target point onto the camera link.
00220             In case of tf exceptions, simply return without an update. """
00221         try:         
00222             self.tf.waitForTransform(self.camera_link, target.header.frame_id, rospy.Time.now(), rospy.Duration(1.0))
00223             camera_target = self.tf.transformPoint(self.camera_link, target)
00224         except (tf.Exception, tf.ConnectivityException, tf.LookupException):
00225             return
00226         
00227         """ The virtual camera image is in the y-z plane """
00228         pan = -camera_target.point.y
00229         tilt = -camera_target.point.z
00230         
00231         """ Compute the distance to the target in the x direction """
00232         distance = float(abs(camera_target.point.x))
00233         
00234         """ Convert the pan and tilt values from meters to radians by dividing by the distance to the target.  Since the Kinect is 
00235             blind to distance within 0.5 meters, check for an exception and use 0.5 meters as a fall back. """
00236         try:
00237             pan /= distance
00238             tilt /= distance
00239         except:
00240             pan /= 0.5
00241             tilt /= 0.5
00242                       
00243         """ Pan the camera only if the displacement of the target point exceeds the threshold """
00244         if abs(pan) > self.pan_threshold:
00245             """ Set the pan speed proportion to the horizontal displacement of the target """
00246             self.pan_speed = trunc(min(self.max_joint_speed, max(ZERO_SPEED, self.k_pan * abs(pan))), 2)
00247                
00248             """ Set the target position ahead or behind the current position """
00249             try:
00250                 current_pan = self.joint_state.position[self.joint_state.name.index(self.head_pan_joint)]
00251             except:
00252                 return
00253             if pan > 0:
00254                 self.pan_position = max(self.min_pan, current_pan - self.lead_target_angle)
00255             else:
00256                 self.pan_position = min(self.max_pan, current_pan + self.lead_target_angle)
00257         else:
00258             self.pan_speed = ZERO_SPEED
00259         
00260         """ Tilt the camera only if the displacement of the target point exceeds the threshold """
00261         if abs(tilt) > self.tilt_threshold:
00262             """ Set the pan speed proportion to the vertical displacement of the target """
00263             self.tilt_speed = trunc(min(self.max_joint_speed, max(ZERO_SPEED, self.k_tilt * abs(tilt))), 2)
00264             
00265             """ Set the target position ahead or behind the current position """
00266             try:
00267                 current_tilt = self.joint_state.position[self.joint_state.name.index(self.head_tilt_joint)]
00268             except:
00269                 return
00270             if tilt < 0:
00271                 self.tilt_position = max(self.min_tilt, current_tilt - self.lead_target_angle)
00272             else:
00273                 self.tilt_position = min(self.max_tilt, current_tilt + self.lead_target_angle)
00274 
00275         else:
00276             self.tilt_speed = ZERO_SPEED
00277         
00278     def shutdown(self):
00279         rospy.loginfo("Shutting down frame tracking node...")
00280         self.center_head_servos()
00281         
00282         # Relax all servos to give them a rest.
00283         for servo in self.dynamixels:
00284             self.torque_enable[servo](False)
00285         
00286 def trunc(f, n):
00287     '''Truncates/pads a float f to n decimal places without rounding'''
00288     slen = len('%.*f' % (n, f))
00289     return float(str(f)[:slen])
00290 
00291                    
00292 if __name__ == '__main__':
00293     try:
00294         tracker = tfTracker()
00295         rospy.spin()
00296     except rospy.ROSInterruptException:
00297         rospy.loginfo("TF tracking node is shut down.")
00298 
00299 
00300 
00301