Merge pull request #201 from damonkohler/work

Visualization improvements and bug fixes.

android_15/src/org/ros/android/view/visualization/Vertices.java

@@ -49,25 +49,41 @@ public class Vertices {
   }
 
   public static void drawPoints(GL10 gl, FloatBuffer vertices, Color color, float size) {
+    vertices.mark();
     color.apply(gl);
     gl.glPointSize(size);
     gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
     gl.glVertexPointer(3, GL10.GL_FLOAT, 0, vertices);
     gl.glDrawArrays(GL10.GL_POINTS, 0, countVertices(vertices, 3));
     gl.glDisableClientState(GL10.GL_VERTEX_ARRAY);
+    vertices.reset();
   }
 
   public static void drawTriangleFan(GL10 gl, FloatBuffer vertices, Color color) {
+    vertices.mark();
     color.apply(gl);
     gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
     gl.glVertexPointer(3, GL10.GL_FLOAT, 0, vertices);
     gl.glDrawArrays(GL10.GL_TRIANGLE_FAN, 0, countVertices(vertices, 3));
     gl.glDisableClientState(GL10.GL_VERTEX_ARRAY);
+    vertices.reset();
+  }
+
+  public static void drawLineLoop(GL10 gl, FloatBuffer vertices, Color color, float width) {
+    vertices.mark();
+    color.apply(gl);
+    gl.glLineWidth(width);
+    gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
+    gl.glVertexPointer(3, GL10.GL_FLOAT, 0, vertices);
+    gl.glDrawArrays(GL10.GL_LINE_LOOP, 0, countVertices(vertices, 3));
+    gl.glDisableClientState(GL10.GL_VERTEX_ARRAY);
+    vertices.reset();
   }
 
   private static int countVertices(FloatBuffer vertices, int size) {
     // FloatBuffer accounts for the size of each float when calling remaining().
-    Preconditions.checkArgument(vertices.remaining() % size == 0);
+    Preconditions.checkArgument(vertices.remaining() % size == 0,
+        "Number of vertices: " + vertices.remaining());
     return vertices.remaining() / size;
   }
 }

android_15/src/org/ros/android/view/visualization/VisualizationView.java

@@ -26,6 +26,7 @@ import android.opengl.GLSurfaceView;
 import android.os.Bundle;
 import android.util.AttributeSet;
 import android.view.MotionEvent;
+
 import org.ros.android.RosActivity;
 import org.ros.android.view.visualization.layer.Layer;
 import org.ros.message.MessageListener;
