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


pi_head_tracking_tutorial
Author(s): Patrick Goebel
autogenerated on Mon Jan 6 2014 11:34:04