ROS pgm转jpg

ROS pgm转jpg

ros通过订阅map（nav_msgs/OccupancyGrid）

转化成对应实体类OccupancyGrid后保存图片

@Data
@NoArgsConstructor
@AllArgsConstructor
@Builder
public class OccupancyGrid {
    private Header header;
    private MapMetaData info;
    private int[] data;
}

OccupancyGrid结构 java

OccupancyGrid
  Header header
    int seq
    Time stamp
      int secs
      int nsecs
    String frame_id
  MapMetaData info
    Time map_load_time
      int secs
      int nsecs
    Double resolution
    Integer width
    Integer height
    Pose origin
      Point position
        Double x
        Double y
        Double z
      Quaternion orientation
        Double x
        Double y
        Double z
        Double w
  int[] data
      

public static File parseToImageFile(OccupancyGrid mapReceive, String path) {
        try {
            File file = new File(path);
            int[] datas = mapReceive.getData();
            int w = mapReceive.getInfo().getWidth();
            int h = mapReceive.getInfo().getHeight();

            int[] newDatas = new int[w * h];

            //Color.RED;
            int[][] buffer = new int[h][w];
            for (int y = 0; y < h; y++) {
                for (int x = 0; x < w; x++) {
                    int index = y * w + x;
                    int data = datas[index];
                    if (data == -1) {
                        buffer[y][x] = 10;
                    } else if (data == 0) {
                        buffer[y][x] = 255;
                    } else {
                        buffer[y][x] = 0;
                    }

                    newDatas[index] = buffer[y][x];

                }
            }


            BufferedImage outputImage = new BufferedImage(w, h, BufferedImage.TYPE_BYTE_GRAY);

            WritableRaster raster = outputImage.getRaster();

            raster.setSamples(0, 0, w, h, 0, newDatas);

            ImageIO.write(outputImage, "jpg", file);


            return file;
        } catch (IOException e) {
            e.printStackTrace();
        }

        return null;
    }

之前自己在弄pgm转jpg但是颜色就是有问题，最后还是公司的架构师解决了，以上代码可以直接把pgm转成jpg

自己找到一位大佬 jpg转pgm的代码https://gitee.com/Keith404/PGMIO 

在此基础上写了自己的pgm转jpg的方法，颜色有问题，生成的是蓝色的，代码如下，慎用

import javax.imageio.ImageIO;
import java.awt.image.BufferedImage;
import java.io.*;
import java.util.ArrayList;
import java.util.List;
import java.util.Scanner;

public final class PgmIO {

    private static final String MAGIC_P5 = "P5";
    private static final String MAGIC_P2 = "P2";
    /**
     * Character indicating a comment.
     */
    private static final char COMMENT = '#';
    /**
     * The maximum gray value.
     */
    private static final int MAXVAL = 255;

    /**
     * Reads a grayscale image from a file in PGM format.
     *
     * @param file the PGM file read from
     * @return two-dimensional byte array representation of the image
     * @throws IOException
     */
    public static int[][] read(final File file) throws IOException {
        final BufferedInputStream stream = new BufferedInputStream(new FileInputStream(file));
        try {
            String type = next(stream);
            if ((!type.equals(MAGIC_P5))&&(!type.equals(MAGIC_P2)))
                throw new IOException("File " + file + " is not a binary PGM image.");
            //宽度
            final int col = Integer.parseInt(next(stream));
            //高度
            final int row = Integer.parseInt(next(stream));
            //最大灰度值
            final int max = Integer.parseInt(next(stream));
            if (max < 0 || max > MAXVAL)
                throw new IOException("The image's maximum gray value must be in range [0, " + MAXVAL + "].");
            final int[][] image = new int[row][col];
            if(MAGIC_P2.equalsIgnoreCase(type)){
                for (int i = 0; i < row; ++i) {
                    for (int j = 0; j < col; ++j) {
                        final int p = Integer.parseInt(nextStr(stream));
                        if (p == -1)
                            throw new IOException("Reached end-of-file prematurely.");
                        else if (p < 0 || p > max)
                            throw new IOException("Pixel value " + p + " outside of range [0, " + max + "].");
                        image[i][j] = p;
                        System.out.print(p + " ");
                    }
                    System.out.println();
                }
            }
            if(MAGIC_P5.equals(type)) {
                for (int i = 0; i < row; ++i) {
                    for (int j = 0; j < col; ++j) {
                        final int p = stream.read();
                        if (p == -1)
                            throw new IOException("Reached end-of-file prematurely.");
                        else if (p < 0 || p > max)
                            throw new IOException("Pixel value " + p + " outside of range [0, " + max + "].");
                        image[i][j] = p;
                        System.out.print(p + " ");
                    }
                    System.out.println();
                }
            }
            return image;
        } finally {
            stream.close();
        }
    }

