00001 package org.ros.android.jskAndroidGui;
00002
00003 import java.io.IOException;
00004 import java.util.ArrayList;
00005
00006 import jsk_gui_msgs.Action;
00007 import jsk_gui_msgs.Tablet;
00008 import jsk_gui_msgs.Touch;
00009
00010 import org.ros.message.MessageFactory;
00011 import org.ros.message.MessageListener;
00012 import org.ros.message.Time;
00013 import org.ros.namespace.GraphName;
00014 import org.ros.node.ConnectedNode;
00015 import org.ros.node.Node;
00016 import org.ros.node.NodeConfiguration;
00017 import org.ros.node.NodeMain;
00018 import org.ros.node.topic.Publisher;
00019 import org.ros.node.topic.Subscriber;
00020
00021 import sensor_msgs.CameraInfo;
00022 import sensor_msgs.CompressedImage;
00023 import android.app.AlertDialog;
00024 import android.content.Context;
00025 import android.content.DialogInterface;
00026 import android.graphics.Bitmap;
00027 import android.graphics.BitmapRegionDecoder;
00028 import android.graphics.Color;
00029 import android.graphics.Point;
00030 import android.graphics.Rect;
00031 import android.util.AttributeSet;
00032 import android.util.Log;
00033 import android.view.LayoutInflater;
00034 import android.view.MotionEvent;
00035 import android.view.View;
00036 import android.widget.EditText;
00037 import android.widget.ImageView;
00038 import android.widget.Toast;
00039
00040 public class SensorImageViewInfo extends ImageView implements Runnable,
00041 NodeMain {
00042
00043
00044
00045
00046 public native long translateBGRtoRGB(int[] src, int width, int height);
00047
00048 private final int minLength = 30, SWIPE_WAIT_TIME = 1, SWIPE_NONE = 0,
00049 SWIPE_UP = 1, SWIPE_DOWN = 2, SWIPE_RIGHT = 3, SWIPE_LEFT = 4;
00050 private float imageHeight = 480F, imageWidth = 640F;
00051 private boolean decodeFlag = true;
00052 private Bitmap bitmap;
00053 private BitmapRegionDecoder bitmapRegionDecoder;
00054 private boolean isMovingFingerInfo = false, isPushOnce = false,
00055 isPickOnce = false, isPlaceOnce = false, isPassToHumanOnce = false,
00056 isGetTemplate = false, isZoomCamera = true,
00057 isCircularManipulation = false, isLinearManipulation = false;
00058 private boolean isDrawLine = false;
00059 private int count = 0, debug_count = 0, fingerCount = 0, SwipeCounter = 0,
00060 SwipeDetectedType, RobotArmId = Action.LARMID, TouchMode = 1;
00061 private float MaxHeight, MaxWidth;
00062 private float ROIoffsetX = 0, ROIoffsetY = 0, ROIRate = 1,
00063 startROIRate = 1, startROIcenterX, startROIcenterY;
00064 private ArrayList<Integer> startXList = new ArrayList(),
00065 startYList = new ArrayList(), lastXList = new ArrayList(),
00066 lastYList = new ArrayList(), fingerList = new ArrayList();
00067
00068
00069
00070 private Integer start_x = 0, start_y = 0;
00071 private ArrayList<Point> pl = new ArrayList<Point>();
00072 private LayoutInflater inflater;
00073
00074 private Publisher<Tablet> TabletCommandPub;
00075
00076 private Publisher<Tablet> TabletTaskPub;
00077 private Publisher<Tablet> TabletPubDebug;
00078 private Publisher<Action> TabletMoveIt;
00079 private Publisher<Tablet> TabletTemplateInfo;
00080 private Subscriber<CompressedImage> imageSub;
00081 private Subscriber<CameraInfo> caminfoSub;
00082 private String cameratopic, camerainfotopic;
00083 private CameraInfo caminfo;
00084 private ConnectedNode connectedNode;
00085 private NodeConfiguration nodeConfiguration;
00086 private MessageFactory messageFactory;
00087 private Thread thread = null;
00088 private boolean nodeStart = false;
00089 private BoundingBoxView boundingBoxView;
00090 private Context context;
00091
00092 public SensorImageViewInfo(Context ctx) {
00093 super(ctx);
00094 context = ctx;
00095 }
00096
00097 public SensorImageViewInfo(Context ctx, AttributeSet attrs, int defStyle) {
00098 super(ctx, attrs, defStyle);
00099 context = ctx;
00100 }
00101
00102 public SensorImageViewInfo(Context ctx, AttributeSet attrs) {
00103 super(ctx, attrs);
00104 context = ctx;
00105 }
00106
00107 public void setCameraTopic(String cameratopic) {
00108 this.cameratopic = cameratopic;
00109 }
00110
00111 public void setCameraInfoTopic(String camerainfotopic) {
00112 this.camerainfotopic = camerainfotopic;
00113 }
00114
00115 public void setView(BoundingBoxView bb) {
00116 boundingBoxView = bb;
00117 }
00118
00119 @Override
00120 public void onStart(ConnectedNode connectedNode) {
00121 this.connectedNode = connectedNode;
00122 this.nodeConfiguration = NodeConfiguration.newPrivate();
00123 this.messageFactory = nodeConfiguration.getTopicMessageFactory();
00124 ResetValue();
00125 imageSub = connectedNode.newSubscriber(cameratopic,
00126 sensor_msgs.CompressedImage._TYPE);
00127 caminfoSub = connectedNode.newSubscriber(camerainfotopic,
00128 sensor_msgs.CameraInfo._TYPE);
00129 TabletCommandPub = connectedNode.newPublisher("/Tablet/Command",
00130 jsk_gui_msgs.Tablet._TYPE);
00131 TabletTaskPub = connectedNode.newPublisher("/Tablet/Task",
00132 jsk_gui_msgs.Tablet._TYPE);
00133 TabletPubDebug = connectedNode.newPublisher("/Tablet/CommandDebug",
00134 jsk_gui_msgs.Tablet._TYPE);
00135 TabletMoveIt = connectedNode.newPublisher("/Tablet/MoveIt",
00136 jsk_gui_msgs.Action._TYPE);
00137 TabletTemplateInfo = connectedNode.newPublisher("/Tablet/TemplateInfo",
00138 jsk_gui_msgs.Tablet._TYPE);
00139 MaxWidth = this.getWidth();
00140 MaxHeight = this.getHeight();
00141 imageSub.addMessageListener(new MessageListener<CompressedImage>() {
00142 @Override
00143 public void onNewMessage(CompressedImage message) {
00144 try {
00145 bitmapRegionDecoder = BitmapRegionDecoder.newInstance(
00146 message.getData().array(), message.getData()
00147 .arrayOffset(), message.getData()
00148 .readableBytes(), false);
00149 } catch (IOException e) {
00150 Log.e("JskAndroidGui", "failed to bitmap region decoder");
00151 e.printStackTrace();
00152 return;
00153 }
00154 setRectBitmap();
00155 post(SensorImageViewInfo.this);
00156 }
00157 });
00158 caminfoSub.addMessageListener(new MessageListener<CameraInfo>() {
00159 @Override
00160 public void onNewMessage(CameraInfo message) {
00161 caminfo = message;
00162 imageHeight = caminfo.getHeight();
00163 imageWidth = caminfo.getWidth();
00164 post(SensorImageViewInfo.this);
00165 }
00166 });
00167 nodeStart = true;
00168 }
00169
00170 public void stop() {
00171 if (imageSub != null)
00172 imageSub.shutdown();
00173 imageSub = null;
00174 if (caminfoSub != null)
00175 caminfoSub.shutdown();
00176 caminfoSub = null;
00177 }
00178
00179 public void SetSwipeDetected(int SwipeType) {
00180 SwipeDetectedType = SwipeType;
00181 SwipeCounter = 0;
00182 }
00183
00184 public void unSetSwipeDetected() {
00185 SwipeDetectedType = SWIPE_NONE;
00186 SwipeCounter = 0;
00187 }
00188
00189 public void SetRobotArm(int armid) {
00190 RobotArmId = armid;
00191 }
00192
00193 public void PublishPushOnce() {
00194 SendCommandMsg("PushOnce", RobotArmId, "TOUCH", 0, null, 0, fingerList,
00195 startXList, startYList, start_x, start_y);
00196 }
00197
00198 public void PublishPickOnce() {
00199 SendCommandMsg("PickObjectSelected", RobotArmId, "TOUCH", 0, null, 0,
00200 fingerList, startXList, startYList, start_x, start_y);
00201 }
00202
00203 public void PublishPlaceOnce() {
00204 SendCommandMsg("PlaceObjectSelected", RobotArmId, "TOUCH", 0, null, 0,
00205 fingerList, startXList, startYList, start_x, start_y);
00206 }
00207
00208 public void PublishGetTemplateOnce() {
00209 SendCommandMsg("GetTemplate", RobotArmId, "TOUCH", 0, null, 0,
00210 fingerList, startXList, startYList, start_x, start_y);
00211 }
00212
00213 public void SetDrawLine() {
00214 isDrawLine = true;
00215 }
00216
00217 public void unSetDrawLine() {
00218 isDrawLine = false;
00219 }
00220
00221 public void SetCircularManipulation() {
00222 isCircularManipulation = true;
00223 }
00224
00225 public void unSetCircularManipulation() {
00226 isCircularManipulation = false;
00227 }
00228
00229 public void SetLinearManipulation() {
00230 isLinearManipulation = true;
00231 }
00232
00233 public void unSetLinearManipulation() {
00234 isLinearManipulation = false;
00235 }
00236
00237 public void CancelManipulation() {
00238 unSetCircularManipulation();
00239 unSetLinearManipulation();
00240 SendTaskMsg("CancelManipulation", RobotArmId, "", 0, null, 0,
00241 fingerList, lastXList, lastYList, 0, 0);
00242 }
00243
00244 public void SetMovingFingerInfo() {
00245 isMovingFingerInfo = true;
00246 }
00247
00248 public void unSetMovingFingerInfo() {
00249 isMovingFingerInfo = false;
00250 }
00251
00252 public void SetGetTemplate() {
00253 isGetTemplate = true;
00254 }
00255
00256 public void unSetGetTemplate() {
00257 isGetTemplate = false;
00258 }
00259
00260 public void SetZoomCamera() {
00261 isZoomCamera = true;
00262 }
00263
00264 public void unSetZoomCamera() {
00265 isZoomCamera = false;
00266 }
00267
00268 public void SetPushOnce() {
00269 isPushOnce = true;
00270 }
00271
00272 public void SetPickOnce() {
00273 isPickOnce = true;
00274 }
00275
00276 public void SetPlaceOnce() {
00277 isPlaceOnce = true;
00278 }
00279
00280 public void SetPassToHumanOnce() {
00281 isPassToHumanOnce = true;
00282 }
00283
00284 public void ChangeTouchMode() {
00285 if (TouchMode == 0) {
00286 TouchMode++;
00287 } else if (TouchMode == 1) {
00288 TouchMode = 0;
00289 }
00290 }
00291
00292 public void PubSwitchSensor(String str) {
00293 SendCommandMsg("SwitchSensor", 0, str, 0, null, 0, fingerList,
00294 startXList, startYList, 0, 0);
00295 ResetROI();
00296 }
00297
00298 public void ResetROI() {
00299 ROIRate = 1;
00300 ROIoffsetX = 0;
00301 ROIoffsetY = 0;
00302 }
00303
00304 public void SetResetAll() {
00305 ResetValue();
00306 isDrawLine = false;
00307 isMovingFingerInfo = false;
00308 SendCommandMsg("ResetAll", 0, "SengMsg", 0, null, 0, fingerList,
00309 startXList, startYList, 0, 0);
00310 }
00311
00312 public void SetResetCollider() {
00313 SendCommandMsg("ResetCollider", 0, "SengMsg", 0, null, 0, fingerList,
00314 startXList, startYList, 0, 0);
00315 }
00316
00317 public void SendCommandMsg(String task_name, int arm_id, String state,
00318 float state_value, String direction, float direction_value,
00319 ArrayList<Integer> fingerList, ArrayList<Integer> xList,
00320 ArrayList<Integer> yList, int touch_x, int touch_y) {
00321 Log.v("JskAndroidGui:SendCommandMsg", "START");
00322 Tablet TabletMsg = TabletCommandPub.newMessage();
00323 TabletMsg.getHeader().setSeq(count++);
00324
00325 TabletMsg.getHeader().setStamp(
00326 Time.fromMillis(System.currentTimeMillis()));
00327 TabletMsg.setHardwareName("Android");
00328 TabletMsg.setHardwareId("JSK Acer");
00329 if (task_name != null)
00330 TabletMsg.getAction().setTaskName(task_name);
00331 if (arm_id != 0)
00332 TabletMsg.getAction().setArmId(arm_id);
00333 if (state != null)
00334 TabletMsg.getAction().setState(state);
00335 if (state_value != 0)
00336 TabletMsg.getAction().setStateValue(state_value);
00337 if (direction != null)
00338 TabletMsg.getAction().setDirection(direction);
00339 if (direction_value != 0)
00340 TabletMsg.getAction().setDirectionValue(direction_value);
00341
00342 for (int i = 0; i < xList.size(); ++i) {
00343 Touch TouchMsg = messageFactory.newFromType(Touch._TYPE);
00344 TouchMsg.setFingerId(fingerList.get(i));
00345 TouchMsg.setX(getImageCoordX(xList.get(i)));
00346 TouchMsg.setY(getImageCoordY(yList.get(i)));
00347 TabletMsg.getTouches().add(TouchMsg);
00348 }
00349 if (touch_x != 0)
00350 TabletMsg.getAction().setTouchX((int) getImageCoordX(touch_x));
00351 if (touch_y != 0)
00352 TabletMsg.getAction().setTouchY((int) getImageCoordY(touch_y));
00353 TabletCommandPub.publish(TabletMsg);
00354 Log.v("JskAndroidGui:SendCommandMsg", "END");
00355 }
00356
00357 public void SendTaskMsg(String task_name, int arm_id, String state,
00358 float state_value, String direction, float direction_value,
00359 ArrayList<Integer> fingerList, ArrayList<Integer> xList,
00360 ArrayList<Integer> yList, int touch_x, int touch_y) {
00361 Log.v("JskAndroidGui:SendTaskMsg", "START");
00362 Tablet TabletMsg = TabletTaskPub.newMessage();
00363 TabletMsg.getHeader().setSeq(count++);
00364 TabletMsg.getHeader().setStamp(
00365 Time.fromMillis(System.currentTimeMillis()));
00366 TabletMsg.setHardwareName("Android");
00367 TabletMsg.setHardwareId("JSK Acer");
00368 if (task_name != null)
00369 TabletMsg.getAction().setTaskName(task_name);
00370 if (arm_id != 0)
00371 TabletMsg.getAction().setArmId(arm_id);
00372 if (state != null)
00373 TabletMsg.getAction().setState(state);
00374 if (state_value != 0)
00375 TabletMsg.getAction().setStateValue(state_value);
00376 if (direction != null)
00377 TabletMsg.getAction().setDirection(direction);
00378 if (direction_value != 0)
00379 TabletMsg.getAction().setDirectionValue(direction_value);
00380
00381 for (int i = 0; i < xList.size(); i++) {
00382 Touch TouchMsg = messageFactory.newFromType(Touch._TYPE);
00383 TouchMsg.setFingerId(fingerList.get(i));
00384 TouchMsg.setX(getImageCoordX(xList.get(i)));
00385 TouchMsg.setY(getImageCoordY(yList.get(i)));
00386 TabletMsg.getTouches().add(TouchMsg);
00387 }
00388 if (touch_x != 0)
00389 TabletMsg.getAction().setTouchX((int) getImageCoordX(touch_x));
00390 if (touch_y != 0)
00391 TabletMsg.getAction().setTouchY((int) getImageCoordY(touch_y));
00392 TabletTaskPub.publish(TabletMsg);
00393 Log.v("JskAndroidGui:SendTaskMsg", "END");
00394 }
00395
00396 public void SendDebugMsg(String state, float state_value, int x, int y) {
00397 Log.v("JskAndroidGui:SendDebugMsg", "START");
00398 Tablet DebugMsg = TabletPubDebug.newMessage();
00399 DebugMsg.getHeader().setSeq(debug_count++);
00400 DebugMsg.getHeader().setStamp(
00401 Time.fromMillis(System.currentTimeMillis()));
00402 DebugMsg.getAction().setState(state);
00403 DebugMsg.getAction().setStateValue(state_value);
00404 Touch TouchMsg = messageFactory.newFromType(Touch._TYPE);
00405 TouchMsg.setFingerId(0);
00406 TouchMsg.setX(getImageCoordX(x));
00407 TouchMsg.setY(getImageCoordY(y));
00408 DebugMsg.getTouches().add(TouchMsg);
00409 TabletPubDebug.publish(DebugMsg);
00410 Log.v("JskAndroidGui:SendDebugMsg", "END");
00411 }
00412
00413 public void SendMoveItMsg(String task_name, String direction, int resolution) {
00414 Action moveItMsg = TabletMoveIt.newMessage();
00415 moveItMsg.setArmId(RobotArmId);
00416 moveItMsg.setTaskName(task_name);
00417 moveItMsg.setDirection(direction);
00418 moveItMsg.setDirectionValue((double) resolution);
00419 TabletMoveIt.publish(moveItMsg);
00420 }
00421
00422 public void SendTemplateInfo(String name, int startX, int startY,
00423 int lastX, int lastY) {
00424 Tablet templateMsg = TabletTemplateInfo.newMessage();
00425 templateMsg.getAction().setTaskName(name);
00426 templateMsg.getAction().setArmId(RobotArmId);
00427 Touch startTouchMsg = messageFactory.newFromType(Touch._TYPE);
00428 Touch lastTouchMsg = messageFactory.newFromType(Touch._TYPE);
00429 startTouchMsg.setX(getImageCoordX(startX));
00430 startTouchMsg.setY(getImageCoordY(startY));
00431 lastTouchMsg.setX(getImageCoordX(lastX));
00432 lastTouchMsg.setY(getImageCoordY(lastY));
00433 templateMsg.getTouches().add(startTouchMsg);
00434 templateMsg.getTouches().add(lastTouchMsg);
00435 TabletTemplateInfo.publish(templateMsg);
00436 }
00437
00438 public void MoveNeck(String direction, float direction_value) {
00439
00440 SendCommandMsg("MoveNeck", RobotArmId, "SWIPE", 0, direction,
00441 direction_value, fingerList, startXList, startYList, 0, 0);
00442 }
00443
00444 private void ZoomCamera(int center_x, int center_y, float startDistance,
00445 float endDistance) {
00446 float ROIcenterX = startROIcenterX;
00447 float ROIcenterY = startROIcenterY;
00448 float tmpROIRate = startROIRate * startDistance / endDistance;
00449 if (tmpROIRate > 1) {
00450 ROIoffsetX = 0;
00451 ROIoffsetY = 0;
00452 ROIRate = 1;
00453 } else {
00454 if (tmpROIRate * imageWidth * (1 - center_x / MaxWidth)
00455 + ROIcenterX > imageWidth) {
00456 ROIcenterX = imageWidth
00457 * (1 - tmpROIRate * (1 - center_x / MaxWidth));
00458 } else if (ROIcenterX - tmpROIRate * imageWidth * center_x
00459 / MaxWidth < 0) {
00460 ROIcenterX = tmpROIRate * imageWidth * center_x / MaxWidth;
00461 }
00462 if (tmpROIRate * imageHeight * (1 - center_y / MaxHeight)
00463 + ROIcenterY > imageHeight) {
00464 ROIcenterY = imageHeight
00465 * (1 - tmpROIRate * (1 - center_y / MaxHeight));
00466 } else if (ROIcenterY - tmpROIRate * imageHeight * center_y
00467 / MaxHeight < 0) {
00468 ROIcenterY = tmpROIRate * imageHeight * center_y / MaxHeight;
00469 }
00470 ROIoffsetX = ROIcenterX - tmpROIRate * imageWidth * center_x
00471 / MaxWidth;
00472 ROIoffsetY = ROIcenterY - tmpROIRate * imageHeight * center_y
00473 / MaxHeight;
00474 ROIRate = tmpROIRate;
00475 }
00476 setRectBitmap();
00477 }
00478
00479 private void setRectBitmap() {
00480 if (decodeFlag) {
00481 decodeFlag = false;
00482 Rect rect = new Rect((int) ROIoffsetX, (int) ROIoffsetY,
00483 (int) (ROIoffsetX + imageWidth * ROIRate),
00484 (int) (ROIoffsetY + imageHeight * ROIRate));
00485 bitmap = bitmapRegionDecoder.decodeRegion(rect, null);
00486 decodeFlag = true;
00487 }
00488 }
00489
00490 public float getImageCoordX(float fingerX) {
00491 return ROIoffsetX + fingerX * imageWidth * ROIRate / MaxWidth;
00492 }
00493
00494 public float getImageCoordY(float fingerY) {
00495 return ROIoffsetY + fingerY * imageHeight * ROIRate / MaxHeight;
00496 }
00497
00498 public void SendOpenDoorMsg() {
00499 SendCommandMsg("OpenDoor", RobotArmId, "SendMsg", 0, null, 0,
00500 fingerList, startXList, startYList, 0, 0);
00501 unSetMovingFingerInfo();
00502 }
00503
00504 public void SendCloseDoorMsg() {
00505 SendCommandMsg("CloseDoor", RobotArmId, "SendMsg", 0, null, 0,
00506 fingerList, startXList, startYList, 0, 0);
00507 unSetMovingFingerInfo();
00508 }
00509
00510 public void SendTuckArmPoseMsg() {
00511 SendCommandMsg("TuckArmPose", RobotArmId, "SendMsg", 0, null, 0,
00512 fingerList, startXList, startYList, 0, 0);
00513 }
00514
00515 public void SendTorsoUpMsg() {
00516 SendCommandMsg("TorsoUp", RobotArmId, "SendMsg", 0, null, 0,
00517 fingerList, startXList, startYList, 0, 0);
00518 }
00519
00520 public void SendTorsoDownMsg() {
00521 SendCommandMsg("TorsoDown", RobotArmId, "SendMsg", 0, null, 0,
00522 fingerList, startXList, startYList, 0, 0);
00523 }
00524
00525 public void SendOpenGripperMsg() {
00526 SendCommandMsg("OpenGripper", RobotArmId, "SendMsg", 0, null, 0,
00527 fingerList, startXList, startYList, 0, 0);
00528 }
00529
00530 public void SendCloseGripperMsg() {
00531 SendCommandMsg("CloseGripper", RobotArmId, "SendMsg", 0, null, 0,
00532 fingerList, startXList, startYList, 0, 0);
00533 }
00534
00535 public void ResetValue() {
00536 fingerList.clear();
00537 startXList.clear();
00538 startYList.clear();
00539 lastXList.clear();
00540 lastYList.clear();
00541 }
00542
00543 public boolean isClockWise(int p1x, int p1y, int p2x, int p2y, int p3x,
00544 int p3y) {
00545 if (((p2x - p1x) * (p3y - p1y)) >= ((p3x - p1x) * (p2y - p1y)))
00546 return false;
00547 return true;
00548 }
00549
00550 @Override
00551 public boolean onTouchEvent(MotionEvent event) {
00552
00553 final int action = event.getAction();
00554 final int curFingerCount = event.getPointerCount();
00555
00556 switch (action & MotionEvent.ACTION_MASK) {
00557
00558 case MotionEvent.ACTION_DOWN:
00559 Log.i("JskAndroidGui:TouchEvent", "[ACTION_DOWN] START");
00560 fingerCount = event.getPointerCount();
00561 fingerList.clear();
00562 startXList.clear();
00563 startYList.clear();
00564 lastXList.clear();
00565 lastYList.clear();
00566
00567 fingerList.ensureCapacity(10);
00568 startXList.ensureCapacity(10);
00569 startYList.ensureCapacity(10);
00570 lastXList.ensureCapacity(10);
00571 lastYList.ensureCapacity(10);
00572
00573 fingerList.add(0);
00574 startXList.add((int) event.getX(0));
00575 startYList.add((int) event.getY(0));
00576 lastXList.add((int) event.getX(0));
00577 lastYList.add((int) event.getY(0));
00578 Log.i("JskAndroidGui:TouchEvent", "[ACTION_DOWN] startXList = "
00579 + startXList + ", startYList = " + startYList);
00580 SendDebugMsg("ActionDown", fingerCount, startXList.get(0),
00581 startYList.get(0));
00582
00583 Log.i("JskAndroidGui:TouchEvent", "[ACTION_DOWN] END");
00584 if (isGetTemplate) {
00585 pl.clear();
00586 pl.add(new Point((int) event.getX(0), (int) event.getY(0)));
00587 boundingBoxView.drawBox(pl);
00588 }
00589 break;
00590
00591
00592 case MotionEvent.ACTION_POINTER_DOWN:
00593 Log.i("JskAndroidGui:TouchEvent", "[ACTION_POINTER_DOWN] START");
00594 fingerCount = event.getPointerCount();
00595
00596 for (int i = 0; i < curFingerCount; i++) {
00597 int fingerId = event.getPointerId(i);
00598 if (!fingerList.contains(fingerId)) {
00599 int pointerId = event.findPointerIndex(fingerId);
00600 fingerList.add(fingerId);
00601 startXList.add((int) event.getX(pointerId));
00602 startYList.add((int) event.getY(pointerId));
00603 lastXList.add((int) event.getX(pointerId));
00604 lastYList.add((int) event.getY(pointerId));
00605 }
00606 }
00607
00608 if (fingerCount == 2 && isZoomCamera) {
00609 startROIRate = ROIRate;
00610 startROIcenterX = getImageCoordX((startXList.get(0) + startXList
00611 .get(1)) / 2);
00612 startROIcenterY = getImageCoordY((startYList.get(0) + startYList
00613 .get(1)) / 2);
00614 }
00615
00616 Log.i("JskAndroidGui:TouchEvent",
00617 "[ACTION_POINTER_DOWN] startXList = " + startXList
00618 + ", startYList = " + startYList);
00619 Log.i("JskAndroidGui:TouchEvent", "[ACTION_POINTER_DOWN] END");
00620 break;
00621
00622
00623 case MotionEvent.ACTION_MOVE:
00624 Log.i("JskAndroidGui:TouchEvent", "[ACTION_MOVE] START");
00625 if (isGetTemplate) {
00626 pl.add(new Point((int) event.getX(0), (int) event.getY(0)));
00627 boundingBoxView.drawBox(pl);
00628 }
00629 for (int i = 0; i < curFingerCount; ++i) {
00630 int fingerId = event.getPointerId(i);
00631 int pointerId = event.findPointerIndex(fingerId);
00632 lastXList.set(fingerList.indexOf(fingerId),
00633 (int) event.getX(pointerId));
00634 lastYList.set(fingerList.indexOf(fingerId),
00635 (int) event.getY(pointerId));
00636 }
00637
00638 Log.i("JskAndroidGui:TouchEvent", "[ACTION_MOVE] lastXList = "
00639 + lastXList + ", lastYList = " + lastYList + ", fingerList"
00640 + fingerList);
00641 SendDebugMsg("ActionMove", fingerCount, lastXList.get(0),
00642 lastYList.get(0));
00643 if (isCircularManipulation && fingerCount == 2
00644 && curFingerCount == 2) {
00645 SendTaskMsg("DrawLine", 0, "TOUCHMOVE", 0, null, 0, fingerList,
00646 lastXList, lastYList, 0, 0);
00647 } else if (isCircularManipulation && fingerCount == 3) {
00648 SendTaskMsg("DrawCircle", RobotArmId, "TOUCHMOVE", 0, null, 0,
00649 fingerList, lastXList, lastYList, 0, 0);
00650 } else if (isZoomCamera && fingerCount == 2 && curFingerCount == 2) {
00651 final float startDistance = Math.round(Math.sqrt(Math.pow(
00652 startXList.get(1) - startXList.get(0), 2)
00653 + Math.pow(startYList.get(1) - startYList.get(0), 2)));
00654 final float endDistance = Math.round(Math.sqrt(Math.pow(
00655 lastXList.get(1) - lastXList.get(0), 2)
00656 + Math.pow(lastYList.get(1) - lastYList.get(0), 2)));
00657 final float pinchLength = startDistance - endDistance;
00658 final int center_x = (lastXList.get(0) + lastXList.get(1)) / 2;
00659 final int center_y = (lastYList.get(0) + lastYList.get(1)) / 2;
00660 SendDebugMsg("PINCH", pinchLength, center_x, center_y);
00661 ZoomCamera(center_x, center_y, startDistance, endDistance);
00662 } else if (isMovingFingerInfo) {
00663 SendTaskMsg("MovingPointInfo", 0, "TOUCHMOVE", 0, null, 0,
00664 fingerList, lastXList, lastYList, 0, 0);
00665 }
00666 Log.i("JskAndroidGui:TouchEvent", "[ACTION_MOVE] END");
00667 break;
00668
00669
00670 case MotionEvent.ACTION_UP:
00671 Log.i("JskAndroidGui:TouchEvent", "[ACTION_UP] START");
00672 if (isMovingFingerInfo) {
00673 return true;
00674 }
00675
00676 if (fingerCount == 1) {
00677 if (isGetTemplate) {
00678 boundingBoxView.drawBox(pl);
00679 boundingBoxView.setSelect();
00680 unSetGetTemplate();
00681
00682
00683 LayoutInflater inflater = LayoutInflater.from(context);
00684 final View view = inflater.inflate(R.layout.dialog, null);
00685 final EditText editText = (EditText) view
00686 .findViewById(R.id.editText1);
00687 new AlertDialog.Builder(context)
00688 .setTitle("Template name")
00689 .setView(view)
00690 .setPositiveButton("Save",
00691 new DialogInterface.OnClickListener() {
00692 @Override
00693 public void onClick(
00694 DialogInterface dialog,
00695 int which) {
00696
00697 SensorImageViewInfo.this
00698 .SendTemplateInfo(editText
00699 .getText()
00700 .toString(),
00701 startXList.get(0),
00702 startYList.get(0),
00703 lastXList.get(0),
00704 lastYList.get(0));
00705 Toast.makeText(
00706 context,
00707 "Create Template Image: "
00708 + editText
00709 .getText()
00710 .toString(),
00711 Toast.LENGTH_SHORT).show();
00712 Log.i("JskAndroidGui:ButtonClicked",
00713 "Create Template Image");
00714 }
00715 }).show();
00716 } else {
00717 start_x = startXList.get(0);
00718 start_y = startYList.get(0);
00719 final float swipeLength = Math.round(Math.sqrt(Math.pow(
00720 getImageCoordX(lastXList.get(0))
00721 - getImageCoordX(start_x), 2)
00722 + Math.pow(getImageCoordY(lastYList.get(0))
00723 - getImageCoordY(start_y), 2)));
00724
00725 if (swipeLength >= minLength) {
00726 SendDebugMsg("SWIPE", swipeLength, lastXList.get(0),
00727 lastYList.get(0));
00728 final int X = (int) (getImageCoordX(start_x) - getImageCoordX(lastXList
00729 .get(0)));
00730 final int Y = (int) (getImageCoordY(lastYList.get(0)) - getImageCoordY(start_y));
00731 final double r = Math.atan2(Y, X);
00732 float swipeAngle = Math.round(r * 180 / Math.PI);
00733 if (swipeAngle < 0)
00734 swipeAngle = 360 - Math.abs(swipeAngle);
00735 if ((swipeAngle <= 45) && (swipeAngle >= 0)) {
00736 MoveNeck("left", swipeLength);
00737 SetSwipeDetected(SWIPE_LEFT);
00738 } else if ((swipeAngle <= 360) && (swipeAngle >= 315)) {
00739 MoveNeck("left", swipeLength);
00740 SetSwipeDetected(SWIPE_LEFT);
00741 } else if ((swipeAngle >= 135) && (swipeAngle <= 225)) {
00742 MoveNeck("right", swipeLength);
00743 SetSwipeDetected(SWIPE_RIGHT);
00744 } else if ((swipeAngle > 45) && (swipeAngle < 135)) {
00745 MoveNeck("down", swipeLength);
00746 SetSwipeDetected(SWIPE_DOWN);
00747 } else {
00748 MoveNeck("up", swipeLength);
00749 SetSwipeDetected(SWIPE_UP);
00750 }
00751 } else {
00752 if (isPushOnce) {
00753 isPushOnce = false;
00754 SendCommandMsg("PushOnce", 0, "TOUCH", 0, null, 0,
00755 fingerList, startXList, startYList,
00756 start_x, start_y);
00757 } else if (isPickOnce) {
00758 isPickOnce = false;
00759
00760
00761 SendCommandMsg("PickObjectSelected", RobotArmId,
00762 "TOUCH", 0, null, 0, fingerList,
00763 startXList, startYList, start_x, start_y);
00764 } else if (isPlaceOnce) {
00765 isPlaceOnce = false;
00766 SendCommandMsg("PlaceOnce", 0, "TOUCH", 0, null, 0,
00767 fingerList, startXList, startYList,
00768 start_x, start_y);
00769 } else if (isPassToHumanOnce) {
00770 isPassToHumanOnce = false;
00771 SendCommandMsg("PassToHumanOnce", RobotArmId,
00772 "TOUCH", 0, null, 0, fingerList,
00773 startXList, startYList, start_x, start_y);
00774 } else if (TouchMode == 0) {
00775 SendCommandMsg("MoveCameraCenter", 0, "TOUCH", 0,
00776 null, 0, fingerList, startXList,
00777 startYList, start_x, start_y);
00778 } else if (TouchMode == 1) {
00779 SendCommandMsg("Show3DScreenPoint", 0, "TOUCH", 0,
00780 null, 0, fingerList, startXList,
00781 startYList, start_x, start_y);
00782 }
00783 }
00784 }
00785 } else if (isCircularManipulation && fingerCount == 3) {
00786 SendTaskMsg("CircularManipulation", RobotArmId, "TOUCH", 0,
00787 null, 0, fingerList, lastXList, lastYList, 0, 0);
00788 } else if (fingerCount == 4) {
00789 if (startYList.get(0) >= imageHeight / 2
00790 && imageHeight / 2 >= lastYList.get(0)) {
00791 SendCommandMsg("TorsoUp", 0, "TOUCH", 0, null, 0,
00792 fingerList, startXList, startYList, 0, 0);
00793 } else if (startYList.get(0) <= imageHeight / 2
00794 && imageHeight / 2 <= lastYList.get(0)) {
00795 SendCommandMsg("TorsoDown", 0, "TOUCH", 0, null, 0,
00796 fingerList, startXList, startYList, 0, 0);
00797 } else {
00798 SendCommandMsg("StopMotion", 0, "TOUCH", 0, null, 0,
00799 fingerList, startXList, startYList, 0, 0);
00800 }
00801 } else if (fingerCount == 5
00802 && (Math.abs(startXList.get(0) - lastXList.get(0)) > minLength)) {
00803 SendCommandMsg("TuckArmPose", RobotArmId, "TOUCH", 0, null, 0,
00804 fingerList, startXList, startYList, 0, 0);
00805 }
00806
00807 Log.i("JskAndroidGui:TouchEvent", "[ACTION_UP] END, ResetValue();");
00808 break;
00809
00810
00811 case MotionEvent.ACTION_POINTER_UP:
00812 Log.i("JskAndroidGui:TouchEvent", "[ACTION_POINTER_UP] START");
00813 Log.v("JskAndroidGui:TouchEvent",
00814 "[ACTION_POINTER_UP] fingerCount = " + fingerCount
00815 + ", curFingerCount_temp = " + curFingerCount);
00816 Log.i("JskAndroidGui:TouchEvent",
00817 "[ACTION_POINTER_UP] isMovingFingerInfo = "
00818 + isMovingFingerInfo);
00819 if (curFingerCount == 2 && fingerCount == 2 && isMovingFingerInfo) {
00820 int p1x, p1y, p2x, p2y, p3x, p3y;
00821 if (Math.abs(startXList.get(0) - lastXList.get(0)) < minLength) {
00822 Log.i("JskAndroidGui:TouchEvent",
00823 "[ACTION_POINTER_UP] true");
00824 p1x = startXList.get(0);
00825 p1y = startYList.get(0);
00826 p2x = startXList.get(1);
00827 p2y = startYList.get(1);
00828 p3x = lastXList.get(1);
00829 p3y = lastYList.get(1);
00830 } else {
00831 Log.i("JskAndroidGui:TouchEvent",
00832 "[ACTION_POINTER_UP] false");
00833 p1x = startXList.get(1);
00834 p1y = startYList.get(1);
00835 p2x = startXList.get(0);
00836 p2y = startYList.get(0);
00837 p3x = lastXList.get(0);
00838 p3y = lastYList.get(0);
00839 }
00840 if (isClockWise(p1x, p1y, p2x, p2y, p3x, p3y)) {
00841 Log.i("JskAndroidGui:TouchEvent", "[ACTION_POINTER_UP] 45");
00842 SendCommandMsg("RotateGripper", RobotArmId, "PINCH", 45,
00843 null, 0, fingerList, startXList, startYList, 0, 0);
00844 } else {
00845 Log.i("JskAndroidGui:TouchEvent", "[ACTION_POINTER_UP] -45");
00846 SendCommandMsg("RotateGripper", RobotArmId, "PINCH", -45,
00847 null, 0, fingerList, startXList, startYList, 0, 0);
00848 }
00849
00850
00851
00852
00853
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863 }
00864
00865 Log.i("JskAndroidGui:TouchEvent", "[ACTION_POINTER_UP] END");
00866 break;
00867
00868
00869 case MotionEvent.ACTION_OUTSIDE:
00870 Log.i("JskAndroidGui:TouchEvent", "[ACTION_CANCEL] START");
00871 Log.i("JskAndroidGui:TouchEvent", "[ACTION_CANCEL] END");
00872 break;
00873
00874
00875 case MotionEvent.ACTION_CANCEL:
00876 Log.i("JskAndroidGui:TouchEvent", "[ACTION_CANCEL] START");
00877 Log.i("JskAndroidGui:TouchEvent", "[ACTION_CANCEL] END");
00878 break;
00879 }
00880 Log.i("JskAndroidGui:TouchEvent", "END");
00881
00882 return super.onTouchEvent(event);
00883 }
00884
00885 @Override
00886 public void run() {
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897 if (SwipeDetectedType != SWIPE_NONE && SwipeCounter < SWIPE_WAIT_TIME) {
00898 Log.v("JskAndroidGui:onNewMessage",
00899 "[changing Bitmap] changing now " + (SwipeCounter + 1)
00900 + " / " + SWIPE_WAIT_TIME);
00901 this.invalidate();
00902 SwipeCounter++;
00903
00904
00905 final int Width = bitmap.getWidth();
00906 final int Height = bitmap.getHeight();
00907 Bitmap bitmap_output = Bitmap.createBitmap(Width, Height,
00908 Bitmap.Config.ARGB_8888);
00909 int[] pixels = new int[Width * Height];
00910 bitmap.getPixels(pixels, 0, Width, 0, 0, Width, Height);
00911
00912 for (int x = 0; x < Width; x++) {
00913 for (int y = 0; y < Height; y++) {
00914
00915 if ((SwipeDetectedType == SWIPE_UP && (-10 * x + y > 0 || 10 * Width < 10
00916 * x + y))
00917 || (SwipeDetectedType == SWIPE_DOWN && (10 * x + y < Height || 10
00918 * Width - Height < 10 * x - y))
00919 || (SwipeDetectedType == SWIPE_LEFT && (-1 * x + 10
00920 * y < 0 || 10 * Height < x + 10 * y))
00921 || (SwipeDetectedType == SWIPE_RIGHT && (x + 10 * y < Width || -1
00922 * Width + 10 * Height < -1 * x + 10 * y))) {
00923 pixels[x + y * Width] = Color.rgb(Color.BLACK,
00924 Color.BLACK, Color.BLACK);
00925 }
00926 }
00927 }
00928 bitmap_output.setPixels(pixels, 0, Width, 0, 0, Width, Height);
00929 setImageBitmap(bitmap_output);
00930
00931
00932
00933
00934
00935
00936 } else if (SwipeCounter >= SWIPE_WAIT_TIME) {
00937 Log.v("JskAndroidGui:onNewMessage",
00938 "[changing Bitmap] back to normal");
00939 unSetSwipeDetected();
00940 } else {
00941 setImageBitmap(bitmap);
00942 }
00943 }
00944
00945 @Override
00946 public void onError(Node node, Throwable throwable) {
00947 }
00948
00949 @Override
00950 public void onShutdown(Node node) {
00951 node.shutdown();
00952 }
00953
00954 @Override
00955 public void onShutdownComplete(Node node) {
00956 }
00957
00958 @Override
00959 public GraphName getDefaultNodeName() {
00960 return GraphName.of("jsk_android_gui/SensorImageViewInfo");
00961 }
00962 }