rigid_transformations.py 文件源码

def publish_to_ros(self, mode='transform', service_name='rigid_transforms/rigid_transform_publisher', namespace=None):
        """Publishes RigidTransform to ROS
        If a transform referencing the same frames already exists in the ROS publisher, it is updated instead.
        This checking is not order sensitive

        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
        ----------
        mode : :obj:`str`
            Mode in which to publish. In {'transform', 'frame'}
            Defaults to 'transform'
        service_name : string, optional
            RigidTransformPublisher service to interface with. If the RigidTransformPublisher services are started through
            rigid_transforms.launch it will be called rigid_transform_publisher
        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_publisher 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)
        publisher = rospy.ServiceProxy(service_name, RigidTransformPublisher)

        trans = self.translation
        rot = self.quaternion

        publisher(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3], self.from_frame, self.to_frame, mode)