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 double[] speedCo = new double[2];
  30         private Paint paint = new Paint();
  31         private Path path = new Path();
  32
  33         private boolean isRunning = false;
  34         private boolean isMonitoring = false;
  35         private boolean hasBeenDrawn = true;
  36
  37         private final ReentrantLock lock = new ReentrantLock();
  38         private final ReentrantLock lockMotion = new ReentrantLock();
  39         private final ReentrantReadWriteLock controlRrwl = new ReentrantReadWriteLock(true);
  40         private final ReadLock rcLock = controlRrwl.readLock();
  41         private final WriteLock wcLock = controlRrwl.writeLock();
  42
  43         public HokuyoView(Context context, AttributeSet attrs) {
  44                 super(context, attrs);
  45
  46                 paint.setStyle(Style.STROKE);
  47                 paint.setStrokeWidth(4);
  48                 paint.setColor(Color.BLACK);
  49                 paint.setAntiAlias(false);
  50         }
  51
  52         @Override
  53         protected void onDraw(Canvas canvas) {
  54                 rcLock.lock();
  55                 try {
  56                         if (isRunning) {
  57                                 lock.lock();
  58                                 try {
  59                                         double norm = (double)getWidth()/(2*COSINUS);
  60                                         if (norm > getHeight())
  61                                                 norm = getHeight();
  62                                         canvas.drawLine((int)(getWidth()*0.95),
  63                                                                         (int)(getHeight()*0.97),
  64                                                                         (int)(getWidth()*0.95-norm/4),
  65                                                                         (int)(getHeight()*0.97),
  66                                                                         paint);
  67                                         paint.setStrokeWidth(2);
  68                                         canvas.drawText("1 m", (int)(getWidth()*0.95-norm/8), (int)(getHeight()*0.97-10), paint);
  69                                         paint.setStrokeWidth(3);
  70                                         if (!hasBeenDrawn) {
  71                                                 path.reset();
  72                                                 path.moveTo(getWidth()/2, getHeight());
  73                                                 for(int i = HOKUYO_INDEX_LOWER+1; i <= HOKUYO_INDEX_UPPER; i++) {
  74                                                         if (data[i] > 4000)
  75                                                                 data[i] = 4000;
  76                                                         data[i] = (int)(((double)data[i]/4000)*norm);
  77                                                         if (data[i] < 5)
  78                                                                 data[i] = 5;
  79                                             int x = (int)(getWidth()/2) - (int)(data[i] * Math.sin(HOKUYO_INDEX_TO_RAD(i)));
  80                                             int y = getHeight() - (int)(data[i] * Math.cos(HOKUYO_INDEX_TO_RAD(i)));
  81                                                         path.lineTo(x, y);
  82                                                 }
  83                                                 path.close();
  84                                                 hasBeenDrawn = true;
  85                                         }
  86                                 }
  87                                 finally {
  88                                         lock.unlock();
  89                                 }
  90                         }
  91                         else {
  92                                 path.reset();
  93                         }
  94                         canvas.drawPath(path, paint);
  95
  96                         if (isMonitoring) {
  97                                 lockMotion.lock();
  98                                 try {
  99                                         double norm;
 100                                         if (getHeight() < getWidth())
 101                                                 norm = getHeight()*0.125;
 102                                         else
 103                                                  norm = getWidth()*0.125;
 104                                         paint.setStrokeWidth(1);
 105                                         canvas.drawLine((int)(10),
 106                                                                         (int)(10+norm*1.5),
 107                                                                         (int)(10+norm*3),
 108                                                                         (int)(10+norm*1.5),
 109                                                                         paint);
 110                                         canvas.drawLine((int)(10+norm*1.5),
 111                                                                         (int)(10),
 112                                                                         (int)(10+norm*1.5),
 113                                                                         (int)(10+norm*3),
 114                                                                         paint);
 115                                         paint.setStrokeWidth(4);
 116                                         canvas.drawLine((int)(10+norm*1.5),
 117                                                                         (int)(10+norm*1.5),
 118                                                                         (int)(speedCo[0]*norm+10+norm*1.5),
 119                                                                         (int)(speedCo[1]*norm+10+norm*1.5),
 120                                                                         paint);
 121                                 } finally {
 122                                         lockMotion.unlock();
 123                                 }
 124                         }
 125                 } finally {
 126                         rcLock.unlock();
 127                 }
 128         }
 129
 130         private void calculateCoordinates(short[] speed) {
 131                 double x, y;
 132                 double temp[] = new double[2];
 133
 134                 temp[0] = (double)speed[0]/16000;
 135                 temp[1] = (double)speed[1]/16000;
 136
 137                 y = (temp[0]+temp[1])/2;
 138                 x = (temp[0]-y)/0.30;
 139
 140                 speedCo[1] = -y;
 141                 speedCo[0] = speedCo[1]>0 ? -x : x;
 142         }
 143
 144         public void run(boolean run) {
 145                 wcLock.lock();
 146                 try {
 147                         isRunning = run;
 148                 } finally {
 149                         wcLock.unlock();
 150                 }
 151         }
 152
 153         public void runMotion(boolean run) {
 154                 wcLock.lock();
 155                 try {
 156                         isMonitoring = run;
 157                 } finally {
 158                         wcLock.unlock();
 159                 }
 160         }
 161
 162         public void setData(int[] data) {
 163                 if (lock.tryLock()) {
 164                         try {
 165                                 this.data = data.clone();
 166                                 hasBeenDrawn = false;
 167                         }
 168                         finally {
 169                                 lock.unlock();
 170                         }
 171                         postInvalidate();
 172                 }
 173         }
 174
 175         public void setDataMotion(short[] speed) {
 176                 if (lockMotion.tryLock()) {
 177                         try {
 178                                 short[] speedCl = speed.clone();
 179                                 calculateCoordinates(speedCl);
 180                         }
 181                         finally {
 182                                 lockMotion.unlock();
 183                         }
 184                         postInvalidate();
 185                 }
 186         }
 187
 188         public static double HOKUYO_INDEX_TO_DEG(int index) {
 189                 return ((HOKUYO_START_ANGLE-index*360.0/HOKUYO_SPLIT_DIVISION) * HOKUYO_ORIENTATION);
 190         }
 191
 192         public static double HOKUYO_INDEX_TO_RAD(int index) {
 193                 return (HOKUYO_INDEX_TO_DEG(index)/180.0*Math.PI);
 194         }
 195
 196         public static int HOKUYO_DEG_TO_INDEX(double d) {
 197                 return (int)((HOKUYO_START_ANGLE-(d)/HOKUYO_ORIENTATION)/(360.0/HOKUYO_SPLIT_DIVISION));
 198         }
 199 }