    /**
     * Finds the next whitespace-delimited string in a stream, ignoring any comments.
     *
     * @param stream the stream read from
     * @return the next whitespace-delimited string
     * @throws IOException
     */
    private static String next(final InputStream stream) throws IOException {
        final List<Byte> bytes = new ArrayList<Byte>();
        while (true) {
            final int b = stream.read();

            if (b != -1) {

                final char c = (char) b;
                if (c == COMMENT) {
                    int d;
                    do {
                        d = stream.read();
                    } while (d != -1 && d != '\n' && d != '\r');
                } else if (!Character.isWhitespace(c)) {
                    bytes.add((byte) b);
                } else if (bytes.size() > 0) {
                    break;
                }

            } else {
                break;
            }

        }
        final byte[] bytesArray = new byte[bytes.size()];
        for (int i = 0; i < bytesArray.length; ++i)
            bytesArray[i] = bytes.get(i);
        return new String(bytesArray);
    }

    private static String nextStr(final InputStream stream) throws IOException {
        final List<Byte> bytes = new ArrayList<Byte>();
        while (true) {
            final int b = stream.read();

            if (b != -1&&b != ' ') {

                final char c = (char) b;
                if (c == COMMENT) {
                    int d;
                    do {
                        d = stream.read();
                    } while (d != -1 && d != '\n' && d != '\r');
                } else if (!Character.isWhitespace(c)) {
                    bytes.add((byte) b);
                } else if (bytes.size() > 0) {
                    break;
                }

            } else {
                break;
            }

        }
        final byte[] bytesArray = new byte[bytes.size()];
        if(bytes.size()==0)return nextStr(stream);
        for (int i = 0; i < bytesArray.length; ++i)
            bytesArray[i] = bytes.get(i);
        return new String(bytesArray);
    }

    /**
     * Writes a grayscale image to a file in PGM format.
     *
     * @param image a two-dimensional byte array representation of the image
     * @param file  the file to write to
     * @throws IllegalArgumentException
     * @throws IOException
     */
    public static void jpgToPgm(final int[][] image, final File file) throws IOException {
        jpgToPgm(image, file, MAXVAL);
    }


    public static void jpgToPgm(BufferedImage image, final File file) throws IOException {
        int[][] rgbArray = new int[image.getHeight()][image.getWidth()];
        for (int j = 0; j < image.getHeight(); j++) {
            for (int i = 0; i < image.getWidth(); i++) {
                int p = image.getRGB(i, j);
                int a = (p >> 24) & 0xff;
                int r = (p >> 16) & 0xff;
                int g = (p >> 8) & 0xff;
                int b = p & 0xff;
                int avg = (r + g + b) / 3;
                p = (a << 24) | (avg << 16) | (avg << 8) | avg;
                System.out.print(p+" ");
                //newImg.setRGB(i, j, p);
                //image.setRGB(i, j, p);
                rgbArray[j][i] = avg;
            }
            System.out.println();
        }
        jpgToPgm(rgbArray, file);
    }

    public static void jpgToPgm(File imageFile, final File file) throws IOException {
        try {
            BufferedImage bf = ImageIO.read(imageFile);
            jpgToPgm(bf, file);
        } catch (IOException e) {
            e.printStackTrace();
        }
    }

