Go to the documentation of this file.00001
00002 import roslib
00003 roslib.load_manifest('pr2_omni_teleop')
00004
00005 import rospy
00006 import tf
00007 import numpy as np
00008 import math
00009 import actionlib
00010 from pr2_controllers_msgs.msg import *
00011 import tf.transformations as tr
00012 import hrl_lib.tf_utils as tfu
00013 from geometry_msgs.msg import Point
00014
00015 class TrackGrippers:
00016
00017 def __init__(self):
00018 self.head_client = actionlib.SimpleActionClient(\
00019 'head_traj_controller/point_head_action',\
00020 PointHeadAction)
00021 self.tflistener = tf.TransformListener()
00022
00023 def run(self):
00024 r = rospy.Rate(50)
00025 while not rospy.is_shutdown():
00026 try:
00027 common_link = '/base_link'
00028 c_T_rgrip = tfu.transform(common_link, '/r_gripper_tool_frame', self.tflistener)
00029 c_T_lgrip = tfu.transform(common_link, '/l_gripper_tool_frame', self.tflistener)
00030 gripper_right_c = np.matrix(tr.translation_from_matrix(c_T_rgrip * tr.translation_matrix([0, 0, 0.])))
00031 gripper_left_c = np.matrix(tr.translation_from_matrix(c_T_lgrip * tr.translation_matrix([0, 0, 0.])))
00032 look_at_point_c = ((gripper_right_c + gripper_left_c) / 2.0).A1.tolist()
00033
00034 g = PointHeadGoal()
00035 g.target.header.frame_id = '/base_link'
00036 g.target.point = Point(*look_at_point_c)
00037 g.min_duration = rospy.Duration(1.0)
00038 g.max_velocity = 10.
00039 self.head_client.send_goal(g)
00040 self.head_client.wait_for_result(rospy.Duration(1.))
00041 r.sleep()
00042 except tf.LookupException, e:
00043 print e
00044
00045
00046 if __name__ == '__main__':
00047 rospy.init_node('teleop_track_gripper')
00048 tg = TrackGrippers()
00049 tg.run()