CompressedImagePublisher.java 文件源码

@Override
public void onNewRawImage(byte[] data, Size size) {
  Preconditions.checkNotNull(data);
  Preconditions.checkNotNull(size);
  if (data != rawImageBuffer || !size.equals(rawImageSize)) {
    rawImageBuffer = data;
    rawImageSize = size;
    yuvImage = new YuvImage(rawImageBuffer, ImageFormat.NV21, size.width, size.height, null);
    rect = new Rect(0, 0, size.width, size.height);
  }

  Time currentTime = connectedNode.getCurrentTime();
  String frameId = "camera";

  sensor_msgs.CompressedImage image = imagePublisher.newMessage();
  image.setFormat("jpeg");
  image.getHeader().setStamp(currentTime);
  image.getHeader().setFrameId(frameId);

  Preconditions.checkState(yuvImage.compressToJpeg(rect, 20, stream));
  image.setData(stream.buffer().copy());
  stream.buffer().clear();

  imagePublisher.publish(image);

  sensor_msgs.CameraInfo cameraInfo = cameraInfoPublisher.newMessage();
  cameraInfo.getHeader().setStamp(currentTime);
  cameraInfo.getHeader().setFrameId(frameId);

  cameraInfo.setWidth(size.width);
  cameraInfo.setHeight(size.height);
  cameraInfoPublisher.publish(cameraInfo);
}