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