00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
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
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
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
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
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
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
00181 def stamp_msg(msg, frame_id):
00182 msg.header.frame_id = frame_id
00183 msg.header.stamp = rospy.Time.now()
00184
00185
00186
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
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
00203 def set_xyz(msg, xyz):
00204 msg.x = xyz[0]
00205 msg.y = xyz[1]
00206 msg.z = xyz[2]
00207
00208
00209
00210 def set_xyzw(msg, xyzw):
00211 set_xyz(msg, xyzw)
00212 msg.w = xyzw[3]
00213
00214
00215
00216 def get_xyz(msg):
00217 return [msg.x, msg.y, msg.z]
00218
00219
00220
00221 def get_xyzw(msg):
00222 return [msg.x, msg.y, msg.z, msg.w]
00223
00224
00225
00226 def transform_pose_stamped(pose_stamped, transform, pre_or_post = "post"):
00227
00228
00229 pose_mat = pose_to_mat(pose_stamped.pose)
00230
00231
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
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
00247 def point_stamped_to_list(tf_listener, point, frame):
00248
00249
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
00261 pos = [trans_point.point.x, trans_point.point.y, trans_point.point.z]
00262
00263 return pos
00264
00265
00266
00267 def vector3_stamped_to_list(tf_listener, vector3, frame):
00268
00269
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
00281 vect = [trans_vector3.vector.x, trans_vector3.vector.y, trans_vector3.vector.z]
00282
00283 return vect
00284
00285
00286
00287 def quaternion_stamped_to_list(tf_listener, quaternion, frame):
00288
00289
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
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
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
00324 def change_pose_stamped_frame(tf_listener, pose, frame):
00325
00326
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
00341 def pose_stamped_to_lists(tf_listener, pose, frame):
00342
00343
00344 trans_pose = change_pose_stamped_frame(tf_listener, pose, frame)
00345
00346
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
00355
00356 def lists_to_pose_stamped(tf_listener, pos, rot, in_frame, to_frame):
00357
00358
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
00372 def create_pose_stamped(pose, frame_id = 'base_link'):
00373 m = PoseStamped()
00374 m.header.frame_id = frame_id
00375
00376 m.header.stamp = rospy.Time()
00377 m.pose = Pose(Point(*pose[0:3]), Quaternion(*pose[3:7]))
00378 return m
00379
00380
00381
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
00387 m.point = Point(*point)
00388 return m
00389
00390
00391
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
00397 m.vector = Vector3(*vector)
00398 return m
00399
00400
00401
00402 def pplist(list):
00403 return ' '.join(['%5.3f'%x for x in list])
00404
00405
00406
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
00417 def rethrow_tf_exception(exception, msg):
00418 try:
00419 raise
00420 finally:
00421 try:
00422 rospy.logerr(msg)
00423 except:
00424 pass
00425