point_head.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 # Copyright (c) 2010, Arizona Robotics Research Group, University of Arizona
00004 # All rights reserved.
00005 # 
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 # 
00009 #     * Redistributions of source code must retain the above copyright
00010 #       notice, this list of conditions and the following disclaimer.
00011 #     * Redistributions in binary form must reproduce the above copyright
00012 #       notice, this list of conditions and the following disclaimer in the
00013 #       documentation and/or other materials provided with the distribution.
00014 #     * Neither the name of the Willow Garage, Inc. nor the names of its
00015 #       contributors may be used to endorse or promote products derived from
00016 #       this software without specific prior written permission.
00017 # 
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS'
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028 # POSSIBILITY OF SUCH DAMAGE.
00029 
00030 # Author: Anh Tran
00031 
00032 # Based on the wubble_head_action.py script by Anh Tran. Modifications made by Patrick Goebel
00033 # for the Pi Robot Project.
00034 
00035 import roslib; roslib.load_manifest('pi_head_tracking_3d_part1')
00036 import rospy
00037 import tf
00038 from geometry_msgs.msg import PointStamped
00039 from std_msgs.msg import Float64
00040 
00041 import math
00042 
00043 class PointHeadNode():
00044 
00045     def __init__(self):
00046         # Initialize new node
00047         rospy.init_node('point_head_node', anonymous=True)
00048         
00049         dynamixel_namespace = rospy.get_namespace()
00050         rate = rospy.get_param('~rate', 1)
00051         r = rospy.Rate(rate)
00052         
00053         # Initialize the target point
00054         self.target_point = PointStamped()
00055         self.last_target_point = PointStamped()
00056         
00057         # Subscribe to the target_point topic
00058         rospy.Subscriber('/target_point', PointStamped, self.update_target_point)
00059 
00060         # Initialize publisher for the pan servo
00061         self.head_pan_frame = 'head_pan_link'
00062         self.head_pan_pub = rospy.Publisher(dynamixel_namespace + 'head_pan_controller/command', Float64)
00063         
00064         # Initialize publisher for the tilt servo
00065         self.head_tilt_frame = 'head_tilt_link'
00066         self.head_tilt_pub = rospy.Publisher(dynamixel_namespace + 'head_tilt_controller/command', Float64)
00067 
00068         # Initialize tf listener
00069         self.tf = tf.TransformListener()
00070         
00071         # Make sure we can see at least the pan and tilt frames
00072         self.tf.waitForTransform(self.head_pan_frame, self.head_tilt_frame, rospy.Time(), rospy.Duration(5.0))
00073             
00074         # Reset the head position to neutral
00075         rospy.sleep(1)
00076         self.center_head()
00077         
00078         rospy.loginfo("Ready to accept target point")
00079         
00080         while not rospy.is_shutdown():
00081             rospy.wait_for_message('/target_point', PointStamped)
00082             if self.target_point == self.last_target_point:
00083                 continue
00084             try:
00085                 target_angles = self.transform_target_point(self.target_point)
00086             except (tf.Exception, tf.ConnectivityException, tf.LookupException):
00087                 rospy.logerr("tf Failure")
00088                 continue
00089                 
00090             self.head_pan_pub.publish(target_angles[0])
00091             self.head_tilt_pub.publish(target_angles[1])
00092             
00093             self.last_target_point = self.target_point
00094             rospy.loginfo("Setting Target Point:\n" + str(self.target_point))
00095             
00096             r.sleep()
00097         
00098     def update_target_point(self, msg):
00099         self.target_point = msg
00100 
00101     def center_head(self):
00102         self.head_pan_pub.publish(0.0)
00103         self.head_tilt_pub.publish(0.0)
00104         rospy.sleep(3)
00105 
00106     def transform_target_point(self, target):
00107         # Set the pan and tilt reference frames to the head_pan_frame and head_tilt_frame defined above
00108         pan_ref_frame = self.head_pan_frame
00109         tilt_ref_frame = self.head_tilt_frame
00110         
00111         # Wait for tf info (time-out in 5 seconds)
00112         self.tf.waitForTransform(pan_ref_frame, target.header.frame_id, rospy.Time(), rospy.Duration(5.0))
00113         self.tf.waitForTransform(tilt_ref_frame, target.header.frame_id, rospy.Time(), rospy.Duration(5.0))
00114 
00115         # Transform target point to pan reference frame & retrieve the pan angle
00116         pan_target = self.tf.transformPoint(pan_ref_frame, target)
00117         pan_angle = math.atan2(pan_target.point.y, pan_target.point.x)
00118 
00119         # Transform target point to tilt reference frame & retrieve the tilt angle
00120         tilt_target = self.tf.transformPoint(tilt_ref_frame, target)
00121         tilt_angle = math.atan2(tilt_target.point.z,
00122                 math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2)))
00123 
00124         return [pan_angle, tilt_angle]
00125 
00126 if __name__ == '__main__':
00127     try:
00128         point_head = PointHeadNode()
00129         rospy.spin()
00130     except rospy.ROSInterruptException:
00131         pass
00132 


pi_head_tracking_3d_part1
Author(s): Patrick Goebel
autogenerated on Mon Jan 6 2014 11:33:45