CompressedMapTransport.java
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2012 Google Inc.
00003  * 
00004  * Licensed under the Apache License, Version 2.0 (the "License"); you may not
00005  * use this file except in compliance with the License. You may obtain a copy of
00006  * the License at
00007  * 
00008  * http://www.apache.org/licenses/LICENSE-2.0
00009  * 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
00012  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
00013  * License for the specific language governing permissions and limitations under
00014  * the License.
00015  */
00016 
00017 package org.ros.android.compressed_map_transport;
00018 
00019 import org.jboss.netty.buffer.ChannelBuffer;
00020 import org.jboss.netty.buffer.ChannelBufferOutputStream;
00021 import org.ros.exception.RosRuntimeException;
00022 import org.ros.internal.message.MessageBuffers;
00023 import org.ros.message.MessageListener;
00024 import org.ros.namespace.GraphName;
00025 import org.ros.node.AbstractNodeMain;
00026 import org.ros.node.ConnectedNode;
00027 import org.ros.node.topic.Publisher;
00028 import org.ros.node.topic.Subscriber;
00029 
00030 import java.awt.Image;
00031 import java.awt.Transparency;
00032 import java.awt.color.ColorSpace;
00033 import java.awt.image.BufferedImage;
00034 import java.awt.image.ColorModel;
00035 import java.awt.image.ComponentColorModel;
00036 import java.awt.image.DataBuffer;
00037 import java.awt.image.DataBufferByte;
00038 import java.awt.image.Raster;
00039 import java.awt.image.SampleModel;
00040 import java.awt.image.WritableRaster;
00041 import java.io.IOException;
00042 
00043 import javax.imageio.ImageIO;
00044 
00050 public class CompressedMapTransport extends AbstractNodeMain {
00051 
00052   private static final int MAXIMUM_WIDTH = 1024;
00053   private static final int MAXIMUM_HEIGHT = 1024;
00054   private static final String IMAGE_FORMAT = "png";
00055   private static final GraphName TOPIC_IN = GraphName.of("map");
00056   private static final GraphName TOPIC_OUT = TOPIC_IN.join(IMAGE_FORMAT);
00057 
00058   private Publisher<nav_msgs.OccupancyGrid> publisher;
00059   private Subscriber<nav_msgs.OccupancyGrid> subscriber;
00060 
00061   @Override
00062   public GraphName getDefaultNodeName() {
00063     return GraphName.of("map_transport");
00064   }
00065 
00066   @Override
00067   public void onStart(ConnectedNode connectedNode) {
00068     publisher = connectedNode.newPublisher(TOPIC_OUT, nav_msgs.OccupancyGrid._TYPE);
00069     publisher.setLatchMode(true);
00070     subscriber = connectedNode.newSubscriber(TOPIC_IN, nav_msgs.OccupancyGrid._TYPE);
00071     subscriber.addMessageListener(new MessageListener<nav_msgs.OccupancyGrid>() {
00072       @Override
00073       public void onNewMessage(nav_msgs.OccupancyGrid message) {
00074         if (message.getInfo().getWidth() > 0 && message.getInfo().getHeight() > 0) {
00075           publisher.publish(scaleAndCompressOccupancyGrid(message));
00076         }
00077       }
00078     });
00079   }
00080 
00081   private nav_msgs.OccupancyGrid scaleAndCompressOccupancyGrid(nav_msgs.OccupancyGrid message) {
00082     BufferedImage bufferedImage = newGrayscaleBufferedImage(message);
00083     BufferedImage scaledBufferedImage = scaleBufferedImage(bufferedImage);
00084     ChannelBuffer buffer = MessageBuffers.dynamicBuffer();
00085     ChannelBufferOutputStream outputStream = new ChannelBufferOutputStream(buffer);
00086     try {
00087       ImageIO.write(scaledBufferedImage, IMAGE_FORMAT, outputStream);
00088     } catch (IOException e) {
00089       throw new RosRuntimeException(e);
00090     }
00091     nav_msgs.OccupancyGrid compressedMessage = publisher.newMessage();
00092     compressedMessage.getHeader().setFrameId(message.getHeader().getFrameId());
00093     compressedMessage.getHeader().setStamp(message.getHeader().getStamp());
00094     compressedMessage.getInfo().setMapLoadTime(message.getInfo().getMapLoadTime());
00095     compressedMessage.getInfo().setOrigin(message.getInfo().getOrigin());
00096     compressedMessage.getInfo().setWidth(scaledBufferedImage.getWidth());
00097     compressedMessage.getInfo().setHeight(scaledBufferedImage.getHeight());
00098     float resolution =
00099         message.getInfo().getResolution() * message.getInfo().getHeight()
00100             / scaledBufferedImage.getHeight();
00101     compressedMessage.getInfo().setResolution(resolution);
00102     compressedMessage.setData(buffer);
00103     return compressedMessage;
00104   }
00105 
00106   private BufferedImage newGrayscaleBufferedImage(nav_msgs.OccupancyGrid message) {
00107     int width = message.getInfo().getWidth();
00108     int height = message.getInfo().getHeight();
00109     ColorSpace colorSpace = ColorSpace.getInstance(ColorSpace.CS_GRAY);
00110     ColorModel colorModel =
00111         new ComponentColorModel(colorSpace, new int[] { 8 }, false, false, Transparency.OPAQUE,
00112             DataBuffer.TYPE_BYTE);
00113     SampleModel sampleModel = colorModel.createCompatibleSampleModel(width, height);
00114     // There is a bug in DataBuffer that causes WritableRaster to ignore the
00115     // offset. As a result, we have to make a copy of the data here so that the
00116     // array is guaranteed to start with the first readable byte.
00117     byte[] data = new byte[message.getData().readableBytes()];
00118     message.getData().readBytes(data);
00119     DataBuffer dataBuffer = new DataBufferByte(data, data.length, 0);
00120     WritableRaster raster = Raster.createWritableRaster(sampleModel, dataBuffer, null);
00121     BufferedImage bufferedImage = new BufferedImage(colorModel, raster, false, null);
00122     return bufferedImage;
00123   }
00124 
00125   private BufferedImage scaleBufferedImage(BufferedImage bufferedImage) {
00126     int height = bufferedImage.getHeight();
00127     int width = bufferedImage.getWidth();
00128     BufferedImage scaledBufferedImage = bufferedImage;
00129     if (height > MAXIMUM_HEIGHT || width > MAXIMUM_WIDTH) {
00130       // Setting the width or height to -1 causes the scaling method to maintain
00131       // the image's aspect ratio.
00132       int scaledHeight = -1;
00133       int scaledWidth = -1;
00134       if (height > width) {
00135         scaledHeight = MAXIMUM_HEIGHT;
00136       } else {
00137         scaledWidth = MAXIMUM_WIDTH;
00138       }
00139       Image image = bufferedImage.getScaledInstance(scaledWidth, scaledHeight, Image.SCALE_SMOOTH);
00140       scaledBufferedImage =
00141           new BufferedImage(image.getWidth(null), image.getHeight(null),
00142               BufferedImage.TYPE_BYTE_GRAY);
00143       scaledBufferedImage.getGraphics().drawImage(image, 0, 0, null);
00144     }
00145     return scaledBufferedImage;
00146   }
00147 }


android_core
Author(s): Damon Kohler
autogenerated on Thu Jun 6 2019 21:20:07