tf_helper.py

import numpy as np
import tf.transformations as tft

def quat_to_array(msg):
    return np.array([msg.x, msg.y, msg.z, msg.w])

def vec_to_array(msg):
    return np.array([msg.x, msg.y, msg.z])

def from_pose_msg(msg):
    return tft.compose_matrix(
        angles=tft.euler_from_quaternion(quat_to_array(msg.orientation))
        translate=vec_to_array(msg.position))

def from_tf_msg(msg):
    return tft.compose_matrix(
        angles=tft.euler_from_quaternion(quat_to_array(msg.rotation))
        translate=vec_to_array(msg.translation)