track_joint.py
Go to the documentation of this file.
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


pi_tracker
Author(s): Patrick Goebel
autogenerated on Tue Jan 7 2014 11:27:49