Function transform_to_pq

script/utils/msg_transform.py:38–49

Convert a C{geometry_msgs/Transform} into position/quaternion np arrays @param msg: ROS message to be converted @return: - p: position as a np.array - q: quaternion as a numpy array (order = [x,y,z,w])

(msg)

def transform_to_pq(msg):
    """Convert a C{geometry_msgs/Transform} into position/quaternion np arrays

    @param msg: ROS message to be converted
    @return:
      - p: position as a np.array
      - q: quaternion as a numpy array (order = [x,y,z,w])
    """
    p = np.array([msg.translation.x, msg.translation.y, msg.translation.z])
    q = np.array([msg.rotation.x, msg.rotation.y,
                  msg.rotation.z, msg.rotation.w])
    return p, q


def transform_stamped_to_pq(msg):