Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 package org.ros.android.view.visualization.layer;
00018
00019 import org.ros.android.view.visualization.Color;
00020 import org.ros.android.view.visualization.Vertices;
00021 import org.ros.android.view.visualization.VisualizationView;
00022 import org.ros.message.MessageListener;
00023 import org.ros.namespace.GraphName;
00024 import org.ros.node.ConnectedNode;
00025 import org.ros.node.topic.Subscriber;
00026
00027 import java.nio.FloatBuffer;
00028
00029 import javax.microedition.khronos.opengles.GL10;
00030
00031 import sensor_msgs.LaserScan;
00032
00039 public class LaserScanLayer extends SubscriberLayer<sensor_msgs.LaserScan> implements TfLayer {
00040
00041 private static final Color FREE_SPACE_COLOR = Color.fromHexAndAlpha("377dfa", 0.1f);
00042 private static final Color OCCUPIED_SPACE_COLOR = Color.fromHexAndAlpha("377dfa", 0.3f);
00043 private static final float LASER_SCAN_POINT_SIZE = 10.f;
00044 private static final int LASER_SCAN_STRIDE = 15;
00045
00046 private final Object mutex;
00047
00048 private GraphName frame;
00049 private FloatBuffer vertexFrontBuffer;
00050 private FloatBuffer vertexBackBuffer;
00051
00052 public LaserScanLayer(String topicName) {
00053 this(GraphName.of(topicName));
00054 }
00055
00056 public LaserScanLayer(GraphName topicName) {
00057 super(topicName, sensor_msgs.LaserScan._TYPE);
00058 mutex = new Object();
00059 }
00060
00061 @Override
00062 public void draw(VisualizationView view, GL10 gl) {
00063 if (vertexFrontBuffer != null) {
00064 synchronized (mutex) {
00065 Vertices.drawTriangleFan(gl, vertexFrontBuffer, FREE_SPACE_COLOR);
00066
00067
00068 FloatBuffer pointVertices = vertexFrontBuffer.duplicate();
00069 pointVertices.position(3);
00070 Vertices.drawPoints(gl, pointVertices, OCCUPIED_SPACE_COLOR, LASER_SCAN_POINT_SIZE);
00071 }
00072 }
00073 }
00074
00075 @Override
00076 public void onStart(VisualizationView view, ConnectedNode connectedNode) {
00077 super.onStart(view, connectedNode);
00078 Subscriber<LaserScan> subscriber = getSubscriber();
00079 subscriber.addMessageListener(new MessageListener<LaserScan>() {
00080 @Override
00081 public void onNewMessage(LaserScan laserScan) {
00082 frame = GraphName.of(laserScan.getHeader().getFrameId());
00083 updateVertexBuffer(laserScan, LASER_SCAN_STRIDE);
00084 }
00085 });
00086 }
00087
00088 private void updateVertexBuffer(LaserScan laserScan, int stride) {
00089 float[] ranges = laserScan.getRanges();
00090 int size = ((ranges.length / stride) + 2) * 3;
00091 if (vertexBackBuffer == null || vertexBackBuffer.capacity() < size) {
00092 vertexBackBuffer = Vertices.allocateBuffer(size);
00093 }
00094 vertexBackBuffer.clear();
00095
00096 vertexBackBuffer.put(0);
00097 vertexBackBuffer.put(0);
00098 vertexBackBuffer.put(0);
00099 float minimumRange = laserScan.getRangeMin();
00100 float maximumRange = laserScan.getRangeMax();
00101 float angle = laserScan.getAngleMin();
00102 float angleIncrement = laserScan.getAngleIncrement();
00103
00104 for (int i = 0; i < ranges.length; i += stride) {
00105 float range = ranges[i];
00106
00107
00108
00109 if (minimumRange < range && range < maximumRange) {
00110
00111 vertexBackBuffer.put((float) (range * Math.cos(angle)));
00112 vertexBackBuffer.put((float) (range * Math.sin(angle)));
00113 vertexBackBuffer.put(0);
00114 }
00115 angle += angleIncrement * stride;
00116 }
00117 vertexBackBuffer.position(0);
00118 synchronized (mutex) {
00119 FloatBuffer tmp = vertexFrontBuffer;
00120 vertexFrontBuffer = vertexBackBuffer;
00121 vertexBackBuffer = tmp;
00122 }
00123 }
00124
00125 @Override
00126 public GraphName getFrame() {
00127 return frame;
00128 }
00129 }