convert_functions.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 # author: Kaijen Hsiao
00035 
00036 ## @package convert_functions
00037 #helper functions for creating, transforming, and converting among messages, 
00038 #scipy matrices, and lists
00039 
00040 from __future__ import division
00041 import roslib
00042 roslib.load_manifest('object_manipulator')
00043 import rospy
00044 from geometry_msgs.msg import Transform, Pose, PoseStamped, Point, Point32, PointStamped, Vector3, Vector3Stamped, Quaternion, QuaternionStamped
00045 from sensor_msgs.msg import PointCloud
00046 from std_msgs.msg import Header
00047 import tf.transformations
00048 import tf
00049 import numpy as np
00050 import scipy
00051 from sensor_msgs.msg import PointCloud, PointCloud2
00052 from object_manipulator.point_cloud import *
00053 
00054 ##convert a PointCloud or PointCloud2 to a 4xn scipy matrix (x y z 1)
00055 def point_cloud_to_mat(point_cloud):
00056     if type(point_cloud) == type(PointCloud()):
00057         points = [[p.x, p.y, p.z, 1] for p in point_cloud.points]
00058     elif type(point_cloud) == type(PointCloud2()):
00059         points = [[p[0], p[1], p[2], 1] for p in read_points(point_cloud, field_names = 'xyz', skip_nans=True)]
00060     else:
00061         print "type not recognized:", type(point_cloud)
00062         return None
00063     points = scipy.matrix(points).T
00064     return points
00065 
00066 
00067 ##convert a 4xn scipy matrix (x y z 1) to a PointCloud 
00068 def mat_to_point_cloud(mat, frame_id):
00069     pc = PointCloud()
00070     pc.header.frame_id = frame_id
00071     for n in range(mat.shape[1]):
00072         column = mat[:,n]
00073         point = Point32()
00074         point.x, point.y, point.z = column[0,0], column[1,0], column[2,0]
00075         pc.points.append(point)
00076     return pc 
00077 
00078 
00079 ##transform a PointCloud or PointCloud2 to be a 4xn scipy matrix (x y z 1) in a new frame
00080 def transform_point_cloud(tf_listener, point_cloud, frame):        
00081     points = point_cloud_to_mat(point_cloud)
00082     transform = get_transform(tf_listener, point_cloud.header.frame_id, frame)
00083     if transform == None:
00084         return (None, None)
00085     points = transform * points
00086     return (points, transform)
00087 
00088 
00089 ##get the 4x4 transformation matrix from frame1 to frame2 from TF
00090 def get_transform(tf_listener, frame1, frame2):
00091     temp_header = Header()
00092     temp_header.frame_id = frame1
00093     temp_header.stamp = rospy.Time(0)
00094     try:
00095         tf_listener.waitForTransform(frame1, frame2, rospy.Time(0), rospy.Duration(5))
00096     except tf.Exception, e:
00097         rethrow_tf_exception(e, "tf transform was not there between %s and %s"%(frame1, frame2))
00098     frame1_to_frame2 = tf_listener.asMatrix(frame2, temp_header)
00099     return scipy.matrix(frame1_to_frame2)
00100 
00101 
00102 def pose_to_mat(pose):
00103     '''Convert a pose message to a 4x4 numpy matrix.
00104 
00105     Args:
00106         pose (geometry_msgs.msg.Pose): Pose rospy message class.
00107 
00108     Returns:
00109         mat (numpy.matrix): 4x4 numpy matrix
00110     '''
00111     quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
00112     pos = np.matrix([pose.position.x, pose.position.y, pose.position.z]).T
00113     mat = np.matrix(tf.transformations.quaternion_matrix(quat))
00114     mat[0:3, 3] = pos
00115     return mat
00116 
00117 
00118 def mat_to_pose(mat, transform = None):
00119     '''Convert a homogeneous matrix to a Pose message, optionally premultiply by a transform.
00120 
00121     Args:
00122         mat (numpy.ndarray): 4x4 array (or matrix) representing a homogenous transform.
00123         transform (numpy.ndarray): Optional 4x4 array representing additional transform
00124 
00125     Returns:
00126         pose (geometry_msgs.msg.Pose): Pose message representing transform.
00127     '''
00128     if transform != None:
00129         mat = np.dot(transform, mat)
00130     pose = Pose()
00131     pose.position.x = mat[0,3]
00132     pose.position.y = mat[1,3]
00133     pose.position.z = mat[2,3]
00134     quat = tf.transformations.quaternion_from_matrix(mat)
00135     pose.orientation.x = quat[0]
00136     pose.orientation.y = quat[1]
00137     pose.orientation.z = quat[2]
00138     pose.orientation.w = quat[3]
00139     return pose
00140 
00141 def transform_to_mat(transform):
00142     '''Convert a tf transform to a 4x4 scipy mat.
00143 
00144     Args:
00145         transform (geometry_msgs.msg.Transform): Transform rospy message class.
00146 
00147     Returns:
00148         mat (numpy.matrix): 4x4 numpy matrix
00149     '''
00150     quat = [transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w]
00151     pos = scipy.matrix([transform.translation.x, transform.translation.y, transform.translation.z]).T
00152     mat = scipy.matrix(tf.transformations.quaternion_matrix(quat))
00153     mat[0:3, 3] = pos
00154     return mat
00155 
00156 def mat_to_transform(mat, transform = None):
00157     '''Convert a 4x5 scipy matrix to a transform message.
00158 
00159     Args:
00160         mat (numpy.matrix): 4x4 numpy matrix
00161         transform (numpy.ndarray): Optional 4x4 array representing additional transform
00162 
00163     Returns:
00164         transform (geometry_msgs.msg.Transform): Transform rospy message class.
00165     '''
00166     if transform != None:
00167         mat = transform * mat
00168     trans = np.array(mat[:3,3] / mat[3,3]).flat
00169     quat = tf.transformations.quaternion_from_matrix(mat)
00170     pose = Transform(Vector3(*trans), Quaternion(*quat))
00171     return pose
00172 
00173 ##convert a 4x4 scipy matrix to position and quaternion lists
00174 def mat_to_pos_and_quat(mat):
00175     quat = tf.transformations.quaternion_from_matrix(mat).tolist()
00176     pos = mat[0:3,3].T.tolist()[0]
00177     return (pos, quat)
00178 
00179 
00180 ##stamp a message by giving it a header with a timestamp of now
00181 def stamp_msg(msg, frame_id):
00182     msg.header.frame_id = frame_id
00183     msg.header.stamp = rospy.Time.now()
00184 
00185 
00186 ##make a PoseStamped out of a Pose
00187 def stamp_pose(pose, frame_id):
00188     pose_stamped = PoseStamped()
00189     stamp_msg(pose_stamped, frame_id)
00190     pose_stamped.pose = pose
00191     return pose_stamped
00192 
00193 
00194 ##make a Vector3Stamped out of a Vector3
00195 def stamp_vector3(vector3, frame_id):
00196     vector3_stamped = Vector3Stamped()
00197     stamp_msg(vector3_stamped, frame_id)
00198     vector3_stamped.vector = vector3
00199     return vector3_stamped
00200 
00201 
00202 ##set x, y, and z fields with a list
00203 def set_xyz(msg, xyz):
00204     msg.x = xyz[0]
00205     msg.y = xyz[1]
00206     msg.z = xyz[2]
00207 
00208 
00209 ##set x, y, z, and w fields with a list
00210 def set_xyzw(msg, xyzw):
00211     set_xyz(msg, xyzw)
00212     msg.w = xyzw[3]
00213 
00214 
00215 ##get x, y, and z fields in the form of a list
00216 def get_xyz(msg):
00217     return [msg.x, msg.y, msg.z]
00218 
00219 
00220 ##get x, y, z, and w fields in the form of a list
00221 def get_xyzw(msg):
00222     return [msg.x, msg.y, msg.z, msg.w]
00223 
00224 
00225 ##transform a poseStamped by a 4x4 scipy matrix 
00226 def transform_pose_stamped(pose_stamped, transform, pre_or_post = "post"):
00227 
00228     #convert the Pose to a 4x3 scipy matrix
00229     pose_mat = pose_to_mat(pose_stamped.pose)
00230     
00231     #post-multiply by the transform
00232     if pre_or_post == "post":
00233         transformed_mat = pose_mat * transform
00234     else:
00235         transformed_mat = transform * pose_mat
00236     (pos, quat) = mat_to_pos_and_quat(transformed_mat)
00237 
00238     #create a new poseStamped
00239     new_pose_stamped = PoseStamped()
00240     new_pose_stamped.header = pose_stamped.header
00241     new_pose_stamped.pose = Pose(Point(*pos), Quaternion(*quat))
00242 
00243     return new_pose_stamped
00244 
00245 
00246 ##convert a pointStamped to a pos list in a desired frame
00247 def point_stamped_to_list(tf_listener, point, frame):
00248 
00249     #convert the pointStamped to the desired frame, if necessary
00250     if point.header.frame_id != frame:
00251         try:
00252             tf_listener.waitForTransform(frame, point.header.frame_id, \
00253                                      rospy.Time(0), rospy.Duration(5))
00254             trans_point = tf_listener.transformPoint(frame, point)
00255         except tf.Exception, e:
00256             rethrow_tf_exception(e, "point_stamped_to_list: error in transforming point from " + point.header.frame_id + " to " + frame + " error msg: %s"%e)
00257     else:
00258         trans_point = point
00259 
00260     #extract position as a list
00261     pos = [trans_point.point.x, trans_point.point.y, trans_point.point.z]
00262 
00263     return pos
00264 
00265 
00266 ##convert a Vector3Stamped to a rot list in a desired frame
00267 def vector3_stamped_to_list(tf_listener, vector3, frame):
00268 
00269     #convert the vector3Stamped to the desired frame, if necessary
00270     if vector3.header.frame_id != frame:
00271         try:
00272             tf_listener.waitForTransform(frame, vector3.header.frame_id, \
00273                         rospy.Time(0), rospy.Duration(5))
00274             trans_vector3 = tf_listener.transformVector3(frame, vector3)
00275         except tf.Exception, e:
00276             rethrow_tf_exception(e, "vector3_stamped_to_list: error in transforming point from " + vector3.header.frame_id + " to " + frame + " error msg: %s"%e)
00277     else:
00278         trans_vector3 = vector3
00279 
00280     #extract vect as a list
00281     vect = [trans_vector3.vector.x, trans_vector3.vector.y, trans_vector3.vector.z]
00282 
00283     return vect
00284 
00285 
00286 ##convert a QuaternionStamped to a quat list in a desired frame
00287 def quaternion_stamped_to_list(tf_listener, quaternion, frame):
00288 
00289     #convert the QuaternionStamped to the desired frame, if necessary
00290     if quaternion.header.frame_id != frame:
00291         try:
00292             tf_listener.waitForTransform(frame, quaternion.header.frame_id, \
00293                         rospy.Time(0), rospy.Duration(5))
00294             trans_quat = tf_listener.transformQuaternion(frame, quaternion)
00295         except tf.Exception, e:
00296             rethrow_tf_exception(e, "quaternion_stamped_to_list: error in transforming point from " + quaternion.header.frame_id + " to " + frame + " error msg: %s"%e)
00297     else:
00298         trans_quat = quaternion
00299 
00300     #extract quat as a list
00301     quat = [trans_quat.quaternion.x, trans_quat.quaternion.y, trans_quat.quaternion.z, trans_quat.quaternion.w]
00302 
00303     return quat
00304 
00305 
00306 ##change the frame of a Vector3Stamped 
00307 def change_vector3_stamped_frame(tf_listener, vector3_stamped, frame):
00308 
00309     if vector3_stamped.header.frame_id != frame:
00310         vector3_stamped.header.stamp = rospy.Time(0)
00311         try:
00312             tf_listener.waitForTransform(frame, vector3_stamped.header.frame_id,\
00313                     vector3_stamped.header.stamp, rospy.Duration(5))
00314             trans_vector3 = tf_listener.transformVector3(frame, vector3_stamped)
00315         except tf.Exception, e:
00316             rethrow_tf_exception(e, "change_vector3_stamped: error in transforming vector3 from " + vector3_stamped.header.frame_id + " to " + frame + " error msg: %s"%e)
00317     else:
00318         trans_vector3 = vector3_stamped
00319 
00320     return trans_vector3
00321 
00322 
00323 ##change the frame of a PoseStamped
00324 def change_pose_stamped_frame(tf_listener, pose, frame):
00325 
00326     #convert the PoseStamped to the desired frame, if necessary
00327     if pose.header.frame_id != frame:
00328         pose.header.stamp = rospy.Time(0)
00329         try:
00330             tf_listener.waitForTransform(frame, pose.header.frame_id, pose.header.stamp, rospy.Duration(5))
00331             trans_pose = tf_listener.transformPose(frame, pose)
00332         except tf.Exception, e:
00333             rethrow_tf_exception(e, "change_pose_stamped_frame: error in transforming pose from " + pose.header.frame_id + " to " + frame + " error msg: %s"%e)
00334     else:
00335         trans_pose = pose
00336 
00337     return trans_pose
00338 
00339 
00340 ##convert a PoseStamped to pos and rot (quaternion) lists in a desired frame
00341 def pose_stamped_to_lists(tf_listener, pose, frame):
00342 
00343     #change the frame, if necessary
00344     trans_pose = change_pose_stamped_frame(tf_listener, pose, frame)
00345 
00346     #extract position and orientation as quaternion
00347     pos = [trans_pose.pose.position.x, trans_pose.pose.position.y, trans_pose.pose.position.z]
00348     rot = [trans_pose.pose.orientation.x, trans_pose.pose.orientation.y, \
00349                trans_pose.pose.orientation.z, trans_pose.pose.orientation.w]
00350 
00351     return (pos, rot)
00352 
00353 
00354 ##convert pos and rot lists (relative to in_frame) to a PoseStamped 
00355 #(relative to to_frame)
00356 def lists_to_pose_stamped(tf_listener, pos, rot, in_frame, to_frame):
00357 
00358     #stick lists in a PoseStamped
00359     m = PoseStamped()
00360     m.header.frame_id = in_frame
00361     m.header.stamp = rospy.get_rostime()
00362     m.pose = Pose(Point(*pos), Quaternion(*rot))
00363 
00364     try:
00365         pose_stamped = tf_listener.transformPose(to_frame, m)
00366     except tf.Exception, e:            
00367         rethrow_tf_exception(e, "error in transforming pose from " + in_frame + " to " + to_frame + " error msg: %s"%e)
00368     return pose_stamped
00369 
00370 
00371 ##create a PoseStamped message
00372 def create_pose_stamped(pose, frame_id = 'base_link'):
00373     m = PoseStamped()
00374     m.header.frame_id = frame_id
00375     #m.header.stamp = rospy.get_rostime()
00376     m.header.stamp = rospy.Time()
00377     m.pose = Pose(Point(*pose[0:3]), Quaternion(*pose[3:7]))
00378     return m
00379 
00380 
00381 ##create a PointStamped message
00382 def create_point_stamped(point, frame_id = 'base_link'):
00383     m = PointStamped()
00384     m.header.frame_id = frame_id
00385     m.header.stamp = rospy.Time()
00386     #m.header.stamp = rospy.get_rostime()
00387     m.point = Point(*point)
00388     return m
00389 
00390 
00391 ##create a Vector3Stamped message
00392 def create_vector3_stamped(vector, frame_id = 'base_link'):
00393     m = Vector3Stamped()
00394     m.header.frame_id = frame_id
00395     m.header.stamp = rospy.Time()
00396     #m.header.stamp = rospy.get_rostime()
00397     m.vector = Vector3(*vector)
00398     return m
00399 
00400 
00401 ##pretty-print list to string
00402 def pplist(list):
00403     return ' '.join(['%5.3f'%x for x in list])
00404 
00405 
00406 ##pretty-print numpy matrix to string
00407 def ppmat(mat):
00408     str = ''
00409     for i in range(mat.shape[0]):
00410         for j in range(mat.shape[1]):
00411             str += '%2.3f\t'%mat[i,j]
00412         str += '\n'
00413     return str
00414 
00415 
00416 ##catch a tf exception, print a message, rethrowing it first to preserve tracebacks
00417 def rethrow_tf_exception(exception, msg):
00418     try:
00419         raise
00420     finally:
00421         try:
00422             rospy.logerr(msg)
00423         except:
00424             pass #ignore errors in printing
00425 


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Thu Jan 2 2014 11:39:04