00001
00002
00003 """
00004 Track a given skeleton joint by panning and tilting the Kinect.
00005
00006 Based on Taylor Veltrop's C++ work (http://www.ros.org/wiki/veltrop-ros-pkg)
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_tracker')
00025 import rospy
00026 import demo_tracker_lib as PTL
00027 from sensor_msgs.msg import JointState, CameraInfo
00028 from demo_tracker.msg import Skeleton
00029 from demo_tracker.srv import *
00030 from std_msgs.msg import Float64
00031 import PyKDL as KDL
00032 from ax12_controller_core.srv import SetSpeed
00033 from math import acos, asin, atan, atan2, pi, radians
00034
00035 class TrackJoint():
00036 def __init__(self):
00037 rospy.init_node('tracker_joint_controller')
00038 rospy.on_shutdown(self.shutdown)
00039
00040 rospy.loginfo("Initializing Head Tracking Node...")
00041
00042 self.image_width = 320
00043 self.image_height = 240
00044
00045 self.rate = rospy.get_param('~head_tracking_rate', 5)
00046 r = rospy.Rate(self.rate)
00047
00048 self.joint_to_track = rospy.get_param('~joint_to_track', 'head')
00049
00050 self.mirror_skeleton = rospy.get_param('~mirror_skeleton', True)
00051
00052 self.default_joint_speed = rospy.get_param('~default_joint_speed', 0.3)
00053 self.max_head_speed = rospy.get_param('~max_head_speed', 1.0)
00054 self.tracker_commands = rospy.get_param('~tracker_commands', ['STOP'])
00055
00056 self.servo_speed = dict()
00057 ax12_namespace = '/ax12_controller'
00058 dynamixels = rospy.get_param(ax12_namespace+'/dynamixels', dict())
00059 for name in sorted(dynamixels):
00060 rospy.loginfo(name)
00061 try:
00062 rospy.wait_for_service(ax12_namespace+'/'+name+'_controller/set_speed')
00063 self.servo_speed[name] = rospy.ServiceProxy(ax12_namespace+'/'+name+'_controller/set_speed', SetSpeed)
00064 self.servo_speed[name](self.default_joint_speed)
00065 except:
00066 pass
00067 if self.mirror_skeleton:
00068 self.mirror = -1.0
00069 else:
00070 self.mirror = 1.0
00071
00072 self.HALF_PI = pi / 2.0
00073
00074
00075 rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler)
00076
00077
00078 self.skeleton = dict()
00079 self.skeleton['confidence'] = dict()
00080 self.skeleton['position'] = dict()
00081 self.skeleton['orientation'] = dict()
00082
00083
00084 self.set_command = rospy.Service('~set_command', SetCommand, self.set_command_callback)
00085
00086
00087 self.head_tilt_pub = rospy.Publisher('/ax12_controller/head_tilt_controller/command', Float64)
00088 self.head_pan_pub = rospy.Publisher('/ax12_controller/head_pan_controller/command', Float64)
00089
00090
00091 self.cmd_joints = JointState()
00092 self.cmd_joints = PTL.get_joints()
00093
00094
00095 self.last_cmd_joints = JointState()
00096 self.last_cmd_joints = PTL.get_joints()
00097
00098
00099 self.tracker_command = "STOP"
00100
00101 """ The pan/tilt thresholds indicate how many pixels the ROI needs to be off-center
00102 before we make a movement. """
00103 self.pan_threshold = int(rospy.get_param("~pan_threshold", 5))
00104 self.tilt_threshold = int(rospy.get_param("~tilt_threshold", 5))
00105
00106 """ The k_pan and k_tilt parameter determine how responsive the servo movements are.
00107 If these are set too high, oscillation can result. """
00108 self.k_pan = rospy.get_param("~k_pan", 0.5)
00109 self.k_tilt = rospy.get_param("~k_tilt", 0.5)
00110
00111 self.max_pan = rospy.get_param("~max_pan", radians(145))
00112 self.min_pan = rospy.get_param("~min_pan", radians(-145))
00113 self.max_tilt = rospy.get_param("~max_tilt", radians(90))
00114 self.min_tilt = rospy.get_param("~min_tilt", radians(-90))
00115
00116 """ Use a JointState message to store the pan and tilt speeds """
00117 self.head_cmd = JointState()
00118 self.head_cmd.position = [0, 0]
00119 self.head_cmd.velocity = [1, 1]
00120
00121 """ Center the head and pan servos at the start. """
00122 self.servo_speed['head_pan'](self.head_cmd.velocity[0])
00123 self.servo_speed['head_tilt'](self.head_cmd.velocity[1])
00124 self.head_pan_pub.publish(self.head_cmd.position[0])
00125 self.head_tilt_pub.publish(self.head_cmd.position[1])
00126
00127 rospy.sleep(5)
00128 rospy.loginfo("READY TO TRACK!")
00129
00130 self.tracking_seq = 0
00131 self.last_tracking_seq = 0
00132
00133
00134
00135 while not rospy.is_shutdown():
00136 if self.last_tracking_seq == self.tracking_seq:
00137 rospy.loginfo("LOST SKELETON!")
00138 self.head_cmd.velocity = [0.01, 0.01]
00139
00140 else:
00141 self.last_tracking_seq = self.tracking_seq
00142 self.servo_speed['head_pan'](self.head_cmd.velocity[0])
00143 self.servo_speed['head_tilt'](self.head_cmd.velocity[1])
00144 self.head_pan_pub.publish(self.head_cmd.position[0])
00145 self.head_tilt_pub.publish(self.head_cmd.position[1])
00146 rospy.loginfo(self.head_cmd)
00147 r.sleep()
00148
00149
00150
00151
00152
00153 def stop_joints(self):
00154 self.cmd_joints = self.last_cmd_joints
00155
00156 def relax_joints(self):
00157 pass
00158
00159 def reset_joints(self):
00160 pass
00161
00162 def enable_joints(self):
00163 pass
00164
00165 def skeleton_handler(self, msg):
00166 self.skeleton = dict()
00167 self.skeleton['confidence'] = dict()
00168 self.skeleton['position'] = dict()
00169 self.skeleton['orientation'] = dict()
00170 self.cmd_joints.header.frame_id = msg.header.frame_id
00171 confident = False
00172 for joint in msg.name:
00173 self.skeleton['confidence'][joint] = msg.confidence[msg.name.index(joint)]
00174 if joint == 'neck' and self.skeleton['confidence'][joint] > 0.5:
00175 confident = True
00176 self.skeleton['position'][joint] = KDL.Vector(msg.position[msg.name.index(joint)].x, msg.position[msg.name.index(joint)].y, msg.position[msg.name.index(joint)].z)
00177 self.skeleton['orientation'][joint] = KDL.Rotation.Quaternion(msg.orientation[msg.name.index(joint)].x, msg.orientation[msg.name.index(joint)].y, msg.orientation[msg.name.index(joint)].z, msg.orientation[msg.name.index(joint)].w)
00178
00179 """ Check to see if we have lost the skeleton. """
00180 if not confident:
00181 rospy.loginfo("LOST SKELETON!")
00182
00183 self.head_cmd.velocity = [0.01, 0.01]
00184 return
00185
00186 """Use this counter to determine when we still have the skeleton """
00187 self.tracking_seq += 1
00188
00189 """ Compute the center of the ROI """
00190 COG_x = -(self.skeleton['position'][self.joint_to_track].x() * 1000 - self.image_width / 2)
00191 COG_y = -(self.skeleton['position'][self.joint_to_track].y() * 1000 - self.image_height / 2)
00192
00193
00194
00195
00196 """ Pan the camera only if the displacement of the COG exceeds the threshold. """
00197 if abs(COG_x) > self.pan_threshold:
00198 """ Set the pan speed proportion to the displacement of the horizontal displacement
00199 of the target. """
00200 self.head_cmd.velocity[0] = min(self.max_head_speed, self.k_pan * abs(COG_x) / float(self.image_width))
00201
00202 """ Set the target position to one of the min or max positions--we'll never
00203 get there since we are tracking using speed. """
00204 if COG_x > 0:
00205 self.head_cmd.position[0] = self.min_pan
00206 else:
00207 self.head_cmd.position[0] = self.max_pan
00208 else:
00209 self.head_cmd.velocity[0] = 0.0
00210
00211 """ Tilt the camera only if the displacement of the COG exceeds the threshold. """
00212 if abs(COG_y) > self.tilt_threshold:
00213 """ Set the tilt speed proportion to the displacement of the vertical displacement
00214 of the target. """
00215 self.head_cmd.velocity[1] = min(self.max_head_speed, self.k_tilt * abs(COG_y) / float(self.image_height))
00216
00217 """ Set the target position to one of the min or max positions--we'll never
00218 get there since we are tracking using speed. """
00219 if COG_y < 0:
00220 self.head_cmd.position[1] = self.min_tilt
00221 else:
00222 self.head_cmd.position[1] = self.max_tilt
00223 else:
00224 self.head_cmd.velocity[1] = 0.0
00225
00226 def joint_state_handler(self, msg):
00227 for joint in msg.name:
00228 self.joint_state = msg
00229
00230 def set_command_callback(self, req):
00231 self.tracker_command = req.command
00232 return SetCommandResponse()
00233
00234 def shutdown(self):
00235 rospy.loginfo('Shutting down Track Joint Controller Node.')
00236
00237 if __name__ == '__main__':
00238 try:
00239 TrackJoint()
00240 except rospy.ROSInterruptException:
00241 pass