Function pose_stamped_to_pq

script/utils/msg_transform.py:27–35

Convert a C{geometry_msgs/PoseStamped} 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 pose_stamped_to_pq(msg):
    """Convert a C{geometry_msgs/PoseStamped} 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])
    """
    return pose_to_pq(msg.pose)


def transform_to_pq(msg):