LaserScanLayer.java
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2011 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.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         // Drop the first point which is required for the triangle fan but is
00067         // not a range reading.
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     // We start with the origin of the triangle fan.
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     // Calculate the coordinates of the laser range values.
00104     for (int i = 0; i < ranges.length; i += stride) {
00105       float range = ranges[i];
00106       // Ignore ranges that are outside the defined range. We are not overly
00107       // concerned about the accuracy of the visualization and this is makes it
00108       // look a lot nicer.
00109       if (minimumRange < range && range < maximumRange) {
00110         // x, y, z
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 }


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