Merge pull request #274 from adamantivm/path-to-line

Make the planner path a line instead of points

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

@@ -30,7 +30,7 @@ import java.nio.FloatBuffer;
 import javax.microedition.khronos.opengles.GL10;
 
 /**
- * Renders a nav_msgs/Path as a dotted line.
+ * Renders a nav_msgs/Path as a solid line.
  * 
  * @author moesenle@google.com (Lorenz Moesenlechner)
  * @author damonkohler@google.com (Damon Kohler)
@@ -38,9 +38,10 @@ import javax.microedition.khronos.opengles.GL10;
 public class PathLayer extends SubscriberLayer<nav_msgs.Path> implements TfLayer {
 
   private static final Color COLOR = Color.fromHexAndAlpha("03dfc9", 0.3f);
-  private static final float POINT_SIZE = 5.0f;
+  private static final float LINE_WIDTH = 4.0f;
 
   private FloatBuffer vertexBuffer;
+  private int numPoints;
   private boolean ready;
   private GraphName frame;
 
@@ -51,6 +52,7 @@ public class PathLayer extends SubscriberLayer<nav_msgs.Path> implements TfLayer
   public PathLayer(GraphName topic) {
     super(topic, "nav_msgs/Path");
     ready = false;
+    numPoints = 0;
   }
 
   @Override
@@ -59,8 +61,8 @@ public class PathLayer extends SubscriberLayer<nav_msgs.Path> implements TfLayer
       gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
       gl.glVertexPointer(3, GL10.GL_FLOAT, 0, vertexBuffer);
       COLOR.apply(gl);
-      gl.glPointSize(POINT_SIZE);
-      gl.glDrawArrays(GL10.GL_POINTS, 0, vertexBuffer.limit() / 3);
+      gl.glLineWidth(LINE_WIDTH);
+      gl.glDrawArrays(GL10.GL_LINE_STRIP, 0, numPoints);
       gl.glDisableClientState(GL10.GL_VERTEX_ARRAY);
     }
   }
@@ -79,27 +81,21 @@ public class PathLayer extends SubscriberLayer<nav_msgs.Path> implements TfLayer
 
   private void updateVertexBuffer(nav_msgs.Path path) {
     ByteBuffer goalVertexByteBuffer =
-        ByteBuffer.allocateDirect(path.getPoses().size() * 3 * Float.SIZE / 8);
+        ByteBuffer.allocateDirect(path.getPoses().size() * 3 * Float.SIZE);
     goalVertexByteBuffer.order(ByteOrder.nativeOrder());
     vertexBuffer = goalVertexByteBuffer.asFloatBuffer();
+    int i = 0;
     if (path.getPoses().size() > 0) {
       frame = GraphName.of(path.getPoses().get(0).getHeader().getFrameId());
-      // Path poses are densely packed and will make the path look like a solid
-      // line even if it is drawn as points. Skipping poses provides the visual
-      // point separation were looking for.
-      int i = 0;
       for (PoseStamped pose : path.getPoses()) {
-        // TODO(damonkohler): Choose the separation between points as a pixel
-        // value. This will require inspecting the zoom level from the camera.
-        if (i % 15 == 0) {
-          vertexBuffer.put((float) pose.getPose().getPosition().getX());
-          vertexBuffer.put((float) pose.getPose().getPosition().getY());
-          vertexBuffer.put((float) pose.getPose().getPosition().getZ());
-        }
+        vertexBuffer.put((float) pose.getPose().getPosition().getX());
+        vertexBuffer.put((float) pose.getPose().getPosition().getY());
+        vertexBuffer.put((float) pose.getPose().getPosition().getZ());
         i++;
       }
     }
     vertexBuffer.position(0);
+    numPoints = i;
   }
 
   @Override