Refactoring of the map view.

 * Everything is rendered into a texture now.
 * Changed the origin and units in OpenGL display to correspond to real world
   coordinates.
 * Got rid of some strange states in handling of touch events.

android_honeycomb_mr2/manifest.xml

@@ -13,6 +13,7 @@
   <depend package="android_gingerbread" />
   <depend package="sensor_msgs" />
   <depend package="nav_msgs" />
+  <depend package="rosjava_geometry" />
 
   <export>
     <rosjava-android-lib target="android-13" />

android_honeycomb_mr2/src/org/ros/android/views/map/Goal.java

@@ -17,7 +17,8 @@
 package org.ros.android.views.map;
 
 import org.ros.message.geometry_msgs.Pose;
-import org.ros.message.geometry_msgs.Quaternion;
+import org.ros.message.geometry_msgs.Vector3;
+import org.ros.rosjava_geometry.Geometry;
 
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
@@ -35,24 +36,24 @@ public class Goal implements OpenGlDrawable {
    */
   protected FloatBuffer vertexBuffer;
   protected Pose pose;
-  protected float theta;
-  protected int goalIndexCount;
-
-  public Goal() {
-    pose = new Pose();
-  }
 
   @Override
   public void draw(GL10 gl) {
+    if (pose == null) {
+      return;
+    }
     gl.glPushMatrix();
     gl.glDisable(GL10.GL_CULL_FACE);
     gl.glFrontFace(GL10.GL_CW);
     gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
     gl.glVertexPointer(3, GL10.GL_FLOAT, 0, vertexBuffer);
     gl.glTranslatef((float) pose.position.x, (float) pose.position.y, 0);
-    gl.glRotatef(theta - 90, 0, 0, 1);
+    Vector3 axis = Geometry.calculateRotationAxis(pose.orientation);
+    gl.glRotatef((float) Math.toDegrees(Geometry.calculateRotationAngle(pose.orientation)),
+        (float) axis.x, (float) axis.y, (float) axis.z);
+    gl.glRotatef(180, 0, 0, 1);
     gl.glColor4f(0.180392157f, 0.71372549f, 0.909803922f, 0.5f);
-    gl.glDrawArrays(GL10.GL_TRIANGLE_FAN, 0, getGoalIndexCount());
+    gl.glDrawArrays(GL10.GL_TRIANGLE_FAN, 0, vertexBuffer.limit() / 3);
     gl.glDisableClientState(GL10.GL_VERTEX_ARRAY);
     gl.glPopMatrix();
   }
@@ -63,32 +64,32 @@ public class Goal implements OpenGlDrawable {
     goalVertices[1] = 0f;
     goalVertices[2] = 0f;
     goalVertices[3] = 0f;
-    goalVertices[4] = 14f;
+    goalVertices[4] = 0.07f;
     goalVertices[5] = 0f;
-    goalVertices[6] = 2f;
-    goalVertices[7] = 2f;
+    goalVertices[6] = 0.1f;
+    goalVertices[7] = 0.1f;
     goalVertices[8] = 0f;
-    goalVertices[9] = 7f;
+    goalVertices[9] = 0.35f;
     goalVertices[10] = 0f;
     goalVertices[11] = 0f;
-    goalVertices[12] = 2f;
-    goalVertices[13] = -2f;
+    goalVertices[12] = 0.1f;
+    goalVertices[13] = -0.1f;
     goalVertices[14] = 0f;
     goalVertices[15] = 0f;
-    goalVertices[16] = -7f;
+    goalVertices[16] = -0.35f;
     goalVertices[17] = 0f;
-    goalVertices[18] = -2f;
-    goalVertices[19] = -2f;
+    goalVertices[18] = -0.1f;
+    goalVertices[19] = -0.1f;
     goalVertices[20] = 0f;
-    goalVertices[21] = -7f;
+    goalVertices[21] = -0.35f;
     goalVertices[22] = 0f;
     goalVertices[23] = 0f;
-    goalVertices[24] = -2f;
-    goalVertices[25] = 2f;
+    goalVertices[24] = -0.1f;
+    goalVertices[25] = 0.1f;
     goalVertices[26] = 0f;
     // Repeat of point 1
     goalVertices[27] = 0f;
-    goalVertices[28] = 14f;
+    goalVertices[28] = 0.07f;
     goalVertices[29] = 0f;
     ByteBuffer goalVertexByteBuffer =
         ByteBuffer.allocateDirect(goalVertices.length * Float.SIZE / 8);
@@ -96,17 +97,6 @@ public class Goal implements OpenGlDrawable {
     vertexBuffer = goalVertexByteBuffer.asFloatBuffer();
     vertexBuffer.put(goalVertices);
     vertexBuffer.position(0);
-    setGoalIndexCount(goalVertices.length / 3);
-  }
-
-  private float calculateHeading(Quaternion orientation) {
-    double heading;
-    double w = orientation.w;
-    double x = orientation.x;
-    double y = orientation.z;
-    double z = orientation.y;
-    heading = Math.atan2(2 * y * w - 2 * x * z, x * x - y * y - z * z + w * w) * 180 / Math.PI;
-    return (float) heading;
   }
 
   /**
@@ -117,18 +107,7 @@ public class Goal implements OpenGlDrawable {
    * @param res
    *          The resolution of the map
    */
-  public void updatePose(Pose pose, float res) {
+  public void updatePose(Pose pose) {
     this.pose = pose;
-    this.pose.position.x /= res;
-    this.pose.position.y /= res;
-    theta = calculateHeading(pose.orientation);
-  }
-
-  public int getGoalIndexCount() {
-    return goalIndexCount;
-  }
-
-  public void setGoalIndexCount(int goalIndexCount) {
-    this.goalIndexCount = goalIndexCount;
   }
 }

android_honeycomb_mr2/src/org/ros/android/views/map/InteractionMode.java

@@ -25,14 +25,7 @@ enum InteractionMode {
   // When the user starts moves the map but the distance moved is less than
   // FINAL_MAP_MODE_DISTANCE_THRESHOLD.
   MOVE_MAP,
-  // When the user starts moves the map and the distance moved is greater than
-  // FINAL_MAP_MODE_DISTANCE_THRESHOLD.
-  MOVE_MAP_FINAL_MODE,
-  // When the user is zooming in/out.
-  ZOOM_MAP,
   // When the user is trying to specify a location (either a goal or initial
   // pose).
-  SPECIFY_LOCATION,
-  // When the user is trying to select a region.
-  SELECT_REGION
+  SPECIFY_LOCATION
 }

android_honeycomb_mr2/src/org/ros/android/views/map/MapPoints.java

@@ -52,7 +52,6 @@ class MapPoints {
     // Initialize the other components of the OpenGL display (if needed).
     if (!ready) {
       initRobot();
-      initRobotOutline();
       initCurrentGoal();
       initUserGoal();
       initPath();
@@ -61,8 +60,8 @@ class MapPoints {
     }
   }
 
-  public void updatePath(org.ros.message.nav_msgs.Path newPath, float res) {
-    path.update(newPath, res);
+  public void updatePath(org.ros.message.nav_msgs.Path newPath) {
+    path.update(newPath);
   }
 
   public void drawMap(GL10 gl) {
@@ -77,18 +76,6 @@ class MapPoints {
     }
   }
 
-  /**
-   * Renders the region currently selected by the user as a rectangle.
-   * 
-   * @param gl
-   *          Instance of the GL interface.
-   */
-  public void drawRegion(GL10 gl) {
-    if (ready) {
-      region.draw(gl);
-    }
-  }
-
   /**
    * Renders the footprint of the robot and the outline of the robot's footprint
    * based on the current zoom level. It compensates for the zoom level allowing
@@ -135,20 +122,16 @@ class MapPoints {
     }
   }
 
-  public void updateRobotPose(Pose pose, float res) {
-    robot.updatePose(pose, res);
-  }
-
-  public void updateCurrentGoalPose(Pose pose, float res) {
-    currentGoal.updatePose(pose, res);
+  public void updateRobotPose(Pose pose) {
+    robot.updatePose(pose);
   }
 
-  public void updateUserGoalLocation(Point realWorldLocation) {
-    userGoal.updateUserGoalLocation(realWorldLocation);
+  public void updateCurrentGoalPose(Pose pose) {
+    currentGoal.updatePose(pose);
   }
 
-  public void updateUserGoalOrientation(float theta) {
-    userGoal.updateUserGoalOrientation(theta);
+  public void updateUserGoal(Pose goal) {
+    userGoal.updateUserGoal(goal);
   }
 
   /**
@@ -169,10 +152,6 @@ class MapPoints {
     robot.initFootprint();
   }
 
-  private void initRobotOutline() {
-    robot.initOutline();
-  }
-
   private void initCurrentGoal() {
     currentGoal.init();
   }

android_honeycomb_mr2/src/org/ros/android/views/map/MapRenderer.java

@@ -16,13 +16,12 @@
 
 package org.ros.android.views.map;
 
-import android.graphics.Point;
 import android.opengl.GLSurfaceView;
-import android.opengl.GLU;
+import org.ros.message.geometry_msgs.Point;
 import org.ros.message.geometry_msgs.Pose;
-import org.ros.message.geometry_msgs.Quaternion;
 import org.ros.message.nav_msgs.OccupancyGrid;
 import org.ros.message.nav_msgs.Path;
+import org.ros.rosjava_geometry.Geometry;
 
 import javax.microedition.khronos.egl.EGLConfig;
 import javax.microedition.khronos.opengles.GL10;
@@ -34,25 +33,13 @@ import javax.microedition.khronos.opengles.GL10;
  */
 class MapRenderer implements GLSurfaceView.Renderer {
   /**
-   * Most the user can zoom in (distance of the camera from origin) in the map
-   * centric view.
+   * Most the user can zoom in.
    */
-  private static final float MIN_ZOOM_MAP_CENTRIC_MODE = -70;
+  private static final float MIN_ZOOM_SCALE_FACTOR = 0.01f;
   /**
-   * Most the user can zoom out (distance of the camera from origin) in the map
-   * centric view.
+   * Most the user can zoom out.
    */
-  private static final float MAX_ZOOM_MAP_CENTRIC_MODE = -400;
-  /**
-   * Most the user can zoom in (distance of the camera from origin) in the robot
-   * centric view.
-   */
-  private static final float MIN_ZOOM_ROBOT_CENTRIC_MODE = -100;
-  /**
-   * Most the user can zoom out (distance of the camera from origin) in the
-   * robot centric view.
-   */
-  private static final float MAX_ZOOM_ROBOT_CENTRIC_MODE = -400;
+  private static final float MAX_ZOOM_SCALE_FACTOR = 1.0f;
   /**
    * Instance of the helper class that draws the map, robot, etc.
    */
@@ -61,20 +48,19 @@ class MapRenderer implements GLSurfaceView.Renderer {
    * Real world (x,y) coordinates of the camera. The depth (z-axis) is based on
    * {@link #zoom}.
    */
-  private Point cameraLocation = new Point(0, 0);
+  private Point cameraPoint = new Point();
   /**
-   * The max x and y coordinates of the map in meters. In the current
-   * Implementation the assumption is that the 0,0 cell of the map is at the
-   * origin of the coordinates system.
-   * 
-   * TODO: This is not a necessary assumption and should not be required.
+   * The minimal x and y coordinates of the map in meters.
+   */
+  private Point topLeftMapPoint = new Point();
+  /**
+   * The maximal x and y coordinates of the map in meters.
    */
-  private Point maxCoords = new Point();
+  private Point bottomRightMapPoint = new Point();
   /**
-   * The distance of the OpenGL camera from the origin. By default it will
-   * zoomed in all the way.
+   * The current zoom factor used to scale the world.
    */
-  private float zoom = MIN_ZOOM_MAP_CENTRIC_MODE;
+  private float scalingFactor = 0.1f;
   /**
    * True when the map is supposed to be in the robot centric mode and false
    * when the map is supposed to be in the map centric mode.
@@ -84,42 +70,34 @@ class MapRenderer implements GLSurfaceView.Renderer {
    * True when the camera should follow the robot in the map centric mode, false
    * otherwise.
    */
-  private boolean centerOnRobot = true;
+  private boolean centerOnRobot = false;
   /**
-   * Robot's x coordinate in the real world.
+   * The Robot pose.
    */
-  private float robotX;
-  /**
-   * Robot's y coordinate in the real world.
-   */
-  private float robotY;
-  /**
-   * Robot's orientation (in radians) coordinate in the real world.
-   */
-  private float robotTheta;
+  private Pose robotPose;
   /**
    * True when the icon for the user to specify a goal must be shown, false
    * otherwise.
    */
   private boolean showUserGoal;
-  /**
-   * True is the regions should be shown, false otherwise.
-   */
-  private boolean showRegion;
 
-  private int viewportHalfWidth;
+  private android.graphics.Point viewport;
 
   @Override
-  public void onSurfaceChanged(GL10 gl, int w, int h) {
-    gl.glViewport(0, 0, w, h);
+  public void onSurfaceChanged(GL10 gl, int width, int height) {
+    gl.glViewport(0, 0, width, height);
     gl.glMatrixMode(GL10.GL_PROJECTION);
     gl.glLoadIdentity();
-    // The aspect ratio is currently set to 1.
-    GLU.gluPerspective(gl, 60.0f, 1f, 1f, 450.0f);
-    viewportHalfWidth = w / 2;
+    gl.glOrthof(-width / 2, -height / 2, width / 2, height / 2, -10.0f, 10.0f);
+    viewport = new android.graphics.Point(width, height);
     gl.glMatrixMode(GL10.GL_MODELVIEW);
     gl.glLoadIdentity();
+    gl.glBlendFunc(GL10.GL_SRC_ALPHA, GL10.GL_ONE_MINUS_SRC_ALPHA);
+    gl.glEnable(GL10.GL_BLEND);
     gl.glHint(GL10.GL_POLYGON_SMOOTH_HINT, GL10.GL_NICEST);
+    gl.glDisable(GL10.GL_LIGHTING);
+    gl.glDisable(GL10.GL_DEPTH_TEST);
+    gl.glEnable(GL10.GL_COLOR_MATERIAL);
   }
 
   @Override
@@ -129,96 +107,67 @@ class MapRenderer implements GLSurfaceView.Renderer {
 
   @Override
   public void onDrawFrame(GL10 gl) {
-    gl.glClearColor(0, 0, 0.0f, 0.0f);
+    gl.glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
     gl.glClear(GL10.GL_COLOR_BUFFER_BIT);
     gl.glLoadIdentity();
-    // If the map view is supposed to be robot centric then move the
-    // camera to the x,y coordinate of the robot and rotate it so it aligns with
-    // the robot's orientation.
-    if (robotCentricMode) {
-      gl.glRotatef(robotTheta - 90, 0, 0, -1f);
-      gl.glTranslatef(robotX - getCenterCoordinates(), robotY - getCenterCoordinates(), zoom);
-    } else {
-      gl.glRotatef(0, 0, 0, -1f);
-      gl.glTranslatef(cameraLocation.x, cameraLocation.y, zoom);
-    }
+    // We need to negate cameraLocation.x because at this point, in the OpenGL
+    // coordinate system, x is pointing left.
+    gl.glScalef(scalingFactor, scalingFactor, 1);
+    gl.glRotatef(90, 0, 0, 1);
+    gl.glTranslatef((float) -cameraPoint.x, (float) -cameraPoint.y, (float) -cameraPoint.z);
     map.drawMap(gl);
     map.drawPath(gl);
     map.drawCurrentGoal(gl);
-    if (showRegion) {
-      map.drawRegion(gl);
-    }
-    map.drawRobot(gl, zoom / MIN_ZOOM_MAP_CENTRIC_MODE);
+    map.drawRobot(gl, 1.0f / scalingFactor);
     if (showUserGoal) {
-      map.drawUserGoal(gl, zoom / MIN_ZOOM_MAP_CENTRIC_MODE);
-    }
+      map.drawUserGoal(gl, 1.0f / scalingFactor);
+     }
   }
 
   public void updateMap(OccupancyGrid newMap) {
     map.updateMap(newMap);
-    maxCoords.x = (int) -newMap.info.width;
-    maxCoords.y = (int) -newMap.info.height;
+    topLeftMapPoint.x = (float) newMap.info.origin.position.x;
+    topLeftMapPoint.y = (float) newMap.info.origin.position.y;
+    bottomRightMapPoint.x = (float) topLeftMapPoint.x + newMap.info.width * newMap.info.resolution;
+    bottomRightMapPoint.y = (float) topLeftMapPoint.y + newMap.info.height * newMap.info.resolution;
   }
 
-  public void updatePath(Path path, float res) {
-    map.updatePath(path, res);
+  public void updatePath(Path path) {
+    map.updatePath(path);
   }
 
-  /**
-   * Draw the region selection box based on the specified pixel coordinates.
-   */
-  public void drawRegion(int x1, int y1, int x2, int y2) {
-    showRegion = true;
-    map.updateRegion(getWorldCoordinate(x1, y1), getWorldCoordinate(x2, y2));
+  public void moveCamera(android.graphics.Point point) {
+    // Point is the relative movement in pixels on the viewport. We need to
+    // scale this by width end height of the viewport.
+    cameraPoint.x += (float) point.y / viewport.y / scalingFactor;
+    cameraPoint.y += (float) point.x / viewport.x / scalingFactor;
+    disableCenterOnRobot();
   }
 
-  public void moveCamera(Point point) {
-    // Since the movement can be relative and need not be based on the real
-    // world coordinates, the change in coordinates is divided by 3 to maintain
-    // a descent panning pace.
-    cameraLocation.x += point.x / 3;
-    cameraLocation.y -= point.y / 3;
-    // Bounds checking to prevent the user from panning too much.
-    if (cameraLocation.x > 0) {
-      cameraLocation.x = 0;
-    } else if (cameraLocation.x < maxCoords.x) {
-      cameraLocation.x = maxCoords.x;
-    }
-    if (cameraLocation.y > 0) {
-      cameraLocation.y = 0;
-    } else if (cameraLocation.y < maxCoords.y) {
-      cameraLocation.y = maxCoords.y;
+  public void zoomCamera(float zoomLevel) {
+    scalingFactor *= zoomLevel;
+    if (scalingFactor < MIN_ZOOM_SCALE_FACTOR) {
+      scalingFactor = MIN_ZOOM_SCALE_FACTOR;
+    } else if (scalingFactor > MAX_ZOOM_SCALE_FACTOR) {
+      scalingFactor = MAX_ZOOM_SCALE_FACTOR;
     }
-    // Disabling the centerOnRobot when the user moves the map is similar to the
-    // navigation app in android.
-    centerOnRobot = false;
   }
 
-  public void zoomCamera(float zoomLevel) {
-    // Diving by 3 to maintain a steady pace for zooming in/out.
-    zoom += zoomLevel / 3f;
-    if (robotCentricMode) {
-      if (zoom < MAX_ZOOM_ROBOT_CENTRIC_MODE) {
-        zoom = MAX_ZOOM_ROBOT_CENTRIC_MODE;
-      } else if (zoom > MIN_ZOOM_ROBOT_CENTRIC_MODE) {
-        zoom = MIN_ZOOM_ROBOT_CENTRIC_MODE;
-      }
-    } else {
-      if (zoom < MAX_ZOOM_MAP_CENTRIC_MODE) {
-        zoom = MAX_ZOOM_MAP_CENTRIC_MODE;
-      } else if (zoom > MIN_ZOOM_MAP_CENTRIC_MODE) {
-        zoom = MIN_ZOOM_MAP_CENTRIC_MODE;
-      }
-    }
+  public void enableCenterOnRobot() {
+    centerOnRobot = true;
+    centerOnRobot();
   }
 
-  public void hideRegion() {
-    showRegion = false;
+  public void disableCenterOnRobot() {
+    centerOnRobot = false;
   }
 
-  public void enableCenterOnRobot() {
-    centerOnRobot = true;
-    centerCamera();
+  public boolean centerOnRobotEnabled() {
+    return centerOnRobot;
+  }
+
+  public void toggleCenterOnRobot() {
+    centerOnRobot = !centerOnRobot;
   }
 
   public void userGoalVisible(boolean visibility) {
@@ -234,13 +183,30 @@ class MapRenderer implements GLSurfaceView.Renderer {
    *          Coordinate of the view in pixels.
    * @return Real world coordinate.
    */
-  public Point getWorldCoordinate(float x, float y) {
-    Point realCoord = new Point();
-    float multiplier = (float) (Math.tan(30 * Math.PI / 180f) * (-zoom));
-    float onePixelToMeter = multiplier / viewportHalfWidth;
-    realCoord.x = cameraLocation.x - (int) ((viewportHalfWidth - x) * onePixelToMeter);
-    realCoord.y = cameraLocation.y - (int) ((viewportHalfWidth - y) * onePixelToMeter);
-    return realCoord;
+  public Point toOpenGLCoordinates(android.graphics.Point screenPoint) {
+    Point worldCoordinate = new Point();
+    worldCoordinate.x =
+        (0.5 - (double) screenPoint.y / viewport.y) / (0.5 * scalingFactor) + cameraPoint.x;
+    worldCoordinate.y =
+        (0.5 - (double) screenPoint.x / viewport.x) / (0.5 * scalingFactor) + cameraPoint.y;
+    worldCoordinate.z = 0;
+    return worldCoordinate;
+  }
+
+  /**
+   * Returns the pose in the OpenGL world that corresponds to a screen
+   * coordinate and an orientation.
+   * 
+   * @param goalScreenPoint
+   *          the point on the screen
+   * @param orientation
+   *          the orientation of the pose on the screen
+   */
+  public Pose toOpenGLPose(android.graphics.Point goalScreenPoint, float orientation) {
+    Pose goal = new Pose();
+    goal.position = toOpenGLCoordinates(goalScreenPoint);
+    goal.orientation = Geometry.axisAngleToQuaternion(0, 0, -1, Math.PI + orientation);
+    return goal;
   }
 
   /**
@@ -248,18 +214,12 @@ class MapRenderer implements GLSurfaceView.Renderer {
    * 
    * @param pose
    *          Robot's pose in real world coordinate.
-   * @param res
-   *          The resolution of the current map in meters/cell.
    */
-  public void updateRobotPose(Pose pose, float res) {
-    map.updateRobotPose(pose, res);
-    robotX = (float) (-pose.position.x);
-    robotY = (float) (-pose.position.y);
-    robotTheta = calculateHeading(pose.orientation);
-    // If the camera is supposed to be centered on the robot then move the
-    // camera to the same coordinates as the robot.
+  public void updateRobotPose(Pose pose) {
+    robotPose = pose;
+    map.updateRobotPose(pose);
     if (centerOnRobot) {
-      centerCamera();
+      centerOnRobot();
     }
   }
 
@@ -272,66 +232,19 @@ class MapRenderer implements GLSurfaceView.Renderer {
    */
   public void setViewMode(boolean isRobotCentricMode) {
     robotCentricMode = isRobotCentricMode;
-    if (robotCentricMode) {
-      if (zoom < MAX_ZOOM_ROBOT_CENTRIC_MODE) {
-        zoom = MAX_ZOOM_ROBOT_CENTRIC_MODE;
-      } else if (zoom > MIN_ZOOM_ROBOT_CENTRIC_MODE) {
-        zoom = MIN_ZOOM_ROBOT_CENTRIC_MODE;
-      }
-    }
-  }
-
-  public void updateCurrentGoalPose(Pose goalPose, float res) {
-    map.updateCurrentGoalPose(goalPose, res);
-  }
-
-  public void updateUserGoalLocation(Point goalScreenPoint) {
-    map.updateUserGoalLocation(getWorldCoordinate(goalScreenPoint.x, goalScreenPoint.y));
-  }
-
-  public void updateUserGoalOrientation(float theta) {
-    map.updateUserGoalOrientation(theta);
   }
 
-  /**
-   * Updates the coordinates of the map.
-   * 
-   * @param x
-   *          The real world x coordinate for the camera.
-   * @param y
-   *          The real world y coordinate for the camera.
-   */
-  public void moveCamera(float x, float y) {
-    cameraLocation.x = (int) x;
-    cameraLocation.y = (int) y;
-  }
-
-  /**
-   * Returns the heading (in degree) from the quaternion passed to it.
-   */
-  private float calculateHeading(Quaternion orientation) {
-    double w = orientation.w;
-    double x = orientation.x;
-    double y = orientation.z;
-    double z = orientation.y;
-    return (float) Math.toDegrees(Math.atan2(2 * y * w - 2 * x * z, x * x - y * y - z * z + w * w));
+  public void updateCurrentGoalPose(Pose goalPose) {
+    map.updateCurrentGoalPose(goalPose);
   }
 
-  /**
-   * Returns the real world coordinates that matches the center of the viewport.
-   */
-  private float getCenterCoordinates() {
-    float multiplier = (float) (Math.tan(Math.toRadians(30)) * (-zoom));
-    float oneMeterToPixel = viewportHalfWidth / multiplier;
-    return oneMeterToPixel;
+  public void centerOnRobot() {
+    if (robotPose != null) {
+      cameraPoint = robotPose.position;
+    }
   }
 
-  /**
-   * Sets the camera coordinates to that of the robot's. Hence the camera starts
-   * to follow the robot. Though the camera orientation is not changed.
-   */
-  private void centerCamera() {
-    cameraLocation.x = (int) robotX;
-    cameraLocation.y = (int) robotY;
+  public void updateUserGoal(Pose goal) {
+    map.updateUserGoal(goal);
   }
 }

android_honeycomb_mr2/src/org/ros/android/views/map/MapView.java

@@ -16,8 +16,6 @@
 
 package org.ros.android.views.map;
 
-import com.google.common.base.Preconditions;
-
 import android.content.Context;
 import android.graphics.PixelFormat;
 import android.graphics.Point;
@@ -29,8 +27,6 @@ import android.view.View.OnTouchListener;
 import org.ros.message.MessageListener;
 import org.ros.message.geometry_msgs.PoseStamped;
 import org.ros.message.geometry_msgs.PoseWithCovarianceStamped;
-import org.ros.message.geometry_msgs.Quaternion;
-import org.ros.message.nav_msgs.MapMetaData;
 import org.ros.message.nav_msgs.OccupancyGrid;
 import org.ros.message.nav_msgs.Path;
 import org.ros.node.Node;
@@ -67,11 +63,6 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
    * Topic name for the subscribed path.
    */
   private static final String PATH_TOPIC = "~global_plan";
-  /**
-   * If the contact on the view moves more than this distance in pixels the
-   * interaction mode is switched to MOVE_MAP_FINAL_MODE.
-   */
-  private static final float FINAL_MAP_MODE_DISTANCE_THRESHOLD = 20f;
   /**
    * Time in milliseconds after which taps are not considered to be double taps.
    */
@@ -81,6 +72,11 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
    * moving to trigger a press and hold gesture.
    */
   private static final int PRESS_AND_HOLD_TIME = 600;
+  /**
+   * Threshold to indicate that the map should be moved. If the user taps and
+   * moves at least that distance, we enter the map move mode.
+   */
+  private static final int MOVE_MAP_DISTANCE_THRESHOLD = 10;
   /**
    * The OpenGL renderer that creates and manages the surface.
    */
@@ -92,17 +88,11 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
    * specifying the goal point for the robot to autonomously navigate to.
    */
   private boolean initialPoseMode;
-  /**
-   * Information (resolution, width, height, etc) about the map.
-   */
-  private MapMetaData mapMetaData = new MapMetaData();
   /**
    * Time in milliseconds when the last contact down occurred. Used to determine
    * a double tap event.
    */
   private long previousContactDownTime;
-  private int goalHeaderSequence;
-  private boolean firstMapRendered;
   /**
    * Records the on-screen location (in pixels) of the contact down event. Later
    * when it is determined that the user was specifying a destination this
@@ -136,19 +126,21 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
       // This might help with the somewhat scattered removeCallbacks.
       longPressHandler.removeCallbacks(longPressRunnable);
       // The user is trying to specify a location to the robot.
-      currentInteractionMode = InteractionMode.SPECIFY_LOCATION;
-      // Show the goal icon.
-      mapRenderer.userGoalVisible(true);
-      // Move the goal icon to the correct location in the map.
-      mapRenderer.updateUserGoalLocation(goalContact);
-      requestRender();
+      if (currentInteractionMode == InteractionMode.INVALID) {
+        currentInteractionMode = InteractionMode.SPECIFY_LOCATION;
+        // Show the goal icon.
+        mapRenderer.userGoalVisible(true);
+        // Move the goal icon to the correct location in the map.
+        mapRenderer.updateUserGoal(mapRenderer.toOpenGLPose(goalContact, 0));
+        requestRender();
+      }
     }
   };
 
   public MapView(Context context) {
     super(context);
     mapRenderer = new MapRenderer();
-    setEGLConfigChooser(8, 8, 8, 8, 16, 0);
+    setEGLConfigChooser(8, 8, 8, 8, 0, 0);
     getHolder().setFormat(PixelFormat.TRANSLUCENT);
     setZOrderOnTop(true);
     setRenderer(mapRenderer);
@@ -177,13 +169,6 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
               public void run() {
                 // Show the map.
                 mapRenderer.updateMap(map);
-                mapMetaData = map.info;
-                // If this is the first time map data is received then center
-                // the camera on the map.
-                if (!firstMapRendered) {
-                  mapRenderer.moveCamera(-mapMetaData.width / 2, -mapMetaData.height / 2);
-                  firstMapRendered = true;
-                }
                 requestRender();
               }
             });
@@ -199,7 +184,7 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
               public void run() {
                 poseFrameId = message.header.frame_id;
                 // Update the robot's location on the map.
-                mapRenderer.updateRobotPose(message.pose, mapMetaData.resolution);
+                mapRenderer.updateRobotPose(message.pose);
                 requestRender();
               }
             });
@@ -214,7 +199,7 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
               @Override
               public void run() {
                 // Update the location of the current goal on the map.
-                mapRenderer.updateCurrentGoalPose(goal.pose, mapMetaData.resolution);
+                mapRenderer.updateCurrentGoalPose(goal.pose);
                 requestRender();
               }
             });
@@ -228,7 +213,7 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
           @Override
           public void run() {
             // Update the path on the map.
-            mapRenderer.updatePath(path, mapMetaData.resolution);
+            mapRenderer.updatePath(path);
           }
         });
       }
@@ -259,11 +244,6 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
           // state machine.
           resetInteractionState();
         }
-        // If the annotate mode is not selected.
-        else if (currentInteractionMode != InteractionMode.SELECT_REGION) {
-          // Assume that the user is trying to zoom the map.
-          currentInteractionMode = InteractionMode.ZOOM_MAP;
-        }
         previousContact[1].x = (int) event.getX(event.getActionIndex());
         previousContact[1].y = (int) event.getY(event.getActionIndex());
         break;
@@ -305,42 +285,27 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
     initialPoseMode = true;
   }
 
-  /**
-   * Temporarily enables the annotate region mode. In this mode user can select
-   * a region of interest by putting two fingers down.
-   * 
-   * TODO: Currently region information is not stored or used in any way. The
-   * map coordinates of the selected region are known and can either be stored
-   * or published along with additional information like region name.
-   */
-  public void annotateRegion() {
-    currentInteractionMode = InteractionMode.SELECT_REGION;
-  }
-
   private void contactMove(MotionEvent event) {
     // If only one contact is on the view.
     if (event.getPointerCount() == 1) {
       // And the user is moving the map.
-      if (currentInteractionMode == InteractionMode.MOVE_MAP
-          || currentInteractionMode == InteractionMode.MOVE_MAP_FINAL_MODE) {
+      if (currentInteractionMode == InteractionMode.INVALID) {
+        if (calcDistance(event.getX(), event.getY(), previousContact[0].x, previousContact[0].y) > MOVE_MAP_DISTANCE_THRESHOLD) {
+          currentInteractionMode = InteractionMode.MOVE_MAP;
+          longPressHandler.removeCallbacks(longPressRunnable);
+          return;
+        }
+      }
+      if (currentInteractionMode == InteractionMode.MOVE_MAP) {
         // Move the map.
         mapRenderer.moveCamera(new Point((int) event.getX(0) - previousContact[0].x, (int) event
             .getY(0) - previousContact[0].y));
-        // If the user moved further than some distance from the location of the
-        // contact down.
-        if (currentInteractionMode != InteractionMode.MOVE_MAP_FINAL_MODE
-            && triggerMoveFinalMode(event.getX(0), event.getY(0))) {
-          // Then enter the MOVE_MAP_FINAL_MODE mode.
-          currentInteractionMode = InteractionMode.MOVE_MAP_FINAL_MODE;
-          // And remove the press and hold callback since the user is moving the
-          // map and not trying to do a press and hold.
-          longPressHandler.removeCallbacks(longPressRunnable);
-        }
       }
       // And the user is specifying an orientation for a pose on the map.
       else if (currentInteractionMode == InteractionMode.SPECIFY_LOCATION) {
         // Set orientation of the goal pose.
-        mapRenderer.updateUserGoalOrientation(getGoalOrientation(event.getX(0), event.getY(0)));
+        mapRenderer.updateUserGoal(mapRenderer.toOpenGLPose(goalContact,
+            getGoalOrientation(event.getX(0), event.getY(0))));
       }
       // Store current contact position.
       previousContact[0].x = (int) event.getX(0);
@@ -348,20 +313,12 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
     }
     // If there are two contacts on the view.
     else if (event.getPointerCount() == 2) {
-      // In zoom mode.
-      if (currentInteractionMode == InteractionMode.ZOOM_MAP) {
-        // Zoom in/out based on the distance between locations of current
-        // contacts and previous contacts.
-        mapRenderer.zoomCamera(calcDistance(event.getX(0), event.getY(0), event.getX(1),
-            event.getY(1))
-            - calcDistance(previousContact[0].x, previousContact[0].y, previousContact[1].x,
+      // Zoom in/out based on the distance between locations of current
+      // contacts and previous contacts.
+      mapRenderer.zoomCamera(calcDistance(event.getX(0), event.getY(0), event.getX(1),
+          event.getY(1))
+            / calcDistance(previousContact[0].x, previousContact[0].y, previousContact[1].x,
                 previousContact[1].y));
-      }
-      // In select region mode.
-      else if (currentInteractionMode == InteractionMode.SELECT_REGION) {
-        mapRenderer.drawRegion(this.getWidth() - (int) event.getX(0), (int) event.getY(0),
-            this.getWidth() - (int) event.getX(1), (int) event.getY(1));
-      }
       // Update contact information.
       previousContact[0].x = (int) event.getX(0);
       previousContact[0].y = (int) event.getY(0);
@@ -374,21 +331,15 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
   }
 
   private void contactUp(MotionEvent event) {
-    Preconditions.checkNotNull(poseFrameId);
     // If the user was trying to specify a pose and just lifted the contact then
     // publish the position based on the initial contact down location and the
     // orientation based on the current contact up location.
-    if (currentInteractionMode == InteractionMode.SPECIFY_LOCATION) {
-      Point goalPoint = mapRenderer.getWorldCoordinate(goalContact.x, goalContact.y);
+    if (poseFrameId != null && currentInteractionMode == InteractionMode.SPECIFY_LOCATION) {
       PoseStamped poseStamped = new PoseStamped();
-      poseStamped.header.seq = goalHeaderSequence++;
       poseStamped.header.frame_id = poseFrameId;
       poseStamped.header.stamp = node.getCurrentTime();
-      poseStamped.pose.position.x = -goalPoint.x * mapMetaData.resolution;
-      poseStamped.pose.position.y = -goalPoint.y * mapMetaData.resolution;
-      poseStamped.pose.position.z = 0;
-      poseStamped.pose.orientation =
-          getQuaternion(Math.toRadians(getGoalOrientation(event.getX(0), event.getY(0))), 0, 0);
+      poseStamped.pose =
+          mapRenderer.toOpenGLPose(goalContact, getGoalOrientation(event.getX(), event.getY()));
       // If the user was trying to specify an initial pose.
       if (initialPoseMode) {
         PoseWithCovarianceStamped poseWithCovarianceStamped = new PoseWithCovarianceStamped();
@@ -408,8 +359,7 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
     // If it's been less than DOUBLE_TAP_TIME milliseconds since the last
     // contact down then the user just performed a double tap gesture.
     if (Calendar.getInstance().getTimeInMillis() - previousContactDownTime < DOUBLE_TAP_TIME) {
-      // Shift the viewport to center on the robot.
-      mapRenderer.enableCenterOnRobot();
+      mapRenderer.toggleCenterOnRobot();
       requestRender();
       // Further information from this contact is no longer needed.
       returnValue = false;
@@ -420,12 +370,9 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
     }
     previousContact[0].x = (int) event.getX(0);
     previousContact[0].y = (int) event.getY(0);
-    goalContact.x = this.getWidth() - previousContact[0].x;
+    goalContact.x = previousContact[0].x;
     goalContact.y = previousContact[0].y;
-    mapRenderer.getWorldCoordinate(previousContact[0].x, previousContact[0].y);
-    if (currentInteractionMode == InteractionMode.INVALID) {
-      currentInteractionMode = InteractionMode.MOVE_MAP;
-    }
+    System.out.println("goal contact: " + goalContact);
     previousContactDownTime = Calendar.getInstance().getTimeInMillis();
     return returnValue;
   }
@@ -435,7 +382,6 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
     longPressHandler.removeCallbacks(longPressRunnable);
     initialPoseMode = false;
     mapRenderer.userGoalVisible(false);
-    mapRenderer.hideRegion();
   }
 
   /**
@@ -457,51 +403,6 @@ public class MapView extends GLSurfaceView implements NodeMain, OnTouchListener
    *         degrees (0 to 360).
    */
   private float getGoalOrientation(float x, float y) {
-    return (float) (360 - Math.toDegrees(Math.atan2(y - goalContact.y,
-        x + goalContact.x - this.getWidth())));
-  }
-
-  /**
-   * Return the quaternion representing the specified heading, attitude, and
-   * bank values.
-   * 
-   * @param heading
-   *          The heading in radians.
-   * @param attitude
-   *          The attitude in radians.
-   * @param bank
-   *          The bank in radians.
-   * @return The quaternion based on the arguments passed.
-   */
-  private Quaternion getQuaternion(double heading, double attitude, double bank) {
-    // Assuming the angles are in radians.
-    double c1 = Math.cos(heading / 2);
-    double s1 = Math.sin(heading / 2);
-    double c2 = Math.cos(attitude / 2);
-    double s2 = Math.sin(attitude / 2);
-    double c3 = Math.cos(bank / 2);
-    double s3 = Math.sin(bank / 2);
-    double c1c2 = c1 * c2;
-    double s1s2 = s1 * s2;
-    Quaternion quaternion = new Quaternion();
-    // Modified math (the order for w,x,y, and z needs to be changed).
-    quaternion.w = c1c2 * c3 - s1s2 * s3;
-    quaternion.x = c1c2 * s3 + s1s2 * c3;
-    quaternion.z = s1 * c2 * c3 + c1 * s2 * s3;
-    quaternion.y = c1 * s2 * c3 - s1 * c2 * s3;
-    return quaternion;
-  }
-
-  /**
-   * Returns true if the user has moved the map beyond
-   * {@link #FINAL_MAP_MODE_DISTANCE_THRESHOLD}.
-   */
-  private boolean triggerMoveFinalMode(float currentX, float currentY) {
-    if (Math.sqrt((currentX - (this.getWidth() - goalContact.x))
-        * (currentX - (this.getWidth() - goalContact.x))
-        + (currentY - previousContact[0].y * (currentY - previousContact[0].y))) > FINAL_MAP_MODE_DISTANCE_THRESHOLD) {
-      return true;
-    }
-    return false;
+    return (float) Math.atan2(y - goalContact.y, x - goalContact.x);
   }
 }

android_honeycomb_mr2/src/org/ros/android/views/map/OccupancyGrid.java

@@ -16,11 +16,11 @@
 
 package org.ros.android.views.map;
 
+import org.ros.rosjava_geometry.Geometry;
+
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
 import java.nio.FloatBuffer;
-import java.util.ArrayList;
-import java.util.List;
 
 import javax.microedition.khronos.opengles.GL10;
 
@@ -30,108 +30,86 @@ import javax.microedition.khronos.opengles.GL10;
  * @author damonkohler@google.com (Damon Kohler)
  */
 public class OccupancyGrid implements OpenGlDrawable {
-  /**
-   * The largest number that can be represented by an unsigned short.
-   */
-  private static final int UNSIGNED_SHORT_MAX = 65535;
+  private OccupancyGridTexture texture;
+  private FloatBuffer vertexBuffer;
+  private FloatBuffer textureBuffer;
+  private org.ros.message.nav_msgs.OccupancyGrid occupancyGrid;
 
-  /**
-   * Vertices for the empty region.
-   */
-  private FloatBuffer emptyVertexBuffer;
-  /**
-   * Vertices for the occupied region.
-   */
-  private FloatBuffer occupiedVertexBuffer;
-  private int totalEmptyCells;
-  private int totalOccupiedCells;
+  public OccupancyGrid() {
+    float vertexCoordinates[] = {
+        // Triangle 1
+        0.0f, 0.0f, 0.0f, // Bottom left
+        1.0f, 0.0f, 0.0f, // Bottom right
+        0.0f, 1.0f, 0.0f, // Top left
+        // Triangle 2
+        1.0f, 0.0f, 0.0f, // Bottom right
+        0.0f, 1.0f, 0.0f, // Top left
+        1.0f, 1.0f, 0.0f, // Top right
+    };
+    ByteBuffer vertexByteBuffer = ByteBuffer.allocateDirect(vertexCoordinates.length * 4);
+    vertexByteBuffer.order(ByteOrder.nativeOrder());
+    vertexBuffer = vertexByteBuffer.asFloatBuffer();
+    vertexBuffer.put(vertexCoordinates);
+    vertexBuffer.position(0);
 
+    float textureCoordinates[] = { 
+        // Triangle 1 
+        0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f,
+        // Triangle 2
+        1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f
+        };
+    ByteBuffer textureByteBuffer = ByteBuffer.allocateDirect(textureCoordinates.length * 4);
+    textureByteBuffer.order(ByteOrder.nativeOrder());
+    textureBuffer = textureByteBuffer.asFloatBuffer();
+    textureBuffer.put(textureCoordinates);
+    textureBuffer.position(0);
+    texture = new OccupancyGridTexture();
+  }
   /**
    * Creates a new set of points to render based on the incoming occupancy grid.
    * 
-   * @param newMap
+   * @param newOccupancyGrid
    *          OccupancyGrid representing the map data.
    */
-  public void update(org.ros.message.nav_msgs.OccupancyGrid newMap) {
-    List<Float> emptyVertices = new ArrayList<Float>();
-    List<Float> occupiedVertices = new ArrayList<Float>();
-    int occupancyGridState = 0;
-    // Reset the count of empty and occupied cells.
-    totalOccupiedCells = 0;
-    totalEmptyCells = 0;
-    // Iterate over all the cells in the map.
-    for (int h = 0; h < newMap.info.height; h++) {
-      for (int w = 0; w < newMap.info.width; w++) {
-        occupancyGridState = newMap.data[(int) (newMap.info.width * h + w)];
-        // If the cell is empty.
-        if (occupancyGridState == 0) {
-          // Add the coordinates of the cell to the empty list.
-          emptyVertices.add((float) w);
-          emptyVertices.add((float) h);
-          emptyVertices.add(0f);
-          totalEmptyCells++;
-        } // If the cell is occupied.
-        else if (occupancyGridState == 100) {
-          // Add the coordinates of the cell to the occupied list.
-          occupiedVertices.add((float) w);
-          occupiedVertices.add((float) h);
-          occupiedVertices.add(0f);
-          totalOccupiedCells++;
-        }
-      }
-    }
-    // Convert the Lists to arrays.
-    float[] emptyFloatArray = new float[emptyVertices.size()];
-    for (int i = 0; i < emptyFloatArray.length; i++) {
-      emptyFloatArray[i] = emptyVertices.get(i);
-    }
-    float[] occupiedFloatArray = new float[occupiedVertices.size()];
-    for (int i = 0; i < occupiedFloatArray.length; i++) {
-      occupiedFloatArray[i] = occupiedVertices.get(i);
-    }
-    // Move the data from the float arrays to byte buffers for OpenGL
-    // consumption.
-    ByteBuffer emptyVertexByteBuffer =
-        ByteBuffer.allocateDirect(emptyVertices.size() * Float.SIZE / 8);
-    emptyVertexByteBuffer.order(ByteOrder.nativeOrder());
-    emptyVertexBuffer = emptyVertexByteBuffer.asFloatBuffer();
-    emptyVertexBuffer.put(emptyFloatArray);
-    emptyVertexBuffer.position(0);
-    ByteBuffer occupiedVertexByteBuffer =
-        ByteBuffer.allocateDirect(occupiedVertices.size() * Float.SIZE / 8);
-    occupiedVertexByteBuffer.order(ByteOrder.nativeOrder());
-    occupiedVertexBuffer = occupiedVertexByteBuffer.asFloatBuffer();
-    occupiedVertexBuffer.put(occupiedFloatArray);
-    occupiedVertexBuffer.position(0);
+  public void update(org.ros.message.nav_msgs.OccupancyGrid newOccupancyGrid) {
+    occupancyGrid = newOccupancyGrid;
+    texture.updateTextureFromOccupancyGrid(newOccupancyGrid);
   }
 
   @Override
   public void draw(GL10 gl) {
-    gl.glEnable(GL10.GL_POINT_SMOOTH);
-    gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
-    gl.glPointSize(5);
-    // Draw empty regions.
-    gl.glVertexPointer(3, GL10.GL_FLOAT, 0, emptyVertexBuffer);
-    gl.glColor4f(0.5f, 0.5f, 0.5f, 0.7f);
-    // This is needed because OpenGLES only allows for a max of
-    // UNSIGNED_SHORT_MAX vertices to be read. Hence all the vertices are
-    // displayed in chunks of UNSIGNED_SHORT_MAX.
-    for (int i = 0; i < totalEmptyCells / UNSIGNED_SHORT_MAX; i++) {
-      gl.glDrawArrays(GL10.GL_POINTS, i * UNSIGNED_SHORT_MAX, (UNSIGNED_SHORT_MAX * (i + 1)));
+    if (vertexBuffer == null) {
+      return;
     }
-    // (totalEmptyCells / UNSIGNED_SHORT_MAX) * UNSIGNED_SHORT_MAX is not the
-    // same as totalEmptyCells. It's integer math.
-    gl.glDrawArrays(GL10.GL_POINTS, (totalEmptyCells / UNSIGNED_SHORT_MAX) * UNSIGNED_SHORT_MAX,
-        (totalEmptyCells % UNSIGNED_SHORT_MAX));
-    // Draw occupied regions.
-    gl.glVertexPointer(3, GL10.GL_FLOAT, 0, occupiedVertexBuffer);
-    gl.glColor4f(0.8f, 0.1f, 0.1f, 1f);
-    for (int i = 0; i < totalOccupiedCells / UNSIGNED_SHORT_MAX; i++) {
-      gl.glDrawArrays(GL10.GL_POINTS, i * UNSIGNED_SHORT_MAX, (UNSIGNED_SHORT_MAX * (i + 1)));
+    texture.maybeInitTexture(gl);
+    try {
+      gl.glBindTexture(GL10.GL_TEXTURE_2D, texture.getTextureHandle());
+    } catch (TextureNotInitialized e) {
+      // This should actually never happen since we call init on the texture
+      // first.
+      e.printStackTrace();
+      return;
     }
-    gl.glDrawArrays(GL10.GL_POINTS, (totalOccupiedCells / UNSIGNED_SHORT_MAX) * UNSIGNED_SHORT_MAX,
-        (totalOccupiedCells % UNSIGNED_SHORT_MAX));
+    gl.glPushMatrix();
+    float scaleFactor = texture.getTextureSize() * occupancyGrid.info.resolution;
+    gl.glTranslatef((float) occupancyGrid.info.origin.position.x,
+        (float) occupancyGrid.info.origin.position.y, (float) occupancyGrid.info.origin.position.z);
+    org.ros.message.geometry_msgs.Vector3 axis =
+        Geometry.calculateRotationAxis(occupancyGrid.info.origin.orientation);
+    gl.glRotatef(
+        (float) Math.toDegrees(Geometry.calculateRotationAngle(occupancyGrid.info.origin.orientation)),
+        (float) axis.x, (float) axis.y, (float) axis.z);
+    gl.glScalef(scaleFactor, scaleFactor, 1.0f);
+    gl.glEnable(GL10.GL_TEXTURE_2D);
+    gl.glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
+    gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
+    gl.glEnableClientState(GL10.GL_TEXTURE_COORD_ARRAY);
+    gl.glVertexPointer(3, GL10.GL_FLOAT, 0, vertexBuffer);
+    gl.glTexCoordPointer(2, GL10.GL_FLOAT, 0, textureBuffer);
+    gl.glDrawArrays(GL10.GL_TRIANGLES, 0, vertexBuffer.limit() / 3);
     gl.glDisableClientState(GL10.GL_VERTEX_ARRAY);
-    gl.glDisable(GL10.GL_POINT_SMOOTH);
+    gl.glDisableClientState(GL10.GL_TEXTURE_COORD_ARRAY);
+    gl.glDisable(GL10.GL_TEXTURE_2D);
+    gl.glPopMatrix();
   }
 }

android_honeycomb_mr2/src/org/ros/android/views/map/OccupancyGridTexture.java

@@ -0,0 +1,137 @@
+package org.ros.android.views.map;
+
+import com.google.common.base.Preconditions;
+
+import android.graphics.Bitmap;
+import android.opengl.GLUtils;
+
+import javax.microedition.khronos.opengles.GL10;
+
+public class OccupancyGridTexture {
+  /**
+   * Color of occupied cells in the map.
+   */
+  private static final int COLOR_OCCUPIED = 0xffcc1919;
+
+  /**
+   * Color of free cells in the map.
+   */
+  private static final int COLOR_FREE = 0xff7d7d7d;
+
+  /**
+   * Color of unknown cells in the map.
+   */
+  private static final int COLOR_UNKNOWN = 0xff000000;
+
+  private boolean needReload;
+  private Bitmap textureBitmap;
+  private int[] textureHandle;
+  private int textureSize;
+
+  public OccupancyGridTexture() {
+    needReload = false;
+  }
+
+  public synchronized void updateTextureFromOccupancyGrid(
+      org.ros.message.nav_msgs.OccupancyGrid occupancyGrid) {
+    needReload = true;
+    textureSize = (int) Math.max(occupancyGrid.info.width, occupancyGrid.info.height);
+    int[] squarePixelArray =
+        makeSquarePixelArray(occupancyGridToPixelArray(occupancyGrid),
+            (int) occupancyGrid.info.width, (int) occupancyGrid.info.height, textureSize,
+            COLOR_UNKNOWN);
+    textureBitmap =
+        Bitmap.createBitmap(squarePixelArray, textureSize, textureSize, Bitmap.Config.ARGB_8888);
+  }
+
+  public synchronized int getTextureHandle() throws TextureNotInitialized {
+    if (textureHandle == null || needReload) {
+      throw new TextureNotInitialized();
+    }
+    return textureHandle[0];
+  }
+
+  /**
+   * If necessary, initializes and/or reloads the texture from the previously
+   * specified occupancy grid. This needs to be called at least once before
+   * calling getTextureHandle.
+   * 
+   * @param gl
+   *          the OpenGL context
+   */
+  public void maybeInitTexture(GL10 gl) {
+    if (needReload) {
+      initTexture(gl);
+    }
+  }
+
+  public int getTextureSize() {
+    return textureSize;
+  }
+
+  private synchronized void initTexture(GL10 gl) {
+    Preconditions.checkNotNull(textureBitmap);
+    if (textureHandle == null) {
+      textureHandle = new int[1];
+      gl.glGenTextures(1, textureHandle, 0);
+    }
+    gl.glBindTexture(GL10.GL_TEXTURE_2D, textureHandle[0]);
+
+    gl.glTexParameterf(GL10.GL_TEXTURE_2D, GL10.GL_TEXTURE_MIN_FILTER, GL10.GL_NEAREST);
+    gl.glTexParameterf(GL10.GL_TEXTURE_2D, GL10.GL_TEXTURE_MAG_FILTER, GL10.GL_NEAREST);
+    gl.glTexParameterf(GL10.GL_TEXTURE_2D, GL10.GL_TEXTURE_WRAP_S, GL10.GL_REPEAT);
+    gl.glTexParameterf(GL10.GL_TEXTURE_2D, GL10.GL_TEXTURE_WRAP_T, GL10.GL_REPEAT);
+
+    GLUtils.texImage2D(GL10.GL_TEXTURE_2D, 0, textureBitmap, 0);
+    textureBitmap.recycle();
+    textureBitmap = null;
+    needReload = false;
+  }
+
+  private int[] occupancyGridToPixelArray(org.ros.message.nav_msgs.OccupancyGrid occupancyGrid) {
+    int pixels[] = new int[occupancyGrid.data.length];
+    for (int i = 0; i < occupancyGrid.data.length; i++) {
+      if (occupancyGrid.data[i] == -1) {
+        pixels[i] = COLOR_UNKNOWN;
+      } else if (occupancyGrid.data[i] == 0) {
+        pixels[i] = COLOR_FREE;
+      } else {
+        pixels[i] = COLOR_OCCUPIED;
+      }
+    }
+    return pixels;
+  }
+
+  /**
+   * Takes a pixel array representing an image of size width and height and
+   * returns a square image with side length goalSize.
+   * 
+   * @param pixels
+   *          input pixels to format
+   * @param width
+   *          width of the input array
+   * @param height
+   *          height of the input array
+   * @param outputSize
+   *          side length of the output image
+   * @param fillColor
+   *          color to use for filling additional pixels
+   * @return the new pixel array with size goalSize * goalSize
+   */
+  private int[] makeSquarePixelArray(int[] pixels, int width, int height, int outputSize,
+      int fillColor) {
+    int[] result = new int[outputSize * outputSize];
+    int maxWidth = width > outputSize ? width : outputSize;
+
+    for (int h = 0, i = 0; h < outputSize; h++) {
+      for (int w = 0; w < maxWidth; w++, i++) {
+        if (h < height && w < width) {
+          result[i] = pixels[h * width + w];
+        } else {
+          result[i] = fillColor;
+        }
+      }
+    }
+    return result;
+  }
+}

android_honeycomb_mr2/src/org/ros/android/views/map/Path.java

@@ -42,12 +42,12 @@ public class Path implements OpenGlDrawable {
    * @param resolution
    *          The resolution of the current map.
    */
-   public void update(org.ros.message.nav_msgs.Path path, float resolution) {
+  public void update(org.ros.message.nav_msgs.Path path) {
     float[] pathVertices = new float[path.poses.size() * 3];
     // Add the path coordinates to the array.
     for (int i = 0; i < path.poses.size(); i++) {
-      pathVertices[i * 3] = (float) path.poses.get(i).pose.position.x / resolution;
-      pathVertices[i * 3 + 1] = (float) path.poses.get(i).pose.position.y / resolution;
+      pathVertices[i * 3] = (float) path.poses.get(i).pose.position.x;
+      pathVertices[i * 3 + 1] = (float) path.poses.get(i).pose.position.y;
       pathVertices[i * 3 + 2] = 0f;
     }
     updateVertices(path, pathVertices);

android_honeycomb_mr2/src/org/ros/android/views/map/Robot.java

@@ -17,7 +17,8 @@
 package org.ros.android.views.map;
 
 import org.ros.message.geometry_msgs.Pose;
-import org.ros.message.geometry_msgs.Quaternion;
+import org.ros.message.geometry_msgs.Vector3;
+import org.ros.rosjava_geometry.Geometry;
 
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
@@ -38,20 +39,9 @@ public class Robot implements OpenGlDrawable {
    * Vertices for the robot's shape.
    */
   private FloatBuffer robotVertexBuffer;
-  /**
-   * Vertices for the robot's outline used while zoomed out.
-   */
-  private FloatBuffer robotOutlineVertexBuffer;
 
-  private int robotIndexCount;
   private Pose robotPose;
-  private float robotTheta;
   private float scaleFactor;
-  private int robotOutlineIndexCount;
-
-  public Robot() {
-    robotPose = new Pose();
-  }
 
   public void initFootprint() {
     float[] robotVertices = new float[12];
@@ -60,18 +50,19 @@ public class Robot implements OpenGlDrawable {
     robotVertices[0] = 0f;
     robotVertices[1] = 0f;
     robotVertices[2] = 0f;
-    // -2,-1
-    robotVertices[3] = -2f;
-    robotVertices[4] = -2f;
+
+    robotVertices[3] = -0.1f;
+    robotVertices[4] = -0.1f;
     robotVertices[5] = 0f;
-    // 2,-1
-    robotVertices[6] = 2f;
-    robotVertices[7] = -2f;
+
+    robotVertices[6] = -0.1f;
+    robotVertices[7] = 0.1f;
     robotVertices[8] = 0f;
-    // 0,5
-    robotVertices[9] = 0f;
-    robotVertices[10] = 5f;
+
+    robotVertices[9] = 0.25f;
+    robotVertices[10] = 0.0f;
     robotVertices[11] = 0f;
+
     // Indices for the robot.
     short[] robotIndices = new short[6];
     // Left triangle (counter-clockwise)
@@ -94,72 +85,31 @@ public class Robot implements OpenGlDrawable {
     robotIndexBuffer = robotIndexByteBuffer.asShortBuffer();
     robotIndexBuffer.put(robotIndices);
     robotIndexBuffer.position(0);
-    robotIndexCount = robotIndices.length;
-  }
-
-  public void initOutline() {
-    float[] robotOutlineVertices = new float[12];
-    // The arrow shaped outline of the robot.
-    // -2,-1
-    robotOutlineVertices[0] = -2f;
-    robotOutlineVertices[1] = -2f;
-    robotOutlineVertices[2] = 0f;
-    // 0,0
-    robotOutlineVertices[3] = 0f;
-    robotOutlineVertices[4] = 0f;
-    robotOutlineVertices[5] = 0f;
-    // 2,-1
-    robotOutlineVertices[6] = 2f;
-    robotOutlineVertices[7] = -2f;
-    robotOutlineVertices[8] = 0f;
-    // 0,5
-    robotOutlineVertices[9] = 0f;
-    robotOutlineVertices[10] = 5f;
-    robotOutlineVertices[11] = 0f;
-    ByteBuffer robotOutlineVertexByteBuffer =
-        ByteBuffer.allocateDirect(robotOutlineVertices.length * Float.SIZE / 8);
-    robotOutlineVertexByteBuffer.order(ByteOrder.nativeOrder());
-    robotOutlineVertexBuffer = robotOutlineVertexByteBuffer.asFloatBuffer();
-    robotOutlineVertexBuffer.put(robotOutlineVertices);
-    robotOutlineVertexBuffer.position(0);
-    robotOutlineIndexCount = 4;
   }
 
   @Override
   public void draw(GL10 gl) {
-    drawOutline(gl);
     drawFootprint(gl);
   }
 
-  // TODO(munjaldesai): The robot size should be drawn based on the robot radius
-  // or the footprint published.
   private void drawFootprint(GL10 gl) {
+    if (robotPose == null) {
+      return;
+    }
     gl.glPushMatrix();
     gl.glDisable(GL10.GL_CULL_FACE);
     gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
-    gl.glTranslatef((float) robotPose.position.x, (float) robotPose.position.y, 0);
-    gl.glRotatef(robotTheta - 90, 0, 0, 1);
-    gl.glPointSize(15);
-    gl.glVertexPointer(3, GL10.GL_FLOAT, 0, robotVertexBuffer);
-    gl.glColor4f(1f, 0.0f, 0.0f, 1f);
-    gl.glDrawElements(GL10.GL_TRIANGLES, robotIndexCount, GL10.GL_UNSIGNED_SHORT, robotIndexBuffer);
-    gl.glDisableClientState(GL10.GL_VERTEX_ARRAY);
-    gl.glPopMatrix();
-  }
-
-  private void drawOutline(GL10 gl) {
-    gl.glPushMatrix();
-    gl.glEnable(GL10.GL_LINE_SMOOTH);
-    gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
-    gl.glTranslatef((float) robotPose.position.x, (float) robotPose.position.y, 0);
-    gl.glRotatef(robotTheta - 90, 0, 0, 1);
+    gl.glTranslatef((float) robotPose.position.x, (float) robotPose.position.y, 0.0f);
+    float robotThetaDegrees =
+        (float) Math.toDegrees(Geometry.calculateRotationAngle(robotPose.orientation));
+    Vector3 axis = Geometry.calculateRotationAxis(robotPose.orientation);
+    gl.glRotatef(robotThetaDegrees, (float) axis.x, (float) axis.y, (float) axis.z);
     gl.glScalef(scaleFactor, scaleFactor, scaleFactor);
-    gl.glLineWidth(2);
-    gl.glVertexPointer(3, GL10.GL_FLOAT, 0, robotOutlineVertexBuffer);
-    gl.glColor4f(1f, 1.0f, 1.0f, 1f);
-    gl.glDrawArrays(GL10.GL_LINE_LOOP, 0, robotOutlineIndexCount);
+    gl.glColor4f(0.0f, 0.635f, 1.0f, 0.5f);
+    gl.glVertexPointer(3, GL10.GL_FLOAT, 0, robotVertexBuffer);
+    gl.glDrawElements(GL10.GL_TRIANGLES, robotIndexBuffer.limit(), GL10.GL_UNSIGNED_SHORT,
+        robotIndexBuffer);
     gl.glDisableClientState(GL10.GL_VERTEX_ARRAY);
-    gl.glDisable(GL10.GL_LINE_SMOOTH);
     gl.glPopMatrix();
   }
 
@@ -167,16 +117,6 @@ public class Robot implements OpenGlDrawable {
     this.scaleFactor = scaleFactor;
   }
 
-  private float calculateHeading(Quaternion orientation) {
-    double heading;
-    double w = orientation.w;
-    double x = orientation.x;
-    double y = orientation.z;
-    double z = orientation.y;
-    heading = Math.atan2(2 * y * w - 2 * x * z, x * x - y * y - z * z + w * w) * 180 / Math.PI;
-    return (float) heading;
-  }
-
   /**
    * Update the robot's location and orientation.
    * 
@@ -185,10 +125,7 @@ public class Robot implements OpenGlDrawable {
    * @param res
    *          The resolution of the map
    */
-  public void updatePose(Pose pose, float res) {
-    this.robotPose = pose;
-    this.robotPose.position.x /= res;
-    this.robotPose.position.y /= res;
-    robotTheta = calculateHeading(pose.orientation);
+  public void updatePose(Pose pose) {
+    robotPose = pose;
   }
 }

android_honeycomb_mr2/src/org/ros/android/views/map/TextureNotInitialized.java

@@ -0,0 +1,5 @@
+package org.ros.android.views.map;
+
+public class TextureNotInitialized extends Exception {
+
+}

android_honeycomb_mr2/src/org/ros/android/views/map/UserGoal.java

@@ -16,7 +16,9 @@
 
 package org.ros.android.views.map;
 
-import android.graphics.Point;
+import org.ros.message.geometry_msgs.Pose;
+import org.ros.message.geometry_msgs.Vector3;
+import org.ros.rosjava_geometry.Geometry;
 
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
@@ -32,17 +34,19 @@ public class UserGoal extends Goal {
 
   @Override
   public void draw(GL10 gl) {
-    System.err.print("Drawing user goal.");
     gl.glPushMatrix();
     gl.glDisable(GL10.GL_CULL_FACE);
     gl.glFrontFace(GL10.GL_CW);
     gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
     gl.glVertexPointer(3, GL10.GL_FLOAT, 0, vertexBuffer);
     gl.glTranslatef((float) pose.position.x, (float) pose.position.y, 0);
-    gl.glRotatef(theta - 90, 0, 0, 1);
+    float thetaDegrees = (float) Math.toDegrees(Geometry.calculateRotationAngle(pose.orientation));
+    Vector3 axis = Geometry.calculateRotationAxis(pose.orientation);
+    gl.glRotatef(thetaDegrees, (float) axis.x, (float) axis.y, (float) axis.z);
     gl.glScalef(getScaleFactor(), getScaleFactor(), getScaleFactor());
+    gl.glScalef(1.5f, 1.5f, 1.5f);
     gl.glColor4f(0.847058824f, 0.243137255f, 0.8f, 1f);
-    gl.glDrawArrays(GL10.GL_TRIANGLE_FAN, 0, goalIndexCount);
+    gl.glDrawArrays(GL10.GL_TRIANGLE_FAN, 0, vertexBuffer.limit() / 3);
     gl.glDisableClientState(GL10.GL_VERTEX_ARRAY);
     gl.glPopMatrix();
   }
@@ -54,32 +58,32 @@ public class UserGoal extends Goal {
     goalVertices[1] = 0f;
     goalVertices[2] = 0f;
     goalVertices[3] = 0f;
-    goalVertices[4] = 21f;
+    goalVertices[4] = 0.21f;
     goalVertices[5] = 0f;
-    goalVertices[6] = 3f;
-    goalVertices[7] = 3f;
+    goalVertices[6] = 0.03f;
+    goalVertices[7] = 0.03f;
     goalVertices[8] = 0f;
-    goalVertices[9] = 10.5f;
+    goalVertices[9] = 0.105f;
     goalVertices[10] = 0f;
     goalVertices[11] = 0f;
-    goalVertices[12] = 3f;
-    goalVertices[13] = -3f;
+    goalVertices[12] = 0.03f;
+    goalVertices[13] = -0.03f;
     goalVertices[14] = 0f;
     goalVertices[15] = 0f;
-    goalVertices[16] = -10.5f;
+    goalVertices[16] = -0.105f;
     goalVertices[17] = 0f;
-    goalVertices[18] = -3f;
-    goalVertices[19] = -3f;
+    goalVertices[18] = -0.03f;
+    goalVertices[19] = -0.03f;
     goalVertices[20] = 0f;
-    goalVertices[21] = -10.5f;
+    goalVertices[21] = -0.105f;
     goalVertices[22] = 0f;
     goalVertices[23] = 0f;
-    goalVertices[24] = -3f;
-    goalVertices[25] = 3f;
+    goalVertices[24] = -0.03f;
+    goalVertices[25] = 0.03f;
     goalVertices[26] = 0f;
     // Repeat of point 1
     goalVertices[27] = 0f;
-    goalVertices[28] = 21f;
+    goalVertices[28] = 0.21f;
     goalVertices[29] = 0f;
     ByteBuffer goalVertexByteBuffer =
         ByteBuffer.allocateDirect(goalVertices.length * Float.SIZE / 8);
@@ -100,22 +104,10 @@ public class UserGoal extends Goal {
   /**
    * Update the location of the goal that the user is trying to specify.
    * 
-   * @param realWorldLocation
-   *          The real world coordinates (in meters) representing the location
-   *          of the user's desired goal.
+   * @param goal
+   *          the user goal
    */
-  public void updateUserGoalLocation(Point realWorldLocation) {
-    pose.position.x = -realWorldLocation.x;
-    pose.position.y = -realWorldLocation.y;
-  }
-
-  /**
-   * Update the orientation of the goal that the user is trying to specify.
-   * 
-   * @param theta
-   *          The orientation of the desired goal in degrees.
-   */
-  public void updateUserGoalOrientation(float theta) {
-    this.theta = theta;
+  public void updateUserGoal(Pose goal) {
+    pose = goal;
   }
 }

android_tutorial_teleop/res/menu/settings_menu.xml

@@ -4,7 +4,6 @@
     android:showAsAction="ifRoom|withText">
     <menu>
       <item android:id="@+id/map_view_initial_pose" android:title="Set initial pose" />
-      <item android:id="@+id/map_view_annotate_region" android:title="Annotate region" />
       <group android:checkableBehavior="all">
         <item android:id="@+id/map_view_robot_centric_view" android:title="Lock to robot" />
       </group>

android_tutorial_teleop/src/org/ros/android/tutorial/teleop/MainActivity.java

@@ -139,10 +139,6 @@ public class MainActivity extends Activity {
       mapView.initialPose();
       return true;
     }
-    case R.id.map_view_annotate_region: {
-      mapView.annotateRegion();
-      return true;
-    }
     case R.id.virtual_joystick_snap: {
       if (!item.isChecked()) {
         item.setChecked(true);