clicked_pose_relay.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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()


assistive_teleop
Author(s): Phillip M. Grice, Advisor: Prof. Charlie Kemp, Lab: The Healthcare Robotoics Lab at Georgia Tech.
autogenerated on Wed Nov 27 2013 11:35:34