def to_pose(T):
"""
Converts a homogeneous transformation (4x4) into a C{geometry_msgs/Pose} ROS message.
@type T: np.array
@param T: The homogeneous transformation
@rtype: geometry_msgs/Pose
@return: The resulting ROS message
"""
pos = Point(*T[:3, 3])
quat = Quaternion(*tr.quaternion_from_matrix(T))
return Pose(pos, quat)
def to_transform(T):
"""
Converts a homogeneous transformation (4x4) into a C{geometry_msgs/Transform}
ROS message.
@type T: np.array
@param T: The homogeneous transformation
@rtype: geometry_msgs/Transform
@return: The resulting ROS message
"""
translation = Vector3(*T[:3, 3])
rotation = Quaternion(*tr.quaternion_from_matrix(T))
return Transform(translation, rotation)