geometry.py
Go to the documentation of this file.
00001 from .registry import converts_from_numpy, converts_to_numpy
00002 from geometry_msgs.msg import Transform, Vector3, Quaternion, Point, Pose
00003 from . import numpify
00004 
00005 import numpy as np
00006 
00007 # basic types
00008 
00009 @converts_to_numpy(Vector3)
00010 def vector3_to_numpy(msg, hom=False):
00011         if hom:
00012                 return np.array([msg.x, msg.y, msg.z, 0])
00013         else:
00014                 return np.array([msg.x, msg.y, msg.z])
00015 
00016 @converts_from_numpy(Vector3)
00017 def numpy_to_vector3(arr):
00018         if arr.shape[-1] == 4:
00019                 assert np.all(arr[...,-1] == 0)
00020                 arr = arr[...,:-1]
00021 
00022         if len(arr.shape) == 1:
00023                 return Vector3(*arr)
00024         else:
00025                 return np.apply_along_axis(lambda v: Vector3(*v), axis=-1, arr=arr)
00026 
00027 @converts_to_numpy(Point)
00028 def point_to_numpy(msg, hom=False):
00029         if hom:
00030                 return np.array([msg.x, msg.y, msg.z, 1])
00031         else:
00032                 return np.array([msg.x, msg.y, msg.z])
00033 
00034 @converts_from_numpy(Point)
00035 def numpy_to_point(arr):
00036         if arr.shape[-1] == 4:
00037                 arr = arr[...,:-1] / arr[...,-1]
00038 
00039         if len(arr.shape) == 1:
00040                 return Point(*arr)
00041         else:
00042                 return np.apply_along_axis(lambda v: Point(*v), axis=-1, arr=arr)
00043 
00044 @converts_to_numpy(Quaternion)
00045 def quat_to_numpy(msg):
00046         return np.array([msg.x, msg.y, msg.z, msg.w])
00047 
00048 @converts_from_numpy(Quaternion)
00049 def numpy_to_quat(arr):
00050         assert arr.shape[-1] == 4
00051 
00052         if len(arr.shape) == 1:
00053                 return Quaternion(*arr)
00054         else:
00055                 return np.apply_along_axis(lambda v: Quaternion(*v), axis=-1, arr=arr)
00056 
00057 
00058 # compound types
00059 # all of these take ...x4x4 homogeneous matrices
00060 
00061 @converts_to_numpy(Transform)
00062 def transform_to_numpy(msg):
00063         from tf import transformations
00064 
00065         return np.dot(
00066         transformations.translation_matrix(numpify(msg.translation)),
00067         transformations.quaternion_matrix(numpify(msg.rotation))
00068     )
00069 
00070 @converts_from_numpy(Transform)
00071 def numpy_to_transform(arr):
00072         from tf import transformations
00073 
00074         shape, rest = arr.shape[:-2], arr.shape[-2:]
00075         assert rest == (4,4)
00076 
00077         if len(shape) == 0:
00078                 trans = transformations.translation_from_matrix(arr)
00079                 quat = transformations.quaternion_from_matrix(arr)
00080 
00081                 return Transform(
00082                         translation=Vector3(*trans),
00083                         rotation=Quaternion(*quat)
00084                 )
00085         else:
00086                 res = np.empty(shape, dtype=np.object_)
00087                 for idx in np.ndindex(shape):
00088                         res[idx] = Transform(
00089                                 translation=Vector3(*transformations.translation_from_matrix(arr[idx])),
00090                                 rotation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
00091                         )
00092 
00093 @converts_to_numpy(Pose)
00094 def pose_to_numpy(msg):
00095         from tf import transformations
00096 
00097         return np.dot(
00098         transformations.translation_matrix(numpify(msg.position)),
00099         transformations.quaternion_matrix(numpify(msg.orientation))
00100     )
00101 
00102 @converts_from_numpy(Pose)
00103 def numpy_to_pose(arr):
00104         from tf import transformations
00105 
00106         shape, rest = arr.shape[:-2], arr.shape[-2:]
00107         assert rest == (4,4)
00108 
00109         if len(shape) == 0:
00110                 trans = transformations.translation_from_matrix(arr)
00111                 quat = transformations.quaternion_from_matrix(arr)
00112 
00113                 return Pose(
00114                         position=Vector3(*trans),
00115                         orientation=Quaternion(*quat)
00116                 )
00117         else:
00118                 res = np.empty(shape, dtype=np.object_)
00119                 for idx in np.ndindex(shape):
00120                         res[idx] = Pose(
00121                                 position=Vector3(*transformations.translation_from_matrix(arr[idx])),
00122                                 orientation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
00123                         )
00124 


ros_numpy
Author(s): Eric Wieser
autogenerated on Tue Mar 28 2017 04:16:38