primesense_sensor.py 文件源码

def _ros_read_images(self, stream_buffer, number, staleness_limit = 10.):
        """ Reads images from a stream buffer

        Parameters
        ----------
        stream_buffer : string
            absolute path to the image buffer service
        number : int
            The number of frames to get. Must be less than the image buffer service's
            current buffer size
        staleness_limit : float, optional
            Max value of how many seconds old the oldest image is. If the oldest image
            grabbed is older than this value, a RuntimeError is thrown.

            If None, staleness is ignored.
        Returns
        -------
        List of nump.ndarray objects, each one an image
        Images are in reverse chronological order (newest first)
        """

        rospy.wait_for_service(stream_buffer, timeout = self.timeout)
        ros_image_buffer = rospy.ServiceProxy(stream_buffer, ImageBuffer)
        ret = ros_image_buffer(number, 1)
        if not staleness_limit == None:
            if ret.timestamps[-1] > staleness_limit:
                raise RuntimeError("Got data {0} seconds old, more than allowed {1} seconds"
                                   .format(ret.timestamps[-1], staleness_limit))

        data = ret.data.reshape(ret.data_dim1, ret.data_dim2, ret.data_dim3).astype(ret.dtype)

        # Special handling for 1 element, since dstack's behavior is different
        if number == 1:
            return [data]
        return np.dsplit(data, number)