-
Notifications
You must be signed in to change notification settings - Fork 0
/
ros_tf_utils.py
73 lines (56 loc) · 2 KB
/
ros_tf_utils.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
import tf.transformations as transformations
import numpy as np
from geometry_msgs.msg import Pose, Point, Quaternion
from nav_msgs.msg import Odometry
import warnings
def odom_to_mat44(odom):
"""
Convert odom msg to 4*4 transformation matrix
:param odom:
:return:
"""
# Be careful Odometry msg contains PoseWithCovariance msg
return pose_to_mat44(odom.pose.pose)
def pose_to_mat44(pose):
"""
Convert pose msg to 4*4 transformation matrix
:param pose:
:return:
"""
[x, y, z] = [pose.position.x, pose.position.y, pose.position.z]
trans_mat = transformations.translation_matrix([x, y, z])
[x, y, z, w] = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
rot_mat = transformations.quaternion_matrix([x, y, z, w])
return np.dot(trans_mat, rot_mat)
def mat44_to_pose(mat):
"""
Convert 4*4 transformation matrix to pose msg (no header information added)
:param mat:
:return:
"""
trans = Point(*tuple(transformations.translation_from_matrix(mat)))
rot = Quaternion(*tuple(transformations.quaternion_from_matrix(mat)))
return Pose(trans, rot)
def quaternion_to_euler(x, y, z, w):
q = (x, y, z, w)
euler = transformations.euler_from_quaternion(q)
return euler[0], euler[1], euler[2]
def euler_to_quaternion(roll, pitch, yaw):
q = transformations.quaternion_from_euler(roll, pitch, yaw)
return q[0], q[1], q[2], q[3]
def euler_to_mat44(roll, pitch, yaw):
warnings.warn('please use this carefully, because the euler angle axis can be very confusing.')
return transformations.euler_matrix(roll, pitch, yaw)
def enu2rviz(orig_mat):
"""
Take a 4*4 matrix that follows ENU definition and output a matrix that follows RVIZ definition
:param orig_mat:
:return:
"""
mat = np.identity(4, dtype=np.float64)
mat[0][0] = 0.0
mat[0][1] = 1.0
mat[1][0] = -1.0
mat[1][1] = 0.0
new_mat = mat.dot(orig_mat).dot(np.linalg.inv(mat))
return new_mat