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