@@ -48,6 +49,7 @@ public class VisualizationView extends GLSurfaceView implements NodeMain {
 
   private static final boolean DEBUG = false;
 
+  private final Object mutex = new Object();
   private final FrameTransformTree frameTransformTree = new FrameTransformTree();
   private final XYOrthographicCamera camera = new XYOrthographicCamera(frameTransformTree);
 
@@ -65,7 +67,7 @@ public class VisualizationView extends GLSurfaceView implements NodeMain {
 
   /**
    * Must be called in {@link Activity#onCreate(Bundle)}.
-   * 
+   *
    * @param layers
    */
   public void onCreate(List<Layer> layers) {
@@ -81,10 +83,10 @@ public class VisualizationView extends GLSurfaceView implements NodeMain {
     renderer = new XYOrthographicRenderer(this);
     setRenderer(renderer);
   }
-  
+
   /**
    * Must be called in {@link RosActivity#init(NodeMainExecutor)}
-   * 
+   *
    * @param nodeMainExecutor
    */
   public void init(NodeMainExecutor nodeMainExecutor) {
@@ -133,13 +135,27 @@ public class VisualizationView extends GLSurfaceView implements NodeMain {
   }
 
   private void startTransformListener() {
-    Subscriber<tf2_msgs.TFMessage> tfSubscriber =
+    final Subscriber<tf2_msgs.TFMessage> tfSubscriber =
         connectedNode.newSubscriber("tf", tf2_msgs.TFMessage._TYPE);
     tfSubscriber.addMessageListener(new MessageListener<tf2_msgs.TFMessage>() {
       @Override
       public void onNewMessage(tf2_msgs.TFMessage message) {
-        for (geometry_msgs.TransformStamped transform : message.getTransforms()) {
-          frameTransformTree.update(transform);
+        synchronized (mutex) {
+          for (geometry_msgs.TransformStamped transform : message.getTransforms()) {
+            frameTransformTree.update(transform);
+          }
+        }
+      }
+    });
+    final Subscriber<tf2_msgs.TFMessage> tfStaticSubscriber =
+        connectedNode.newSubscriber("tf_static", tf2_msgs.TFMessage._TYPE);
+    tfStaticSubscriber.addMessageListener(new MessageListener<tf2_msgs.TFMessage>() {
+      @Override
+      public void onNewMessage(tf2_msgs.TFMessage message) {
+        synchronized (mutex) {
+          for (geometry_msgs.TransformStamped transform : message.getTransforms()) {
+            frameTransformTree.update(transform);
+          }
         }
       }
     });

android_15/src/org/ros/android/view/visualization/layer/LaserScanLayer.java

@@ -16,19 +16,20 @@
 
 package org.ros.android.view.visualization.layer;
 
-import org.ros.android.view.visualization.VisualizationView;
 import org.ros.android.view.visualization.Color;
 import org.ros.android.view.visualization.Vertices;
+import org.ros.android.view.visualization.VisualizationView;
 import org.ros.message.MessageListener;
 import org.ros.namespace.GraphName;
 import org.ros.node.ConnectedNode;
 import org.ros.node.topic.Subscriber;
-import sensor_msgs.LaserScan;
 
 import java.nio.FloatBuffer;
 
 import javax.microedition.khronos.opengles.GL10;
 
+import sensor_msgs.LaserScan;
+
 /**
  * A {@link SubscriberLayer} that visualizes sensor_msgs/LaserScan messages.
  * 
@@ -116,7 +117,7 @@ public class LaserScanLayer extends SubscriberLayer<sensor_msgs.LaserScan> imple
     vertexBackBuffer.position(0);
     synchronized (mutex) {
       FloatBuffer tmp = vertexFrontBuffer;
-      LaserScanLayer.this.vertexFrontBuffer = vertexBackBuffer;
+      vertexFrontBuffer = vertexBackBuffer;
       vertexBackBuffer = tmp;
     }
   }

android_15/src/org/ros/android/view/visualization/shape/TextShapeFactory.java

@@ -1,23 +1,30 @@
 package org.ros.android.view.visualization.shape;
 
+import android.graphics.Typeface;
+
 import org.ros.android.view.visualization.VisualizationView;
-import uk.co.blogspot.fractiousg.texample.GLText;
 
 import javax.microedition.khronos.opengles.GL10;
 
+import uk.co.blogspot.fractiousg.texample.GLText;
+
 public class TextShapeFactory {
 
   private final GLText glText;
 
-  public TextShapeFactory(VisualizationView view, GL10 gl) {
+  public TextShapeFactory(final VisualizationView view, final GL10 gl) {
     glText = new GLText(gl, view.getContext().getAssets());
   }
 
-  public void loadFont(String file, int size, int padX, int padY) {
+  public void loadFont(final Typeface typeface, final int size, final int padX, final int padY) {
+    glText.load(typeface, size, padX, padY);
+  }
+
+  public void loadFont(final String file, final int size, final int padX, final int padY) {
     glText.load(file, size, padX, padY);
   }
 
-  public TextShape newTextShape(String text) {
+  public TextShape newTextShape(final String text) {
     return new TextShape(glText, text);
   }
 }

android_15/src/uk/co/blogspot/fractiousg/texample/GLText.java

@@ -96,6 +96,11 @@ public class GLText {
       spaceX = 0.0f;
    }
 
+  public boolean load(String file, int size, int padX, int padY) {
+    Typeface tf = Typeface.createFromAsset( assets, file );  // Create the Typeface from Font File
+    return load(tf, size, padX, padY);
+  }
+
    //--Load Font--//
    // description
    //    this will load the specified font file, create a texture for the defined
@@ -104,14 +109,13 @@ public class GLText {
    //    file - Filename of the font (.ttf, .otf) to use. In 'Assets' folder.
    //    size - Requested pixel size of font (height)
    //    padX, padY - Extra padding per character (X+Y Axis); to prevent overlapping characters.
-   public boolean load(String file, int size, int padX, int padY) {
+   public boolean load(Typeface tf, int size, int padX, int padY) {
 
       // setup requested values
       fontPadX = padX;                                // Set Requested X Axis Padding
       fontPadY = padY;                                // Set Requested Y Axis Padding
 
-      // load the font and setup paint instance for drawing
-      Typeface tf = Typeface.createFromAsset( assets, file );  // Create the Typeface from Font File
+      // setup paint instance for drawing
       Paint paint = new Paint();                      // Create Android Paint Instance
       paint.setAntiAlias( true );                     // Enable Anti Alias
       paint.setTextSize( size );                      // Set Text Size