    /**
     * Writes a grayscale image to a file in PGM format.
     *
     * @param image  a two-dimensional byte array representation of the image
     * @param file   the file to write to
     * @param maxval the maximum gray value
     * @throws IllegalArgumentException
     * @throws IOException
     */
    public static void jpgToPgm(final int[][] image, final File file, final int maxval) throws IOException {
        if (maxval > MAXVAL)
            throw new IllegalArgumentException("The maximum gray value cannot exceed " + MAXVAL + ".");
        final BufferedOutputStream stream = new BufferedOutputStream(new FileOutputStream(file));
        try {
            stream.write(MAGIC_P5.getBytes());
            stream.write("\n".getBytes());
            stream.write(Integer.toString(image[0].length).getBytes());
            stream.write(" ".getBytes());
            stream.write(Integer.toString(image.length).getBytes());
            stream.write("\n".getBytes());
            stream.write(Integer.toString(maxval).getBytes());
            stream.write("\n".getBytes());

            for (int i = 0; i < image.length; i++) {
                for (int j = 0; j < image[0].length; j++) {
                    final int p = image[i][j];
                    if (p < 0 || p > maxval)
                        throw new IOException("Pixel value " + p + " outside of range [0, " + maxval + "].");
                    stream.write(image[i][j]);
                }
            }

            //you can delete println
            System.out.println("PGM create successful, file name :" + file.getName() + "\nwidth:" + image[0].length + ",height:" + image.length);

        } finally {
            stream.close();
        }
    }

    private static void P2ToJpg(File imageFile, final File file) throws IOException {
        int[][] read = readP2(imageFile);
        BufferedImage image = new BufferedImage(read[0].length,read.length,  BufferedImage.TYPE_INT_RGB);
        for (int row = 0; row < read[0].length; row++) {
            for (int col = 0; col < read.length; col++) {
                image.setRGB(row,col,read[col][row]);
            }
        }
        ImageIO.write(image, "png", file);
    }

    private static int[][] readP2(File imageFile) throws IOException {
        FileInputStream f = new FileInputStream(imageFile);
        BufferedReader d = new BufferedReader(new InputStreamReader(f));
        String magic = d.readLine(); // first line contains P2 or P5
        String line = d.readLine();  // second line contains height and width
        while (line.startsWith("#")) {
            line = d.readLine();
        }
        Scanner s = new Scanner(line);
        int width = s.nextInt();
        int height = s.nextInt();

        line = d.readLine();// third line contains maxVal
        s = new Scanner(line);
        int maxVal = s.nextInt();
        int[][] result = new int[height][width];
        int count = 0;
        while ((line = d.readLine())!=null){
            String[] s1 = line.split("  ");
            for (String s2 : s1) {
                result[count / width][count % width] = Integer.parseInt(s2.trim());
                count++;
            }
        }
        return result;
    }

    public static void pgm2png(File dest, File from) throws IOException {
        BufferedImage image  = ImageIO.read(from);
        int width = image.getWidth();
        int height = image.getHeight();
        System.out.println(width);
        System.out.println(height);
        System.out.println(image.getData());
        // 创建BufferedImage对象
        BufferedImage image2 = new BufferedImage(width, height, BufferedImage.TYPE_BYTE_GRAY);// 灰色

        image2.setData(image.getData());

        // 保存文件
        ImageIO.write(image2, "jpg", dest);
    }

    private static void P5ToJpg(File imageFile, final File file) throws IOException {
        int[][] read = read(imageFile);
        BufferedImage image = new BufferedImage(read[0].length,read.length,  BufferedImage.TYPE_INT_BGR);// 灰色
        for (int row = 0; row < read[0].length; row++) {
            for (int col = 0; col < read.length; col++) {
                image.setRGB(row,col,read[col][row]);
            }
        }
        ImageIO.write(image, "jpg", file);
    }

}