00001
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
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
00103 self.servo_speed[name](self.default_joint_speed)
00104
00105
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
00111 self.torque_enable[name](False)
00112
00113
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
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