def get_transform(self, src_frame, dest_frame): """ Return the transform from the src_frame to dest_frame :param src_frame: source frame :param dest_frame: destination frame :type src_frame: string :type dest_frame: basestring :return: tuple (trans, rot_mat, quat ) trans: translational vector (shape: :math:`[3, 1]`) rot_mat: rotational matrix (shape: :math:`[3, 3]`) quat: rotational matrix in the form of quaternion (shape: :math:`[4,]`) :rtype: tuple or ROS PoseStamped """ trans, quat = prutil.get_tf_transform(self.tf_listener, src_frame, dest_frame) rot_mat = prutil.quat_to_rot_mat(quat) trans = np.array(trans).reshape(-1, 1) rot_mat = np.array(rot_mat) quat = np.array(quat) return trans, rot_mat, quat
def get_link_transform(self, src, tgt): """ Returns the latest transformation from the target_frame to the source frame, i.e., the transform of source frame w.r.t target frame. If the returned transform is applied to data, it will transform data in the source_frame into the target_frame For more information, please refer to http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms :param src: source frame :param tgt: target frame :type src: string :type tgt: string :returns: tuple(trans, rot, T) trans: translational vector (shape: :math:`[3,]`) rot: rotation matrix (shape: :math:`[3, 3]`) T: transofrmation matrix (shape: :math:`[4, 4]`) :rtype: tuple(np.ndarray, np.ndarray, np.ndarray) """ trans, quat = prutil.get_tf_transform(self._tf_listener, tgt, src) rot = prutil.quat_to_rot_mat(quat) T = np.eye(4) T[:3, :3] = rot T[:3, 3] = trans return trans, rot, T