$search
00001 #! /usr/bin/python 00002 import roslib 00003 roslib.load_manifest('pr2_pan_tilt') 00004 00005 import rospy 00006 import actionlib 00007 import math 00008 from actionlib_msgs.msg import * 00009 from pr2_controllers_msgs.msg import * 00010 from geometry_msgs.msg import * 00011 00012 import tf 00013 00014 00015 def handle_android_pose(msg, listener): 00016 br = tf.TransformBroadcaster() 00017 pose = msg.pose 00018 tran = False 00019 try: 00020 (trans,rot) = listener.lookupTransform('/base_link', '/wide_stereo_optical_frame', rospy.Time(0)) 00021 tran = trans 00022 except (tf.LookupException, tf.ConnectivityException): 00023 rospy.logwarn("No transform from base_link to head - application not working") 00024 return 00025 if (tran == False): 00026 rospy.logerr("Unknown transformation problem") 00027 return 00028 00029 br.sendTransform(trans, (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w), 00030 rospy.Time.now(), "head_goal", "base_link") 00031 00032 g = PointHeadGoal() 00033 g.target.header.frame_id = 'head_goal' 00034 g.target.point.x = 0.0 00035 g.target.point.y = 0.0 00036 g.target.point.z = -1.0 00037 g.min_duration = rospy.Duration(1.0) 00038 00039 client.send_goal(g) 00040 00041 00042 00043 if __name__ == '__main__': 00044 rospy.init_node('move_the_head', anonymous=True) 00045 listener = tf.TransformListener() 00046 client = actionlib.SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction) 00047 client.wait_for_server() 00048 rospy.Subscriber("/android/orientation", PoseStamped, handle_android_pose, listener) 00049 rospy.spin()