orte/Robot_Demo/src/org/ocera/orte/demo/HokuyoView.java

   1 package org.ocera.orte.demo;
   2
   3 import java.util.concurrent.locks.ReentrantLock;
   4 import java.util.concurrent.locks.ReentrantReadWriteLock;
   5 import java.util.concurrent.locks.ReentrantReadWriteLock.ReadLock;
   6 import java.util.concurrent.locks.ReentrantReadWriteLock.WriteLock;
   7
   8 import android.content.Context;
   9 import android.graphics.Canvas;
  10 import android.graphics.Color;
  11 import android.graphics.Paint;
  12 import android.graphics.Path;
  13 import android.graphics.Paint.Style;
  14 import android.util.AttributeSet;
  15 import android.view.View;
  16
  17 public class HokuyoView extends View {
  18
  19         public static final double HOKUYO_START_ANGLE = 239.77/2;
  20         public static final int HOKUYO_SPLIT_DIVISION = 1024;
  21         public static final int HOKUYO_ORIENTATION = 1;
  22         public static final double HOKUYO_RANGE_ANGLE_LEFT = 70.0;
  23         public static final double HOKUYO_RANGE_ANGLE_RIGHT = 70.0;
  24         public static final int HOKUYO_INDEX_LOWER = HOKUYO_DEG_TO_INDEX(HOKUYO_RANGE_ANGLE_LEFT);
  25         public static final int HOKUYO_INDEX_UPPER = HOKUYO_DEG_TO_INDEX(-HOKUYO_RANGE_ANGLE_RIGHT);
  26         public static final double COSINUS = Math.cos((90-HOKUYO_RANGE_ANGLE_LEFT)/180.0*Math.PI);
  27
  28         private int[] data = new int[681];
  29         private Paint paint = new Paint();
  30         private Path path = new Path();
  31
  32         private boolean isRunning = false;
  33         private boolean hasBeenDrawn = true;
  34         private final ReentrantLock lock = new ReentrantLock();
  35         private final ReentrantReadWriteLock controlRrwl = new ReentrantReadWriteLock(true);
  36         private final ReadLock rcLock = controlRrwl.readLock();
  37         private final WriteLock wcLock = controlRrwl.writeLock();
  38
  39         public HokuyoView(Context context, AttributeSet attrs) {
  40                 super(context, attrs);
  41
  42                 paint.setStyle(Style.STROKE);
  43                 paint.setStrokeWidth(4);
  44                 paint.setColor(Color.BLACK);
  45                 paint.setAntiAlias(false);
  46         }
  47
  48         @Override
  49         protected void onDraw(Canvas canvas) {
  50                 rcLock.lock();
  51                 try {
  52                         if (isRunning) {
  53                                 lock.lock();
  54                                 try {
  55                                         double norm = (double)getWidth()/(2*COSINUS);
  56                                         if (norm > getHeight())
  57                                                 norm = getHeight();
  58                                         canvas.drawLine((int)(getWidth()*0.95),
  59                                                                         (int)(getHeight()*0.97),
  60                                                                         (int)(getWidth()*0.95-norm/4),
  61                                                                         (int)(getHeight()*0.97),
  62                                                                         paint);
  63                                         paint.setStrokeWidth(2);
  64                                         canvas.drawText("1 m", (int)(getWidth()*0.95-norm/8), (int)(getHeight()*0.97-10), paint);
  65                                         paint.setStrokeWidth(3);
  66                                         if (!hasBeenDrawn) {
  67                                                 path.reset();
  68                                                 path.moveTo(getWidth()/2, getHeight());
  69                                                 for(int i = HOKUYO_INDEX_LOWER+1; i <= HOKUYO_INDEX_UPPER; i++) {
  70                                                         data[i] = (int)(((double)data[i]/4000)*norm);
  71                                             int x = (int)(getWidth()/2) - (int)(data[i] * Math.sin(HOKUYO_INDEX_TO_RAD(i)));
  72                                             int y = getHeight() - (int)(data[i] * Math.cos(HOKUYO_INDEX_TO_RAD(i)));
  73                                                         path.lineTo(x, y);
  74                                                 }
  75                                                 path.close();
  76                                                 hasBeenDrawn = true;
  77                                         }
  78                                 }
  79                                 finally {
  80                                         lock.unlock();
  81                                 }
  82                         }
  83                         else {
  84                                 path.reset();
  85                         }
  86                         canvas.drawPath(path, paint);
  87                 } finally {
  88                         rcLock.unlock();
  89                 }
  90         }
  91
  92         public void run(boolean run) {
  93                 wcLock.lock();
  94                 try {
  95                         isRunning = run;
  96                 } finally {
  97                         wcLock.unlock();
  98                 }
  99         }
 100
 101         public void setData(int[] data) {
 102                 if (lock.tryLock()) {
 103                         try {
 104                                 this.data = data.clone();
 105                                 hasBeenDrawn = false;
 106                         }
 107                         finally {
 108                                 lock.unlock();
 109                         }
 110                         postInvalidate();
 111                 }
 112         }
 113
 114         public static double HOKUYO_INDEX_TO_DEG(int index) {
 115                 return ((HOKUYO_START_ANGLE-index*360.0/HOKUYO_SPLIT_DIVISION) * HOKUYO_ORIENTATION);
 116         }
 117
 118         public static double HOKUYO_INDEX_TO_RAD(int index) {
 119                 return (HOKUYO_INDEX_TO_DEG(index)/180.0*Math.PI);
 120         }
 121
 122         public static int HOKUYO_DEG_TO_INDEX(double d) {
 123                 return (int)((HOKUYO_START_ANGLE-(d)/HOKUYO_ORIENTATION)/(360.0/HOKUYO_SPLIT_DIVISION));
 124         }
 125 }