VirtualJoystickView.java

Go to the documentation of this file.

/*
 * Copyright (C) 2011 Google Inc.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 * 
 * http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */

package org.ros.android.view;

import android.content.Context;
import android.graphics.Point;
import android.util.AttributeSet;
import android.view.Gravity;
import android.view.LayoutInflater;
import android.view.MotionEvent;
import android.view.animation.Animation;
import android.view.animation.Animation.AnimationListener;
import android.view.animation.AnimationSet;
import android.view.animation.LinearInterpolator;
import android.view.animation.RotateAnimation;
import android.view.animation.ScaleAnimation;
import android.widget.ImageView;
import android.widget.RelativeLayout;
import android.widget.TextView;
import org.ros.android.android_honeycomb_mr2.R;
import org.ros.message.MessageListener;
import org.ros.namespace.GraphName;
import org.ros.node.ConnectedNode;
import org.ros.node.Node;
import org.ros.node.NodeMain;
import org.ros.node.topic.Publisher;
import org.ros.node.topic.Subscriber;

import java.util.Timer;
import java.util.TimerTask;

public class VirtualJoystickView extends RelativeLayout implements AnimationListener,
    MessageListener<nav_msgs.Odometry>, NodeMain {

  private static final float BOX_TO_CIRCLE_RATIO = 1.363636f;
  private float magnetTheta = 10.0f;
  private static final float ORIENTATION_TACK_FADE_RANGE = 40.0f;
  private static final long TURN_IN_PLACE_CONFIRMATION_DELAY = 200L;
  private static final float FLOAT_EPSILON = 0.001f;
  private static final float THUMB_DIVET_RADIUS = 16.5f;
  private static final float POST_LOCK_MAGNET_THETA = 20.0f;
  private static final int INVALID_POINTER_ID = -1;
  private Publisher<geometry_msgs.Twist> publisher;
  private RelativeLayout mainLayout;
  private ImageView intensity;
  private ImageView thumbDivet;
  private ImageView lastVelocityDivet;
  private ImageView[] orientationWidget;
  private TextView magnitudeText;
  private float contactTheta;
  private float normalizedMagnitude;
  private float contactRadius;
  private float deadZoneRatio = Float.NaN;
  private float joystickRadius = Float.NaN;
  private float parentSize = Float.NaN;
  private float normalizingMultiplier;
  private ImageView currentRotationRange;
  private ImageView previousRotationRange;
  private volatile boolean turnInPlaceMode;
  private float turnInPlaceStartTheta = Float.NaN;
  private float rightTurnOffset;
  private volatile float currentOrientation;
  private int pointerId = INVALID_POINTER_ID;
  private Point contactUpLocation;
  private boolean previousVelocityMode;
  private boolean magnetizedXAxis;
  private boolean holonomic;
  private volatile boolean publishVelocity;
  private Timer publisherTimer;
  private geometry_msgs.Twist currentVelocityCommand;
  private String topicName;

  public VirtualJoystickView(Context context) {
    super(context);
    initVirtualJoystick(context);
    topicName = "~cmd_vel";
  }

  public VirtualJoystickView(Context context, AttributeSet attrs) {
    super(context, attrs);
    initVirtualJoystick(context);
    topicName = "~cmd_vel";
  }

  public VirtualJoystickView(Context context, AttributeSet attrs, int defStyle) {
    super(context, attrs, defStyle);
    topicName = "~cmd_vel";
  }

  public void setHolonomic(boolean enabled) {
    holonomic = enabled;
  }

  @Override
  public void onAnimationEnd(Animation animation) {
    contactRadius = 0f;
    normalizedMagnitude = 0f;
    updateMagnitudeText();
  }

  @Override
  public void onAnimationRepeat(Animation animation) {
  }

  @Override
  public void onAnimationStart(Animation animation) {
  }

  @Override
  public void onNewMessage(final nav_msgs.Odometry message) {
    double heading;
    // For some reason the values of z and y seem to be interchanged. If they
    // are not swapped then heading is always incorrect.
    double w = message.getPose().getPose().getOrientation().getW();
    double x = message.getPose().getPose().getOrientation().getX();
    double y = message.getPose().getPose().getOrientation().getZ();
    double z = message.getPose().getPose().getOrientation().getY();
    heading = Math.atan2(2 * y * w - 2 * x * z, x * x - y * y - z * z + w * w) * 180 / Math.PI;
    // Negating the orientation to make the math for rotation in
    // turn-in-place mode easy. Since the actual heading is irrelevant it does
    // no harm.
    currentOrientation = (float) -heading;
    // Only update the orientation images if the turn-in-place mode is active.
    if (turnInPlaceMode) {
      post(new Runnable() {
        @Override
        public void run() {
          updateTurnInPlaceRotation();
        }
      });
      postInvalidate();
    }
  }

  @Override
  public boolean onTouchEvent(MotionEvent event) {
    final int action = event.getAction();
    switch (action & MotionEvent.ACTION_MASK) {
    case MotionEvent.ACTION_MOVE: {
      // If the primary contact point is no longer on the screen then ignore
      // the event.
      if (pointerId != INVALID_POINTER_ID) {
        // If the virtual joystick is in resume-previous-velocity mode.
        if (previousVelocityMode) {
          // And the current contact is close to the contact location prior to
          // ContactUp.
          if (inLastContactRange(event.getX(event.getActionIndex()),
              event.getY(event.getActionIndex()))) {
            // Then use the previous velocity.
            onContactMove(contactUpLocation.x + joystickRadius, contactUpLocation.y
                + joystickRadius);
          }
          // Since the current contact is not close to the prior location.
          else {
            // Exit the resume-previous-velocity mode.
            previousVelocityMode = false;
          }
        }
        // Since the resume-previous-velocity mode is not active generate
        // velocities based on current contact position.
        else {
          onContactMove(event.getX(event.findPointerIndex(pointerId)),
              event.getY(event.findPointerIndex(pointerId)));
        }
      }
      break;
    }
    case MotionEvent.ACTION_DOWN: {
      // Get the coordinates of the pointer that is initiating the
      // interaction.
      pointerId = event.getPointerId(event.getActionIndex());
      onContactDown();
      // If the current contact is close to the location of the contact prior
      // to contactUp.
      if (inLastContactRange(event.getX(event.getActionIndex()), event.getY(event.getActionIndex()))) {
        // Trigger resume-previous-velocity mode.
        previousVelocityMode = true;
        // The animation calculations/operations are performed in
        // onContactMove(). If this is not called and the user's finger stays
        // perfectly still after the down event, no operation is performed.
        // Calling onContactMove avoids this.
        onContactMove(contactUpLocation.x + joystickRadius, contactUpLocation.y + joystickRadius);
      } else {
        onContactMove(event.getX(event.getActionIndex()), event.getY(event.getActionIndex()));
      }
      break;
    }
    case MotionEvent.ACTION_POINTER_UP:
    case MotionEvent.ACTION_UP: {
      // Check if the contact that initiated the interaction is up.
      if ((action & MotionEvent.ACTION_POINTER_ID_MASK) >> MotionEvent.ACTION_POINTER_ID_SHIFT == pointerId) {
        onContactUp();
      }
      break;
    }
    }
    return true;
  }

  public void EnableSnapping() {
    magnetTheta = 10;
  }

  public void DisableSnapping() {
    magnetTheta = 1;
  }

  @Override
  protected void onLayout(boolean changed, int l, int t, int r, int b) {
    // Call the parent's onLayout to setup the views.
    super.onLayout(changed, l, t, r, b);
    // The parent container must be a square. A square container simplifies the
    // code. A non-square container does not provide any benefit over a
    // square.
    if (mainLayout.getWidth() != mainLayout.getHeight()) {
      // TODO(munjaldesai): Need to throw an exception/error. For now the
      // touch events will not be processed.
      this.setOnTouchListener(null);
    }
    parentSize = mainLayout.getWidth();
    if (parentSize < 200 || parentSize > 400) {
      // TODO: Need to throw an exception for attempting to create
      // a virtual joystick that is either too small or too big. For now the
      // touch events will be processed.
      this.setOnTouchListener(null);
    }
    // Calculate the center coordinates (radius) of parent container
    // (mainLayout).
    joystickRadius = mainLayout.getWidth() / 2;
    normalizingMultiplier = BOX_TO_CIRCLE_RATIO / (parentSize / 2);
    // Calculate the radius of the center divet as a normalize value.
    deadZoneRatio = THUMB_DIVET_RADIUS * normalizingMultiplier;
    // Determine the font size for the text view showing linear velocity. 8.3%
    // of the overall size seems to work well.
    magnitudeText.setTextSize(parentSize / 12);
  }

  private void animateIntensityCircle(float endScale) {
    AnimationSet intensityCircleAnimation = new AnimationSet(true);
    intensityCircleAnimation.setInterpolator(new LinearInterpolator());
    intensityCircleAnimation.setFillAfter(true);
    RotateAnimation rotateAnim;
    rotateAnim = new RotateAnimation(contactTheta, contactTheta, joystickRadius, joystickRadius);
    rotateAnim.setInterpolator(new LinearInterpolator());
    rotateAnim.setDuration(0);
    rotateAnim.setFillAfter(true);
    intensityCircleAnimation.addAnimation(rotateAnim);
    ScaleAnimation scaleAnim;
    scaleAnim =
        new ScaleAnimation(contactRadius, endScale, contactRadius, endScale, joystickRadius,
            joystickRadius);
    scaleAnim.setDuration(0);
    scaleAnim.setFillAfter(true);
    intensityCircleAnimation.addAnimation(scaleAnim);
    // Apply the animation.
    intensity.startAnimation(intensityCircleAnimation);
  }

  private void animateIntensityCircle(float endScale, long duration) {
    AnimationSet intensityCircleAnimation = new AnimationSet(true);
    intensityCircleAnimation.setInterpolator(new LinearInterpolator());
    intensityCircleAnimation.setFillAfter(true);
    // The listener is needed to set the magnitude text to 0 only after the
    // animation is over.
    intensityCircleAnimation.setAnimationListener(this);
    RotateAnimation rotateAnim;
    rotateAnim = new RotateAnimation(contactTheta, contactTheta, joystickRadius, joystickRadius);
    rotateAnim.setInterpolator(new LinearInterpolator());
    rotateAnim.setDuration(duration);
    rotateAnim.setFillAfter(true);
    intensityCircleAnimation.addAnimation(rotateAnim);
    ScaleAnimation scaleAnim;
    scaleAnim =
        new ScaleAnimation(contactRadius, endScale, contactRadius, endScale, joystickRadius,
            joystickRadius);
    scaleAnim.setDuration(duration);
    scaleAnim.setFillAfter(true);
    intensityCircleAnimation.addAnimation(scaleAnim);
    // Apply the animation.
    intensity.startAnimation(intensityCircleAnimation);
  }

  private void animateOrientationWidgets() {
    float deltaTheta;
    for (int i = 0; i < orientationWidget.length; i++) {
      deltaTheta = differenceBetweenAngles(i * 15, contactTheta);
      if (deltaTheta < ORIENTATION_TACK_FADE_RANGE) {
        orientationWidget[i].setAlpha(1.0f - deltaTheta / ORIENTATION_TACK_FADE_RANGE);
      } else {
        orientationWidget[i].setAlpha(0.0f);
      }
    }
  }

  private float differenceBetweenAngles(float angle0, float angle1) {
    return Math.abs((angle0 + 180 - angle1) % 360 - 180);
  }

  private void endTurnInPlaceRotation() {
    turnInPlaceMode = false;
    currentRotationRange.setAlpha(0.0f);
    previousRotationRange.setAlpha(0.0f);
    intensity.setAlpha(1.0f);
  }

  private void initVirtualJoystick(Context context) {
    // All the virtual joystick elements must be centered on the parent.
    setGravity(Gravity.CENTER);
    // Instantiate the elements from the layout XML file.
    LayoutInflater.from(context).inflate(R.layout.virtual_joystick, this, true);
    mainLayout = (RelativeLayout) findViewById(R.id.virtual_joystick_layout);
    magnitudeText = (TextView) findViewById(R.id.magnitude);
    intensity = (ImageView) findViewById(R.id.intensity);
    thumbDivet = (ImageView) findViewById(R.id.thumb_divet);
    orientationWidget = new ImageView[24];
    orientationWidget[0] = (ImageView) findViewById(R.id.widget_0_degrees);
    orientationWidget[1] = (ImageView) findViewById(R.id.widget_15_degrees);
    orientationWidget[2] = (ImageView) findViewById(R.id.widget_30_degrees);
    orientationWidget[3] = (ImageView) findViewById(R.id.widget_45_degrees);
    orientationWidget[4] = (ImageView) findViewById(R.id.widget_60_degrees);
    orientationWidget[5] = (ImageView) findViewById(R.id.widget_75_degrees);
    orientationWidget[6] = (ImageView) findViewById(R.id.widget_90_degrees);
    orientationWidget[7] = (ImageView) findViewById(R.id.widget_105_degrees);
    orientationWidget[8] = (ImageView) findViewById(R.id.widget_120_degrees);
    orientationWidget[9] = (ImageView) findViewById(R.id.widget_135_degrees);
    orientationWidget[10] = (ImageView) findViewById(R.id.widget_150_degrees);
    orientationWidget[11] = (ImageView) findViewById(R.id.widget_165_degrees);
    orientationWidget[12] = (ImageView) findViewById(R.id.widget_180_degrees);
    orientationWidget[13] = (ImageView) findViewById(R.id.widget_195_degrees);
    orientationWidget[14] = (ImageView) findViewById(R.id.widget_210_degrees);
    orientationWidget[15] = (ImageView) findViewById(R.id.widget_225_degrees);
    orientationWidget[16] = (ImageView) findViewById(R.id.widget_240_degrees);
    orientationWidget[17] = (ImageView) findViewById(R.id.widget_255_degrees);
    orientationWidget[18] = (ImageView) findViewById(R.id.widget_270_degrees);
    orientationWidget[19] = (ImageView) findViewById(R.id.widget_285_degrees);
    orientationWidget[20] = (ImageView) findViewById(R.id.widget_300_degrees);
    orientationWidget[21] = (ImageView) findViewById(R.id.widget_315_degrees);
    orientationWidget[22] = (ImageView) findViewById(R.id.widget_330_degrees);
    orientationWidget[23] = (ImageView) findViewById(R.id.widget_345_degrees);
    // Initially hide all the widgets.
    for (ImageView tack : orientationWidget) {
      tack.setAlpha(0.0f);
      tack.setVisibility(INVISIBLE);
    }
    // The value (radius) 40 is arbitrary, but small enough to work for the
    // smallest sized virtual joystick. Once the layout is set a value is
    // calculated based on the size of the virtual joystick.
    magnitudeText.setTranslationX((float) (40 * Math.cos((90 + contactTheta) * Math.PI / 180.0)));
    magnitudeText.setTranslationY((float) (40 * Math.sin((90 + contactTheta) * Math.PI / 180.0)));
    // Hide the intensity circle.
    animateIntensityCircle(0);
    // Initially the orientationWidgets should point to 0 degrees.
    contactTheta = 0;
    animateOrientationWidgets();
    currentRotationRange = (ImageView) findViewById(R.id.top_angle_slice);
    previousRotationRange = (ImageView) findViewById(R.id.mid_angle_slice);
    // Hide the slices/arcs used during the turn-in-place mode.
    currentRotationRange.setAlpha(0.0f);
    previousRotationRange.setAlpha(0.0f);
    lastVelocityDivet = (ImageView) findViewById(R.id.previous_velocity_divet);
    contactUpLocation = new Point(0, 0);
    holonomic = false;
    for (ImageView tack : orientationWidget) {
      tack.setVisibility(INVISIBLE);
    }
  }

  private void onContactDown() {
    // The divets should be completely opaque indicating
    // the virtual joystick is active.
    thumbDivet.setAlpha(1.0f);
    magnitudeText.setAlpha(1.0f);
    // Previous contact location need not be shown any more.
    lastVelocityDivet.setAlpha(0.0f);
    // Restore the orientation tacks.
    for (ImageView tack : orientationWidget) {
      tack.setVisibility(VISIBLE);
    }
    publishVelocity = true;
  }

  private void onContactMove(float x, float y) {
    // Get the coordinates of the contact relative to the center of the main
    // layout.
    float thumbDivetX = x - joystickRadius;
    float thumbDivetY = y - joystickRadius;
    // Convert the coordinates from Cartesian to Polar.
    contactTheta = (float) (Math.atan2(thumbDivetY, thumbDivetX) * 180 / Math.PI + 90);
    contactRadius =
        (float) Math.sqrt(thumbDivetX * thumbDivetX + thumbDivetY * thumbDivetY)
            * normalizingMultiplier;
    // Calculate the distance (0 to 1) from the center divet to the contact
    // point.
    normalizedMagnitude = (contactRadius - deadZoneRatio) / (1 - deadZoneRatio);
    // Perform bounds checking.
    if (contactRadius >= 1f) {
      // Since the contact is outside the outer ring, reset the coordinate for
      // the thumb divet to the on the outer ring.
      thumbDivetX /= contactRadius;
      thumbDivetY /= contactRadius;
      // The magnitude should not exceed 1.
      normalizedMagnitude = 1f;
      contactRadius = 1f;
    } else if (contactRadius < deadZoneRatio) {
      // Since the contact is inside the dead zone snap the thumb divet to the
      // dead zone. It should stay there till the contact gets outside the
      // deadzone area.
      thumbDivetX = 0;
      thumbDivetY = 0;
      // Prevent normalizedMagnitude going negative inside the deadzone.
      normalizedMagnitude = 0f;
    }

    // Magnetize!
    // If the contact is not snapped to the x axis.
    if (!magnetizedXAxis) {
      // Check if the contact should be snapped to either axis.
      if ((contactTheta + 360) % 90 < magnetTheta) {
        // If the current angle is within MAGNET_THETA degrees + 0, 90, 180, or
        // 270 then subtract the additional degrees so that the current theta is
        // 0, 90, 180, or 270.
        contactTheta -= ((contactTheta + 360) % 90);
      } else if ((contactTheta + 360) % 90 > (90 - magnetTheta)) {
        // If the current angle is within MAGNET_THETA degrees - 0, 90, 180, or
        // 270 then add the additional degrees so that the current theta is 0,
        // 90, 180, or 270.
        contactTheta += (90 - ((contactTheta + 360) % 90));
      }
      // Indicate that the contact has been snapped to the x-axis.
      if (floatCompare(contactTheta, 90) || floatCompare(contactTheta, 270)) {
        magnetizedXAxis = true;
      }
    } else {
      // Use a wider range to keep the contact snapped in.
      if (differenceBetweenAngles((contactTheta + 360) % 360, 90) < POST_LOCK_MAGNET_THETA) {
        contactTheta = 90;
      } else if (differenceBetweenAngles((contactTheta + 360) % 360, 270) < POST_LOCK_MAGNET_THETA) {
        contactTheta = 270;
      }
      // Indicate that the contact is not snapped to the x-axis.
      else {
        magnetizedXAxis = false;
      }
    }

    // Update the size and location (scale and rotation) of various elements.
    animateIntensityCircle(contactRadius);
    animateOrientationWidgets();
    updateThumbDivet(thumbDivetX, thumbDivetY);
    updateMagnitudeText();
    // Publish the velocities.
    if (holonomic) {
      publishVelocity(normalizedMagnitude * Math.cos(contactTheta * Math.PI / 180.0),
          normalizedMagnitude * Math.sin(contactTheta * Math.PI / 180.0), 0);
    } else {
      publishVelocity(normalizedMagnitude * Math.cos(contactTheta * Math.PI / 180.0), 0,
          normalizedMagnitude * Math.sin(contactTheta * Math.PI / 180.0));
    }

    // Check if the turn-in-place mode needs to be activated/deactivated.
    updateTurnInPlaceMode();
  }

  private void updateTurnInPlaceMode() {
    if (!turnInPlaceMode) {
      if (floatCompare(contactTheta, 270)) {
        // If the user is turning left and the turn-in-place mode is not active
        // then activate it for a left turn.
        turnInPlaceMode = true;
        rightTurnOffset = 0;
      } else if (floatCompare(contactTheta, 90)) {
        // If the user is turning right and the turn-in-place mode is not active
        // then activate it for a right turn.
        turnInPlaceMode = true;
        rightTurnOffset = 15;
      } else {
        // Nothing to do while not in turn-in-place mode and not at 270/90.
        return;
      }
      // Initiate the turn-in-place mode but wait some time before changing the
      // images. This is to avoid the users getting seizures because of the
      // quick changes every time they cross 270 or 90.
      initiateTurnInPlace();
      // Start a timer and if the user is still turning in place when the timer
      // is up, then visually indicate entering turn-in-place mode.
      new Timer().schedule(new TimerTask() {
        @Override
        public void run() {
          post(new Runnable() {
            @Override
            public void run() {
              if (turnInPlaceMode) {
                currentRotationRange.setAlpha(1.0f);
                previousRotationRange.setAlpha(1.0f);
                intensity.setAlpha(0.2f);
              }
            }
          });
          postInvalidate();
        }
      }, TURN_IN_PLACE_CONFIRMATION_DELAY);
    } else if (!(floatCompare(contactTheta, 270) || floatCompare(contactTheta, 90))) {
      // If the user was in turn-in-place mode and is now no longer on the x
      // axis, then exit turn-in-place mode.
      endTurnInPlaceRotation();
    }
  }

  private void onContactUp() {
    // TODO(munjaldesai): The 1000 should eventually be replaced with a number
    // that reflects the physical characteristics of the motor controller along
    // with the latency associated with the connection.
    animateIntensityCircle(0, (long) (normalizedMagnitude * 1000));
    magnitudeText.setAlpha(0.4f);
    // Place the lastVelocityDivet at the location of the last known contact.
    lastVelocityDivet.setTranslationX(thumbDivet.getTranslationX());
    lastVelocityDivet.setTranslationY(thumbDivet.getTranslationY());
    lastVelocityDivet.setAlpha(0.4f);
    contactUpLocation.x = (int) (thumbDivet.getTranslationX());
    contactUpLocation.y = (int) (thumbDivet.getTranslationY());
    // Move the thumb divet back to the center.
    updateThumbDivet(0, 0);
    // Reset the pointer id.
    pointerId = INVALID_POINTER_ID;
    // The robot should stop moving.
    publishVelocity(0, 0, 0);
    // Stop publishing the velocity since the contact is no longer on the
    // screen.
    publishVelocity = false;
    // Publish one last message to make sure the robot stops.
    publisher.publish(currentVelocityCommand);
    // Turn-in-place should not be active anymore.
    endTurnInPlaceRotation();
    // Hide the orientation tacks.
    for (ImageView tack : orientationWidget) {
      tack.setVisibility(INVISIBLE);
    }
  }

  private void publishVelocity(double linearVelocityX, double linearVelocityY,
      double angularVelocityZ) {
    currentVelocityCommand.getLinear().setX(linearVelocityX);
    currentVelocityCommand.getLinear().setY(-linearVelocityY);
    currentVelocityCommand.getLinear().setZ(0);
    currentVelocityCommand.getAngular().setX(0);
    currentVelocityCommand.getAngular().setY(0);
    currentVelocityCommand.getAngular().setZ(-angularVelocityZ);
  }

  private void initiateTurnInPlace() {
    // Record the orientation when the turn-in-place was initiated.
    turnInPlaceStartTheta = (currentOrientation + 360) % 360;
    RotateAnimation rotateAnim;
    rotateAnim =
        new RotateAnimation(rightTurnOffset, rightTurnOffset, joystickRadius, joystickRadius);
    rotateAnim.setInterpolator(new LinearInterpolator());
    rotateAnim.setDuration(0);
    rotateAnim.setFillAfter(true);
    currentRotationRange.startAnimation(rotateAnim);
    rotateAnim = new RotateAnimation(15, 15, joystickRadius, joystickRadius);
    rotateAnim.setInterpolator(new LinearInterpolator());
    rotateAnim.setDuration(0);
    rotateAnim.setFillAfter(true);
    previousRotationRange.startAnimation(rotateAnim);
  }

  private void updateMagnitudeText() {
    // Don't update when the user is turning in place.
    if (!turnInPlaceMode) {
      magnitudeText.setText(String.valueOf((int) (normalizedMagnitude * 100)) + "%");
      magnitudeText.setTranslationX((float) (parentSize / 4 * Math.cos((90 + contactTheta)
          * Math.PI / 180.0)));
      magnitudeText.setTranslationY((float) (parentSize / 4 * Math.sin((90 + contactTheta)
          * Math.PI / 180.0)));
    }
  }

  private void updateTurnInPlaceRotation() {
    final float currentTheta = (currentOrientation + 360) % 360;
    float offsetTheta;
    // Calculate the difference between the orientations.
    offsetTheta = (turnInPlaceStartTheta - currentTheta + 360) % 360;
    offsetTheta = 360 - offsetTheta;
    // Show the current rotation amount.
    magnitudeText.setText(String.valueOf((int) offsetTheta));
    // Calculate theta in increments of 15 degrees. (0-14 => 0, 15-29=>15, etc).
    offsetTheta = (int) (offsetTheta - (offsetTheta % 15));
    // Rotate the 2 arcs based on the offset in orientation.
    RotateAnimation rotateAnim;
    rotateAnim =
        new RotateAnimation(offsetTheta + rightTurnOffset, offsetTheta + rightTurnOffset,
            joystickRadius, joystickRadius);
    rotateAnim.setInterpolator(new LinearInterpolator());
    rotateAnim.setDuration(0);
    rotateAnim.setFillAfter(true);
    currentRotationRange.startAnimation(rotateAnim);
    rotateAnim =
        new RotateAnimation(offsetTheta + 15, offsetTheta + 15, joystickRadius, joystickRadius);
    rotateAnim.setInterpolator(new LinearInterpolator());
    rotateAnim.setDuration(0);
    rotateAnim.setFillAfter(true);
    previousRotationRange.startAnimation(rotateAnim);
  }

  private void updateThumbDivet(float x, float y) {
    // Offset the specified coordinates to ensure that the center of the thumb
    // divet is under the thumb.
    thumbDivet.setTranslationX(-THUMB_DIVET_RADIUS);
    thumbDivet.setTranslationY(-THUMB_DIVET_RADIUS);
    // Set the orientation. This must be done before translation.
    thumbDivet.setRotation(contactTheta);
    thumbDivet.setTranslationX(x);
    thumbDivet.setTranslationY(y);
  }

  private boolean floatCompare(float v1, float v2) {
    if (Math.abs(v1 - v2) < FLOAT_EPSILON) {
      return true;
    } else {
      return false;
    }
  }

  private boolean inLastContactRange(float x, float y) {
    if (Math.sqrt((x - contactUpLocation.x - joystickRadius)
        * (x - contactUpLocation.x - joystickRadius) + (y - contactUpLocation.y - joystickRadius)
        * (y - contactUpLocation.y - joystickRadius)) < THUMB_DIVET_RADIUS) {
      return true;
    }
    return false;
  }
  
  public void setTopicName(String topicName) {
    this.topicName = topicName;
  }

  @Override
  public GraphName getDefaultNodeName() {
    return GraphName.of("android_honeycomb_mr2/virtual_joystick_view");
  }

  @Override
  public void onStart(ConnectedNode connectedNode) {
    publisher = connectedNode.newPublisher(topicName, geometry_msgs.Twist._TYPE);
    currentVelocityCommand = publisher.newMessage();
    Subscriber<nav_msgs.Odometry> subscriber =
        connectedNode.newSubscriber("odom", nav_msgs.Odometry._TYPE);
    subscriber.addMessageListener(this);
    publisherTimer = new Timer();
    publisherTimer.schedule(new TimerTask() {
      @Override
      public void run() {
        if (publishVelocity) {
          publisher.publish(currentVelocityCommand);
        }
      }
    }, 0, 80);
  }

  @Override
  public void onShutdown(Node node) {
  }

  @Override
  public void onShutdownComplete(Node node) {
    publisherTimer.cancel();
    publisherTimer.purge();
  }

  @Override
  public void onError(Node node, Throwable throwable) {
  }
}