Function pose_to_pq

script/utils/msg_transform.py:13–24

Convert a C{geometry_msgs/Pose} 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)

import numpy as np

def pose_to_pq(msg):
    """Convert a C{geometry_msgs/Pose} 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.position.x, msg.position.y, msg.position.z])
    q = np.array([msg.orientation.x, msg.orientation.y,
                  msg.orientation.z, msg.orientation.w])
    return p, q


def pose_stamped_to_pq(msg):