Go to the documentation of this file.00001
00002
00003 import argparse
00004
00005 import roslib;roslib.load_manifest('hrl_face_adls')
00006 import rospy
00007 from geometry_msgs.msg import PoseStamped, Quaternion
00008 from tf import transformations as trans
00009
00010 import pose_utils as pu
00011
00012 class ClickedPoseRelay(object):
00013 def __init__(self, translation, quaternion):
00014 """Setup pub/subs and transform parameters"""
00015 self.pose_sub = rospy.Subscriber('pose_in',
00016 PoseStamped,
00017 self.pose_in_cb)
00018 self.pose_pub = rospy.Publisher('pose_out', PoseStamped)
00019 self.offset_x = translation[0]
00020 self.offset_y = translation[1]
00021 self.offset_z = translation[2]
00022 self.quat_offset = quaternion
00023 rospy.loginfo('['+rospy.get_name()[1:]+']'+
00024 'Pose relay node started with:'+
00025 '\r\nTranslation: '+ str(translation) +
00026 '\r\nRotation: '+ str(quaternion))
00027
00028 def pose_in_cb(self, ps_in):
00029 """Apply transform to received pose and republish"""
00030 trans_pose = pu.pose_relative_trans(ps_in, self.offset_x,
00031 self.offset_y, self.offset_z)
00032
00033 ps_out = PoseStamped()
00034 ps_out.header.frame_id = ps_in.header.frame_id
00035 ps_out.header.stamp = ps_in.header.stamp
00036 ps_out.pose.position = trans_pose.pose.position
00037
00038 quat_in = (ps_in.pose.orientation.x, ps_in.pose.orientation.y,
00039 ps_in.pose.orientation.z, ps_in.pose.orientation.w)
00040 quat_out = trans.quaternion_multiply(quat_in, self.quat_offset)
00041 ps_out.pose.orientation = Quaternion(*quat_out)
00042
00043 self.pose_pub.publish(ps_out)
00044
00045 if __name__=='__main__':
00046 parser = argparse.ArgumentParser(description="Apply a transform "+
00047 "(in the 'frame' of the pose) "+
00048 "to an incoming pose and republish")
00049 parser.add_argument('-t','--translation', nargs=3, default=(0.,0.,0.),
00050 type=float, help='The translation to be applied to'+
00051 ' the pose (with respect to itself)')
00052 parser.add_argument('-q','--quaternion', nargs=4, default=(0.,0.,0.,1.),
00053 type=float, help='The rotation quaterion to '+
00054 'be applied to the pose')
00055 parser.add_argument('-r','-rot','-rpy','--rotation', nargs=3,
00056 type=float, help='The roll-pitch-yaw rotation '+
00057 '(with respect to itself) '+
00058 'to be applied to the pose')
00059 args = parser.parse_known_args()
00060
00061 if args[0].rotation:
00062 rpy = args[0].rotation
00063 if args[0].quaternion != (0.,0.,0.,1.):
00064 parser.exit(status=1, message="Please specify only one rotation")
00065 else:
00066 quaternion = trans.quaternion_from_euler(rpy[0],rpy[1],rpy[2])
00067 else:
00068 quaternion = args[0].quaternion
00069
00070 rospy.init_node('clicked_pose_relay')
00071 relay = ClickedPoseRelay(args[0].translation, quaternion)
00072 rospy.spin()