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


pr2_pan_tilt
Author(s): Tony Pratkanis
autogenerated on Fri Dec 6 2013 22:10:39