$search
00001 #!/usr/bin/env python 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 # Subscribe to the skeleton topic. 00075 rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler) 00076 00077 # Store the current skeleton configuration in a local dictionary. 00078 self.skeleton = dict() 00079 self.skeleton['confidence'] = dict() 00080 self.skeleton['position'] = dict() 00081 self.skeleton['orientation'] = dict() 00082 00083 # Set up the tracker command service for this node. 00084 self.set_command = rospy.Service('~set_command', SetCommand, self.set_command_callback) 00085 00086 # The joints are controlled by publishing joint positions on the ax12 controller topics. 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 # The get_joints command parses a URDF description of the robot to get all the non-fixed joints. 00091 self.cmd_joints = JointState() 00092 self.cmd_joints = PTL.get_joints() 00093 00094 # Store the last joint command so we can stop and hold a given posture. 00095 self.last_cmd_joints = JointState() 00096 self.last_cmd_joints = PTL.get_joints() 00097 00098 # Initialize the robot in the stopped state. 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 #rospy.Subscriber('/camera/camera_info', CameraInfo, self.getCameraInfo) 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 #self.head_cmd.position = [0, 0] 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 # def getCameraInfo(self, msg): 00150 # self.image_width = msg.width 00151 # self.image_height = msg.height 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 #self.head_cmd.position = [0, 0] 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 #rospy.loginfo("COG_X, COG_Y: " + str(COG_x) + " " + str(COG_y)) 00194 #rospy.loginfo("X, Y: " + str(self.skeleton['position'][self.joint_to_track].x()) + " " + str(self.skeleton['position'][self.joint_to_track].y())) 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