Example #1
0
 @Override
 public void draw(VisualizationView view, GL10 gl) {
   if (ready) {
     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.glDisableClientState(GL10.GL_VERTEX_ARRAY);
   }
 }
Example #2
0
/**
 * Renders a nav_msgs/Path as a dotted line.
 *
 * @author [email protected] (Lorenz Moesenlechner)
 * @author [email protected] (Damon Kohler)
 */
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 FloatBuffer vertexBuffer;
  private boolean ready;
  private GraphName frame;

  public PathLayer(String topic) {
    this(GraphName.of(topic));
  }

  public PathLayer(GraphName topic) {
    super(topic, "nav_msgs/Path");
    ready = false;
  }

  @Override
  public void draw(VisualizationView view, GL10 gl) {
    if (ready) {
      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.glDisableClientState(GL10.GL_VERTEX_ARRAY);
    }
  }

  @Override
  public void onStart(VisualizationView view, ConnectedNode connectedNode) {
    super.onStart(view, connectedNode);
    getSubscriber()
        .addMessageListener(
            new MessageListener<nav_msgs.Path>() {
              @Override
              public void onNewMessage(nav_msgs.Path path) {
                updateVertexBuffer(path);
                ready = true;
              }
            });
  }

  private void updateVertexBuffer(nav_msgs.Path path) {
    ByteBuffer goalVertexByteBuffer =
        ByteBuffer.allocateDirect(path.getPoses().size() * 3 * Float.SIZE / 8);
    goalVertexByteBuffer.order(ByteOrder.nativeOrder());
    vertexBuffer = goalVertexByteBuffer.asFloatBuffer();
    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());
        }
        i++;
      }
    }
    vertexBuffer.position(0);
  }

  @Override
  public GraphName getFrame() {
    return frame;
  }
}