$search
00001 #!/usr/bin/env python 00002 00003 """ 00004 head_track_node.py - Version 1.0 2010-12-28 00005 00006 Move the head to track a target given by (x,y) coordinates 00007 00008 Created for the Pi Robot Project: http://www.pirobot.org 00009 Copyright (c) 2010 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_tutorial') 00025 import rospy 00026 from sensor_msgs.msg import JointState, RegionOfInterest, CameraInfo 00027 from math import radians 00028 00029 class head_track(): 00030 def __init__(self): 00031 rospy.init_node("head_track") 00032 rospy.on_shutdown(self.shutdown) 00033 00034 """ Publish the movement commands on the /cmd_joints topic using the 00035 JointState message type. """ 00036 self.head_pub = rospy.Publisher("/cmd_joints", JointState) 00037 00038 self.rate = rospy.get_param("~rate", 10) 00039 00040 """ The pan/tilt thresholds indicate how many pixels the ROI needs to be off-center 00041 before we make a movement. """ 00042 self.pan_threshold = int(rospy.get_param("~pan_threshold", 5)) 00043 self.tilt_threshold = int(rospy.get_param("~tilt_threshold", 5)) 00044 00045 """ The k_pan and k_tilt parameter determine how responsive the servo movements are. 00046 If these are set too high, oscillation can result. """ 00047 self.k_pan = rospy.get_param("~k_pan", 7.0) 00048 self.k_tilt = rospy.get_param("~k_tilt", 5.0) 00049 00050 self.max_pan = rospy.get_param("~max_pan", radians(145)) 00051 self.min_pan = rospy.get_param("~min_pan", radians(-145)) 00052 self.max_tilt = rospy.get_param("~max_tilt", radians(90)) 00053 self.min_tilt = rospy.get_param("~min_tilt", radians(-90)) 00054 00055 r = rospy.Rate(self.rate) 00056 00057 self.head_cmd = JointState() 00058 self.joints = ["head_pan_joint", "head_tilt_joint"] 00059 self.head_cmd.name = self.joints 00060 self.head_cmd.position = [0, 0] 00061 self.head_cmd.velocity = [1, 1] 00062 self.head_cmd.header.stamp = rospy.Time.now() 00063 self.head_cmd.header.frame_id = 'head_pan_joint' 00064 00065 """ Center the head and pan servos at the start. """ 00066 for i in range(3): 00067 self.head_pub.publish(self.head_cmd) 00068 rospy.sleep(1) 00069 00070 self.tracking_seq = 0 00071 self.last_tracking_seq = -1 00072 00073 rospy.Subscriber('roi', RegionOfInterest, self.setPanTiltSpeeds) 00074 rospy.Subscriber('camera_info', CameraInfo, self.getCameraInfo) 00075 00076 while not rospy.is_shutdown(): 00077 """ Publish the pan/tilt movement commands. """ 00078 self.head_cmd.header.stamp = rospy.Time.now() 00079 self.head_cmd.header.frame_id = 'head_pan_joint' 00080 if self.last_tracking_seq == self.tracking_seq: 00081 self.head_cmd.velocity = [0.0001, 0.0001] 00082 else: 00083 self.last_tracking_seq = self.tracking_seq 00084 self.head_pub.publish(self.head_cmd) 00085 r.sleep() 00086 00087 def setPanTiltSpeeds(self, msg): 00088 """ When OpenCV loses the ROI, the message stops updating. Use this counter to 00089 determine when it stops. """ 00090 self.tracking_seq += 1 00091 00092 """ Check to see if we have lost the ROI. """ 00093 if msg.width == 0 or msg.height == 0 or msg.width > self.image_width / 2 or \ 00094 msg.height > self.image_height / 2: 00095 self.head_cmd.velocity = [0, 0] 00096 return 00097 00098 """ Compute the center of the ROI """ 00099 COG_x = msg.x_offset + msg.width / 2 - self.image_width / 2 00100 COG_y = msg.y_offset + msg.height / 2 - self.image_height / 2 00101 00102 """ Pan the camera only if the displacement of the COG exceeds the threshold. """ 00103 if abs(COG_x) > self.pan_threshold: 00104 """ Set the pan speed proportion to the displacement of the horizontal displacement 00105 of the target. """ 00106 self.head_cmd.velocity[0] = self.k_pan * abs(COG_x) / float(self.image_width) 00107 00108 """ Set the target position to one of the min or max positions--we'll never 00109 get there since we are tracking using speed. """ 00110 if COG_x > 0: 00111 self.head_cmd.position[0] = self.min_pan 00112 else: 00113 self.head_cmd.position[0] = self.max_pan 00114 else: 00115 self.head_cmd.velocity[0] = 0.0001 00116 00117 """ Tilt the camera only if the displacement of the COG exceeds the threshold. """ 00118 if abs(COG_y) > self.tilt_threshold: 00119 """ Set the tilt speed proportion to the displacement of the vertical displacement 00120 of the target. """ 00121 self.head_cmd.velocity[1] = self.k_tilt * abs(COG_y) / float(self.image_height) 00122 00123 """ Set the target position to one of the min or max positions--we'll never 00124 get there since we are tracking using speed. """ 00125 if COG_y < 0: 00126 self.head_cmd.position[1] = self.min_tilt 00127 else: 00128 self.head_cmd.position[1] = self.max_tilt 00129 else: 00130 self.head_cmd.velocity[1] = 0.0001 00131 00132 def getCameraInfo(self, msg): 00133 self.image_width = msg.width 00134 self.image_height = msg.height 00135 00136 def shutdown(self): 00137 rospy.loginfo("Shutting down head tracking node...") 00138 00139 if __name__ == '__main__': 00140 try: 00141 head_track() 00142 except rospy.ROSInterruptException: 00143 rospy.loginfo("Head tracking node is shut down.") 00144 00145 00146 00147