rigid_transformations.py 文件源码

def rigid_transform_from_ros(from_frame, to_frame, service_name='rigid_transforms/rigid_transform_listener', namespace=None):
        """Gets transform from ROS as a rigid transform

        Requires ROS rigid_transform_publisher service to be running. Assuming autolab_core is installed as a catkin package,
        this can be done with: roslaunch autolab_core rigid_transforms.launch

        Parameters
        ----------
        from_frame : :obj:`str`
        to_frame : :obj:`str`
        service_name : string, optional
            RigidTransformListener service to interface with. If the RigidTransformListener services are started through
            rigid_transforms.launch it will be called rigid_transform_listener
        namespace : string, optional
            Namespace to prepend to transform_listener_service. If None, current namespace is prepended.

        Raises
        ------
        rospy.ServiceException
            If service call to rigid_transform_listener fails
        """
        if namespace == None:
            service_name = rospy.get_namespace() + service_name
        else:
            service_name = namespace + service_name

        rospy.wait_for_service(service_name, timeout = 10)
        listener = rospy.ServiceProxy(service_name, RigidTransformListener)

        ret = listener(from_frame, to_frame)

        quat = np.asarray([ret.w_rot, ret.x_rot, ret.y_rot, ret.z_rot])
        trans = np.asarray([ret.x_trans, ret.y_trans, ret.z_trans])

        rot = RigidTransform.rotation_from_quaternion(quat)

        return RigidTransform(rotation=rot, translation=trans, from_frame=from_frame, to_frame=to_frame)