VirtualJoystickView.java
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2011 Google Inc.
00003  * 
00004  * Licensed under the Apache License, Version 2.0 (the "License"); you may not
00005  * use this file except in compliance with the License. You may obtain a copy of
00006  * the License at
00007  * 
00008  * http://www.apache.org/licenses/LICENSE-2.0
00009  * 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
00012  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
00013  * License for the specific language governing permissions and limitations under
00014  * the License.
00015  */
00016 
00017 package org.ros.android.view;
00018 
00019 import android.content.Context;
00020 import android.graphics.Point;
00021 import android.util.AttributeSet;
00022 import android.view.Gravity;
00023 import android.view.LayoutInflater;
00024 import android.view.MotionEvent;
00025 import android.view.animation.Animation;
00026 import android.view.animation.Animation.AnimationListener;
00027 import android.view.animation.AnimationSet;
00028 import android.view.animation.LinearInterpolator;
00029 import android.view.animation.RotateAnimation;
00030 import android.view.animation.ScaleAnimation;
00031 import android.widget.ImageView;
00032 import android.widget.RelativeLayout;
00033 import android.widget.TextView;
00034 import org.ros.android.android_15.R;
00035 import org.ros.message.MessageListener;
00036 import org.ros.namespace.GraphName;
00037 import org.ros.node.ConnectedNode;
00038 import org.ros.node.Node;
00039 import org.ros.node.NodeMain;
00040 import org.ros.node.topic.Publisher;
00041 import org.ros.node.topic.Subscriber;
00042 
00043 import java.util.Timer;
00044 import java.util.TimerTask;
00045 
00053 public class VirtualJoystickView extends RelativeLayout implements AnimationListener,
00054     MessageListener<nav_msgs.Odometry>, NodeMain {
00055 
00062   private static final float BOX_TO_CIRCLE_RATIO = 1.363636f;
00069   private float magnetTheta = 10.0f;
00074   private static final float ORIENTATION_TACK_FADE_RANGE = 40.0f;
00079   private static final long TURN_IN_PLACE_CONFIRMATION_DELAY = 200L;
00083   private static final float FLOAT_EPSILON = 0.001f;
00090   private static final float THUMB_DIVET_RADIUS = 16.5f;
00095   private static final float POST_LOCK_MAGNET_THETA = 20.0f;
00096   private static final int INVALID_POINTER_ID = -1;
00097   private Publisher<geometry_msgs.Twist> publisher;
00102   private RelativeLayout mainLayout;
00106   private ImageView intensity;
00112   private ImageView thumbDivet;
00118   private ImageView lastVelocityDivet;
00124   private ImageView[] orientationWidget;
00132   private TextView magnitudeText;
00134   private float contactTheta;
00140   private float normalizedMagnitude;
00148   private float contactRadius;
00157   private float deadZoneRatio = Float.NaN;
00164   private float joystickRadius = Float.NaN;
00169   private float parentSize = Float.NaN;
00176   private float normalizingMultiplier;
00181   private ImageView currentRotationRange;
00186   private ImageView previousRotationRange;
00191   private volatile boolean turnInPlaceMode;
00196   private float turnInPlaceStartTheta = Float.NaN;
00202   private float rightTurnOffset;
00206   private volatile float currentOrientation;
00211   private int pointerId = INVALID_POINTER_ID;
00216   private Point contactUpLocation;
00220   private boolean previousVelocityMode;
00225   private boolean magnetizedXAxis;
00231   private boolean holonomic;
00236   private volatile boolean publishVelocity;
00240   private Timer publisherTimer;
00241   private geometry_msgs.Twist currentVelocityCommand;
00242   private String topicName;
00243 
00244   public VirtualJoystickView(Context context) {
00245     super(context);
00246     initVirtualJoystick(context);
00247     topicName = "~cmd_vel";
00248   }
00249 
00250   public VirtualJoystickView(Context context, AttributeSet attrs) {
00251     super(context, attrs);
00252     initVirtualJoystick(context);
00253     topicName = "~cmd_vel";
00254   }
00255 
00256   public VirtualJoystickView(Context context, AttributeSet attrs, int defStyle) {
00257     super(context, attrs, defStyle);
00258     topicName = "~cmd_vel";
00259   }
00260 
00267   public void setHolonomic(boolean enabled) {
00268     holonomic = enabled;
00269   }
00270 
00271   @Override
00272   public void onAnimationEnd(Animation animation) {
00273     contactRadius = 0f;
00274     normalizedMagnitude = 0f;
00275     updateMagnitudeText();
00276   }
00277 
00278   @Override
00279   public void onAnimationRepeat(Animation animation) {
00280   }
00281 
00282   @Override
00283   public void onAnimationStart(Animation animation) {
00284   }
00285 
00286   @Override
00287   public void onNewMessage(final nav_msgs.Odometry message) {
00288     double heading;
00289     // For some reason the values of z and y seem to be interchanged. If they
00290     // are not swapped then heading is always incorrect.
00291     double w = message.getPose().getPose().getOrientation().getW();
00292     double x = message.getPose().getPose().getOrientation().getX();
00293     double y = message.getPose().getPose().getOrientation().getZ();
00294     double z = message.getPose().getPose().getOrientation().getY();
00295     heading = Math.atan2(2 * y * w - 2 * x * z, x * x - y * y - z * z + w * w) * 180 / Math.PI;
00296     // Negating the orientation to make the math for rotation in
00297     // turn-in-place mode easy. Since the actual heading is irrelevant it does
00298     // no harm.
00299     currentOrientation = (float) -heading;
00300     // Only update the orientation images if the turn-in-place mode is active.
00301     if (turnInPlaceMode) {
00302       post(new Runnable() {
00303         @Override
00304         public void run() {
00305           updateTurnInPlaceRotation();
00306         }
00307       });
00308       postInvalidate();
00309     }
00310   }
00311 
00312   @Override
00313   public boolean onTouchEvent(MotionEvent event) {
00314     final int action = event.getAction();
00315     switch (action & MotionEvent.ACTION_MASK) {
00316     case MotionEvent.ACTION_MOVE: {
00317       // If the primary contact point is no longer on the screen then ignore
00318       // the event.
00319       if (pointerId != INVALID_POINTER_ID) {
00320         // If the virtual joystick is in resume-previous-velocity mode.
00321         if (previousVelocityMode) {
00322           // And the current contact is close to the contact location prior to
00323           // ContactUp.
00324           if (inLastContactRange(event.getX(event.getActionIndex()),
00325               event.getY(event.getActionIndex()))) {
00326             // Then use the previous velocity.
00327             onContactMove(contactUpLocation.x + joystickRadius, contactUpLocation.y
00328                 + joystickRadius);
00329           }
00330           // Since the current contact is not close to the prior location.
00331           else {
00332             // Exit the resume-previous-velocity mode.
00333             previousVelocityMode = false;
00334           }
00335         }
00336         // Since the resume-previous-velocity mode is not active generate
00337         // velocities based on current contact position.
00338         else {
00339           onContactMove(event.getX(event.findPointerIndex(pointerId)),
00340               event.getY(event.findPointerIndex(pointerId)));
00341         }
00342       }
00343       break;
00344     }
00345     case MotionEvent.ACTION_DOWN: {
00346       // Get the coordinates of the pointer that is initiating the
00347       // interaction.
00348       pointerId = event.getPointerId(event.getActionIndex());
00349       onContactDown();
00350       // If the current contact is close to the location of the contact prior
00351       // to contactUp.
00352       if (inLastContactRange(event.getX(event.getActionIndex()), event.getY(event.getActionIndex()))) {
00353         // Trigger resume-previous-velocity mode.
00354         previousVelocityMode = true;
00355         // The animation calculations/operations are performed in
00356         // onContactMove(). If this is not called and the user's finger stays
00357         // perfectly still after the down event, no operation is performed.
00358         // Calling onContactMove avoids this.
00359         onContactMove(contactUpLocation.x + joystickRadius, contactUpLocation.y + joystickRadius);
00360       } else {
00361         onContactMove(event.getX(event.getActionIndex()), event.getY(event.getActionIndex()));
00362       }
00363       break;
00364     }
00365     case MotionEvent.ACTION_POINTER_UP:
00366     case MotionEvent.ACTION_UP: {
00367       // Check if the contact that initiated the interaction is up.
00368       if ((action & MotionEvent.ACTION_POINTER_ID_MASK) >> MotionEvent.ACTION_POINTER_ID_SHIFT == pointerId) {
00369         onContactUp();
00370       }
00371       break;
00372     }
00373     }
00374     return true;
00375   }
00376 
00380   public void EnableSnapping() {
00381     magnetTheta = 10;
00382   }
00383 
00387   public void DisableSnapping() {
00388     magnetTheta = 1;
00389   }
00390 
00395   @Override
00396   protected void onLayout(boolean changed, int l, int t, int r, int b) {
00397     // Call the parent's onLayout to setup the views.
00398     super.onLayout(changed, l, t, r, b);
00399     // The parent container must be a square. A square container simplifies the
00400     // code. A non-square container does not provide any benefit over a
00401     // square.
00402     if (mainLayout.getWidth() != mainLayout.getHeight()) {
00403       // TODO(munjaldesai): Need to throw an exception/error. For now the
00404       // touch events will not be processed.
00405       this.setOnTouchListener(null);
00406     }
00407     parentSize = mainLayout.getWidth();
00408     if (parentSize < 200 || parentSize > 400) {
00409       // TODO: Need to throw an exception for attempting to create
00410       // a virtual joystick that is either too small or too big. For now the
00411       // touch events will be processed.
00412       this.setOnTouchListener(null);
00413     }
00414     // Calculate the center coordinates (radius) of parent container
00415     // (mainLayout).
00416     joystickRadius = mainLayout.getWidth() / 2;
00417     normalizingMultiplier = BOX_TO_CIRCLE_RATIO / (parentSize / 2);
00418     // Calculate the radius of the center divet as a normalize value.
00419     deadZoneRatio = THUMB_DIVET_RADIUS * normalizingMultiplier;
00420     // Determine the font size for the text view showing linear velocity. 8.3%
00421     // of the overall size seems to work well.
00422     magnitudeText.setTextSize(parentSize / 12);
00423   }
00424 
00435   private void animateIntensityCircle(float endScale) {
00436     AnimationSet intensityCircleAnimation = new AnimationSet(true);
00437     intensityCircleAnimation.setInterpolator(new LinearInterpolator());
00438     intensityCircleAnimation.setFillAfter(true);
00439     RotateAnimation rotateAnim;
00440     rotateAnim = new RotateAnimation(contactTheta, contactTheta, joystickRadius, joystickRadius);
00441     rotateAnim.setInterpolator(new LinearInterpolator());
00442     rotateAnim.setDuration(0);
00443     rotateAnim.setFillAfter(true);
00444     intensityCircleAnimation.addAnimation(rotateAnim);
00445     ScaleAnimation scaleAnim;
00446     scaleAnim =
00447         new ScaleAnimation(contactRadius, endScale, contactRadius, endScale, joystickRadius,
00448             joystickRadius);
00449     scaleAnim.setDuration(0);
00450     scaleAnim.setFillAfter(true);
00451     intensityCircleAnimation.addAnimation(scaleAnim);
00452     // Apply the animation.
00453     intensity.startAnimation(intensityCircleAnimation);
00454   }
00455 
00467   private void animateIntensityCircle(float endScale, long duration) {
00468     AnimationSet intensityCircleAnimation = new AnimationSet(true);
00469     intensityCircleAnimation.setInterpolator(new LinearInterpolator());
00470     intensityCircleAnimation.setFillAfter(true);
00471     // The listener is needed to set the magnitude text to 0 only after the
00472     // animation is over.
00473     intensityCircleAnimation.setAnimationListener(this);
00474     RotateAnimation rotateAnim;
00475     rotateAnim = new RotateAnimation(contactTheta, contactTheta, joystickRadius, joystickRadius);
00476     rotateAnim.setInterpolator(new LinearInterpolator());
00477     rotateAnim.setDuration(duration);
00478     rotateAnim.setFillAfter(true);
00479     intensityCircleAnimation.addAnimation(rotateAnim);
00480     ScaleAnimation scaleAnim;
00481     scaleAnim =
00482         new ScaleAnimation(contactRadius, endScale, contactRadius, endScale, joystickRadius,
00483             joystickRadius);
00484     scaleAnim.setDuration(duration);
00485     scaleAnim.setFillAfter(true);
00486     intensityCircleAnimation.addAnimation(scaleAnim);
00487     // Apply the animation.
00488     intensity.startAnimation(intensityCircleAnimation);
00489   }
00490 
00498   private void animateOrientationWidgets() {
00499     float deltaTheta;
00500     for (int i = 0; i < orientationWidget.length; i++) {
00501       deltaTheta = differenceBetweenAngles(i * 15, contactTheta);
00502       if (deltaTheta < ORIENTATION_TACK_FADE_RANGE) {
00503         orientationWidget[i].setAlpha(1.0f - deltaTheta / ORIENTATION_TACK_FADE_RANGE);
00504       } else {
00505         orientationWidget[i].setAlpha(0.0f);
00506       }
00507     }
00508   }
00509 
00523   private float differenceBetweenAngles(float angle0, float angle1) {
00524     return Math.abs((angle0 + 180 - angle1) % 360 - 180);
00525   }
00526 
00531   private void endTurnInPlaceRotation() {
00532     turnInPlaceMode = false;
00533     currentRotationRange.setAlpha(0.0f);
00534     previousRotationRange.setAlpha(0.0f);
00535     intensity.setAlpha(1.0f);
00536   }
00537 
00541   private void initVirtualJoystick(Context context) {
00542     // All the virtual joystick elements must be centered on the parent.
00543     setGravity(Gravity.CENTER);
00544     // Instantiate the elements from the layout XML file.
00545     LayoutInflater.from(context).inflate(R.layout.virtual_joystick, this, true);
00546     mainLayout = (RelativeLayout) findViewById(R.id.virtual_joystick_layout);
00547     magnitudeText = (TextView) findViewById(R.id.magnitude);
00548     intensity = (ImageView) findViewById(R.id.intensity);
00549     thumbDivet = (ImageView) findViewById(R.id.thumb_divet);
00550     orientationWidget = new ImageView[24];
00551     orientationWidget[0] = (ImageView) findViewById(R.id.widget_0_degrees);
00552     orientationWidget[1] = (ImageView) findViewById(R.id.widget_15_degrees);
00553     orientationWidget[2] = (ImageView) findViewById(R.id.widget_30_degrees);
00554     orientationWidget[3] = (ImageView) findViewById(R.id.widget_45_degrees);
00555     orientationWidget[4] = (ImageView) findViewById(R.id.widget_60_degrees);
00556     orientationWidget[5] = (ImageView) findViewById(R.id.widget_75_degrees);
00557     orientationWidget[6] = (ImageView) findViewById(R.id.widget_90_degrees);
00558     orientationWidget[7] = (ImageView) findViewById(R.id.widget_105_degrees);
00559     orientationWidget[8] = (ImageView) findViewById(R.id.widget_120_degrees);
00560     orientationWidget[9] = (ImageView) findViewById(R.id.widget_135_degrees);
00561     orientationWidget[10] = (ImageView) findViewById(R.id.widget_150_degrees);
00562     orientationWidget[11] = (ImageView) findViewById(R.id.widget_165_degrees);
00563     orientationWidget[12] = (ImageView) findViewById(R.id.widget_180_degrees);
00564     orientationWidget[13] = (ImageView) findViewById(R.id.widget_195_degrees);
00565     orientationWidget[14] = (ImageView) findViewById(R.id.widget_210_degrees);
00566     orientationWidget[15] = (ImageView) findViewById(R.id.widget_225_degrees);
00567     orientationWidget[16] = (ImageView) findViewById(R.id.widget_240_degrees);
00568     orientationWidget[17] = (ImageView) findViewById(R.id.widget_255_degrees);
00569     orientationWidget[18] = (ImageView) findViewById(R.id.widget_270_degrees);
00570     orientationWidget[19] = (ImageView) findViewById(R.id.widget_285_degrees);
00571     orientationWidget[20] = (ImageView) findViewById(R.id.widget_300_degrees);
00572     orientationWidget[21] = (ImageView) findViewById(R.id.widget_315_degrees);
00573     orientationWidget[22] = (ImageView) findViewById(R.id.widget_330_degrees);
00574     orientationWidget[23] = (ImageView) findViewById(R.id.widget_345_degrees);
00575     // Initially hide all the widgets.
00576     for (ImageView tack : orientationWidget) {
00577       tack.setAlpha(0.0f);
00578       tack.setVisibility(INVISIBLE);
00579     }
00580     // The value (radius) 40 is arbitrary, but small enough to work for the
00581     // smallest sized virtual joystick. Once the layout is set a value is
00582     // calculated based on the size of the virtual joystick.
00583     magnitudeText.setTranslationX((float) (40 * Math.cos((90 + contactTheta) * Math.PI / 180.0)));
00584     magnitudeText.setTranslationY((float) (40 * Math.sin((90 + contactTheta) * Math.PI / 180.0)));
00585     // Hide the intensity circle.
00586     animateIntensityCircle(0);
00587     // Initially the orientationWidgets should point to 0 degrees.
00588     contactTheta = 0;
00589     animateOrientationWidgets();
00590     currentRotationRange = (ImageView) findViewById(R.id.top_angle_slice);
00591     previousRotationRange = (ImageView) findViewById(R.id.mid_angle_slice);
00592     // Hide the slices/arcs used during the turn-in-place mode.
00593     currentRotationRange.setAlpha(0.0f);
00594     previousRotationRange.setAlpha(0.0f);
00595     lastVelocityDivet = (ImageView) findViewById(R.id.previous_velocity_divet);
00596     contactUpLocation = new Point(0, 0);
00597     holonomic = false;
00598     for (ImageView tack : orientationWidget) {
00599       tack.setVisibility(INVISIBLE);
00600     }
00601   }
00602 
00606   private void onContactDown() {
00607     // The divets should be completely opaque indicating
00608     // the virtual joystick is active.
00609     thumbDivet.setAlpha(1.0f);
00610     magnitudeText.setAlpha(1.0f);
00611     // Previous contact location need not be shown any more.
00612     lastVelocityDivet.setAlpha(0.0f);
00613     // Restore the orientation tacks.
00614     for (ImageView tack : orientationWidget) {
00615       tack.setVisibility(VISIBLE);
00616     }
00617     publishVelocity = true;
00618   }
00619 
00629   private void onContactMove(float x, float y) {
00630     // Get the coordinates of the contact relative to the center of the main
00631     // layout.
00632     float thumbDivetX = x - joystickRadius;
00633     float thumbDivetY = y - joystickRadius;
00634     // Convert the coordinates from Cartesian to Polar.
00635     contactTheta = (float) (Math.atan2(thumbDivetY, thumbDivetX) * 180 / Math.PI + 90);
00636     contactRadius =
00637         (float) Math.sqrt(thumbDivetX * thumbDivetX + thumbDivetY * thumbDivetY)
00638             * normalizingMultiplier;
00639     // Calculate the distance (0 to 1) from the center divet to the contact
00640     // point.
00641     normalizedMagnitude = (contactRadius - deadZoneRatio) / (1 - deadZoneRatio);
00642     // Perform bounds checking.
00643     if (contactRadius >= 1f) {
00644       // Since the contact is outside the outer ring, reset the coordinate for
00645       // the thumb divet to the on the outer ring.
00646       thumbDivetX /= contactRadius;
00647       thumbDivetY /= contactRadius;
00648       // The magnitude should not exceed 1.
00649       normalizedMagnitude = 1f;
00650       contactRadius = 1f;
00651     } else if (contactRadius < deadZoneRatio) {
00652       // Since the contact is inside the dead zone snap the thumb divet to the
00653       // dead zone. It should stay there till the contact gets outside the
00654       // deadzone area.
00655       thumbDivetX = 0;
00656       thumbDivetY = 0;
00657       // Prevent normalizedMagnitude going negative inside the deadzone.
00658       normalizedMagnitude = 0f;
00659     }
00660 
00661     // Magnetize!
00662     // If the contact is not snapped to the x axis.
00663     if (!magnetizedXAxis) {
00664       // Check if the contact should be snapped to either axis.
00665       if ((contactTheta + 360) % 90 < magnetTheta) {
00666         // If the current angle is within MAGNET_THETA degrees + 0, 90, 180, or
00667         // 270 then subtract the additional degrees so that the current theta is
00668         // 0, 90, 180, or 270.
00669         contactTheta -= ((contactTheta + 360) % 90);
00670       } else if ((contactTheta + 360) % 90 > (90 - magnetTheta)) {
00671         // If the current angle is within MAGNET_THETA degrees - 0, 90, 180, or
00672         // 270 then add the additional degrees so that the current theta is 0,
00673         // 90, 180, or 270.
00674         contactTheta += (90 - ((contactTheta + 360) % 90));
00675       }
00676       // Indicate that the contact has been snapped to the x-axis.
00677       if (floatCompare(contactTheta, 90) || floatCompare(contactTheta, 270)) {
00678         magnetizedXAxis = true;
00679       }
00680     } else {
00681       // Use a wider range to keep the contact snapped in.
00682       if (differenceBetweenAngles((contactTheta + 360) % 360, 90) < POST_LOCK_MAGNET_THETA) {
00683         contactTheta = 90;
00684       } else if (differenceBetweenAngles((contactTheta + 360) % 360, 270) < POST_LOCK_MAGNET_THETA) {
00685         contactTheta = 270;
00686       }
00687       // Indicate that the contact is not snapped to the x-axis.
00688       else {
00689         magnetizedXAxis = false;
00690       }
00691     }
00692 
00693     // Update the size and location (scale and rotation) of various elements.
00694     animateIntensityCircle(contactRadius);
00695     animateOrientationWidgets();
00696     updateThumbDivet(thumbDivetX, thumbDivetY);
00697     updateMagnitudeText();
00698     // Publish the velocities.
00699     if (holonomic) {
00700       publishVelocity(normalizedMagnitude * Math.cos(contactTheta * Math.PI / 180.0),
00701           normalizedMagnitude * Math.sin(contactTheta * Math.PI / 180.0), 0);
00702     } else {
00703       publishVelocity(normalizedMagnitude * Math.cos(contactTheta * Math.PI / 180.0), 0,
00704           normalizedMagnitude * Math.sin(contactTheta * Math.PI / 180.0));
00705     }
00706 
00707     // Check if the turn-in-place mode needs to be activated/deactivated.
00708     updateTurnInPlaceMode();
00709   }
00710 
00714   private void updateTurnInPlaceMode() {
00715     if (!turnInPlaceMode) {
00716       if (floatCompare(contactTheta, 270)) {
00717         // If the user is turning left and the turn-in-place mode is not active
00718         // then activate it for a left turn.
00719         turnInPlaceMode = true;
00720         rightTurnOffset = 0;
00721       } else if (floatCompare(contactTheta, 90)) {
00722         // If the user is turning right and the turn-in-place mode is not active
00723         // then activate it for a right turn.
00724         turnInPlaceMode = true;
00725         rightTurnOffset = 15;
00726       } else {
00727         // Nothing to do while not in turn-in-place mode and not at 270/90.
00728         return;
00729       }
00730       // Initiate the turn-in-place mode but wait some time before changing the
00731       // images. This is to avoid the users getting seizures because of the
00732       // quick changes every time they cross 270 or 90.
00733       initiateTurnInPlace();
00734       // Start a timer and if the user is still turning in place when the timer
00735       // is up, then visually indicate entering turn-in-place mode.
00736       new Timer().schedule(new TimerTask() {
00737         @Override
00738         public void run() {
00739           post(new Runnable() {
00740             @Override
00741             public void run() {
00742               if (turnInPlaceMode) {
00743                 currentRotationRange.setAlpha(1.0f);
00744                 previousRotationRange.setAlpha(1.0f);
00745                 intensity.setAlpha(0.2f);
00746               }
00747             }
00748           });
00749           postInvalidate();
00750         }
00751       }, TURN_IN_PLACE_CONFIRMATION_DELAY);
00752     } else if (!(floatCompare(contactTheta, 270) || floatCompare(contactTheta, 90))) {
00753       // If the user was in turn-in-place mode and is now no longer on the x
00754       // axis, then exit turn-in-place mode.
00755       endTurnInPlaceRotation();
00756     }
00757   }
00758 
00763   private void onContactUp() {
00764     // TODO(munjaldesai): The 1000 should eventually be replaced with a number
00765     // that reflects the physical characteristics of the motor controller along
00766     // with the latency associated with the connection.
00767     animateIntensityCircle(0, (long) (normalizedMagnitude * 1000));
00768     magnitudeText.setAlpha(0.4f);
00769     // Place the lastVelocityDivet at the location of the last known contact.
00770     lastVelocityDivet.setTranslationX(thumbDivet.getTranslationX());
00771     lastVelocityDivet.setTranslationY(thumbDivet.getTranslationY());
00772     lastVelocityDivet.setAlpha(0.4f);
00773     contactUpLocation.x = (int) (thumbDivet.getTranslationX());
00774     contactUpLocation.y = (int) (thumbDivet.getTranslationY());
00775     // Move the thumb divet back to the center.
00776     updateThumbDivet(0, 0);
00777     // Reset the pointer id.
00778     pointerId = INVALID_POINTER_ID;
00779     // The robot should stop moving.
00780     publishVelocity(0, 0, 0);
00781     // Stop publishing the velocity since the contact is no longer on the
00782     // screen.
00783     publishVelocity = false;
00784     // Publish one last message to make sure the robot stops.
00785     publisher.publish(currentVelocityCommand);
00786     // Turn-in-place should not be active anymore.
00787     endTurnInPlaceRotation();
00788     // Hide the orientation tacks.
00789     for (ImageView tack : orientationWidget) {
00790       tack.setVisibility(INVISIBLE);
00791     }
00792   }
00793 
00802   private void publishVelocity(double linearVelocityX, double linearVelocityY,
00803       double angularVelocityZ) {
00804     currentVelocityCommand.getLinear().setX(linearVelocityX);
00805     currentVelocityCommand.getLinear().setY(-linearVelocityY);
00806     currentVelocityCommand.getLinear().setZ(0);
00807     currentVelocityCommand.getAngular().setX(0);
00808     currentVelocityCommand.getAngular().setY(0);
00809     currentVelocityCommand.getAngular().setZ(-angularVelocityZ);
00810   }
00811 
00815   private void initiateTurnInPlace() {
00816     // Record the orientation when the turn-in-place was initiated.
00817     turnInPlaceStartTheta = (currentOrientation + 360) % 360;
00818     RotateAnimation rotateAnim;
00819     rotateAnim =
00820         new RotateAnimation(rightTurnOffset, rightTurnOffset, joystickRadius, joystickRadius);
00821     rotateAnim.setInterpolator(new LinearInterpolator());
00822     rotateAnim.setDuration(0);
00823     rotateAnim.setFillAfter(true);
00824     currentRotationRange.startAnimation(rotateAnim);
00825     rotateAnim = new RotateAnimation(15, 15, joystickRadius, joystickRadius);
00826     rotateAnim.setInterpolator(new LinearInterpolator());
00827     rotateAnim.setDuration(0);
00828     rotateAnim.setFillAfter(true);
00829     previousRotationRange.startAnimation(rotateAnim);
00830   }
00831 
00835   private void updateMagnitudeText() {
00836     // Don't update when the user is turning in place.
00837     if (!turnInPlaceMode) {
00838       magnitudeText.setText(String.valueOf((int) (normalizedMagnitude * 100)) + "%");
00839       magnitudeText.setTranslationX((float) (parentSize / 4 * Math.cos((90 + contactTheta)
00840           * Math.PI / 180.0)));
00841       magnitudeText.setTranslationY((float) (parentSize / 4 * Math.sin((90 + contactTheta)
00842           * Math.PI / 180.0)));
00843     }
00844   }
00845 
00850   private void updateTurnInPlaceRotation() {
00851     final float currentTheta = (currentOrientation + 360) % 360;
00852     float offsetTheta;
00853     // Calculate the difference between the orientations.
00854     offsetTheta = (turnInPlaceStartTheta - currentTheta + 360) % 360;
00855     offsetTheta = 360 - offsetTheta;
00856     // Show the current rotation amount.
00857     magnitudeText.setText(String.valueOf((int) offsetTheta));
00858     // Calculate theta in increments of 15 degrees. (0-14 => 0, 15-29=>15, etc).
00859     offsetTheta = (int) (offsetTheta - (offsetTheta % 15));
00860     // Rotate the 2 arcs based on the offset in orientation.
00861     RotateAnimation rotateAnim;
00862     rotateAnim =
00863         new RotateAnimation(offsetTheta + rightTurnOffset, offsetTheta + rightTurnOffset,
00864             joystickRadius, joystickRadius);
00865     rotateAnim.setInterpolator(new LinearInterpolator());
00866     rotateAnim.setDuration(0);
00867     rotateAnim.setFillAfter(true);
00868     currentRotationRange.startAnimation(rotateAnim);
00869     rotateAnim =
00870         new RotateAnimation(offsetTheta + 15, offsetTheta + 15, joystickRadius, joystickRadius);
00871     rotateAnim.setInterpolator(new LinearInterpolator());
00872     rotateAnim.setDuration(0);
00873     rotateAnim.setFillAfter(true);
00874     previousRotationRange.startAnimation(rotateAnim);
00875   }
00876 
00887   private void updateThumbDivet(float x, float y) {
00888     // Offset the specified coordinates to ensure that the center of the thumb
00889     // divet is under the thumb.
00890     thumbDivet.setTranslationX(-THUMB_DIVET_RADIUS);
00891     thumbDivet.setTranslationY(-THUMB_DIVET_RADIUS);
00892     // Set the orientation. This must be done before translation.
00893     thumbDivet.setRotation(contactTheta);
00894     thumbDivet.setTranslationX(x);
00895     thumbDivet.setTranslationY(y);
00896   }
00897 
00906   private boolean floatCompare(float v1, float v2) {
00907     if (Math.abs(v1 - v2) < FLOAT_EPSILON) {
00908       return true;
00909     } else {
00910       return false;
00911     }
00912   }
00913 
00914   private boolean inLastContactRange(float x, float y) {
00915     if (Math.sqrt((x - contactUpLocation.x - joystickRadius)
00916         * (x - contactUpLocation.x - joystickRadius) + (y - contactUpLocation.y - joystickRadius)
00917         * (y - contactUpLocation.y - joystickRadius)) < THUMB_DIVET_RADIUS) {
00918       return true;
00919     }
00920     return false;
00921   }
00922   
00923   public void setTopicName(String topicName) {
00924     this.topicName = topicName;
00925   }
00926 
00927   @Override
00928   public GraphName getDefaultNodeName() {
00929     return GraphName.of("android_15/virtual_joystick_view");
00930   }
00931 
00932   @Override
00933   public void onStart(ConnectedNode connectedNode) {
00934     publisher = connectedNode.newPublisher(topicName, geometry_msgs.Twist._TYPE);
00935     currentVelocityCommand = publisher.newMessage();
00936     Subscriber<nav_msgs.Odometry> subscriber =
00937         connectedNode.newSubscriber("odom", nav_msgs.Odometry._TYPE);
00938     subscriber.addMessageListener(this);
00939     publisherTimer = new Timer();
00940     publisherTimer.schedule(new TimerTask() {
00941       @Override
00942       public void run() {
00943         if (publishVelocity) {
00944           publisher.publish(currentVelocityCommand);
00945         }
00946       }
00947     }, 0, 80);
00948   }
00949 
00950   @Override
00951   public void onShutdown(Node node) {
00952   }
00953 
00954   @Override
00955   public void onShutdownComplete(Node node) {
00956     publisherTimer.cancel();
00957     publisherTimer.purge();
00958   }
00959 
00960   @Override
00961   public void onError(Node node, Throwable throwable) {
00962   }
00963 }


android_core
Author(s): Damon Kohler
autogenerated on Thu Jun 6 2019 21:20:07