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(3);
  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                                         paint.setStyle(Style.STROKE);
  63                                         paint.setStrokeWidth(3);
  64                                         paint.setColor(Color.BLACK);
  65                                         canvas.drawLine((int)(getWidth()*0.95),
  66                                                                         (int)(getHeight()*0.97),
  67                                                                         (int)(getWidth()*0.95-norm/4),
  68                                                                         (int)(getHeight()*0.97),
  69                                                                         paint);
  70                                         paint.setStrokeWidth(2);
  71                                         canvas.drawText("1 m", (int)(getWidth()*0.95-norm/8), (int)(getHeight()*0.97-10), paint);
  72                                         paint.setStrokeWidth(3);
  73                                         paint.setStyle(Style.FILL);
  74                                         paint.setColor(Color.argb(40, 62, 62, 171));
  75                                         if (!hasBeenDrawn) {
  76                                                 path.reset();
  77                                                 path.moveTo(getWidth()/2, getHeight());
  78                                                 for(int i = HOKUYO_INDEX_LOWER+1; i <= HOKUYO_INDEX_UPPER; i++) {
  79                                                         if (data[i] > 4000)
  80                                                                 data[i] = 4000;
  81                                                         data[i] = (int)(((double)data[i]/4000)*norm);
  82                                                         if (data[i] < 5)
  83                                                                 data[i] = 5;
  84                                             int x = (int)(getWidth()/2) - (int)(data[i] * Math.sin(HOKUYO_INDEX_TO_RAD(i)));
  85                                             int y = getHeight() - (int)(data[i] * Math.cos(HOKUYO_INDEX_TO_RAD(i)));
  86                                                         path.lineTo(x, y);
  87                                                 }
  88                                                 path.close();
  89                                                 hasBeenDrawn = true;
  90                                         }
  91                                 }
  92                                 finally {
  93                                         lock.unlock();
  94                                 }
  95                         }
  96                         else {
  97                                 path.reset();
  98                         }
  99                         canvas.drawPath(path, paint);
 100                         paint.setStyle(Style.STROKE);
 101                         paint.setColor(Color.BLACK);
 102                         canvas.drawPath(path, paint);
 103
 104                         if (isMonitoring) {
 105                                 lockMotion.lock();
 106                                 try {
 107                                         double norm;
 108                                         if (getHeight() < getWidth())
 109                                                 norm = getHeight()*0.125;
 110                                         else
 111                                                  norm = getWidth()*0.125;
 112                                         paint.setStyle(Style.STROKE);
 113                                         paint.setStrokeWidth(1);
 114                                         paint.setColor(Color.BLACK);
 115                                         canvas.drawLine((int)(10),
 116                                                                         (int)(10+norm*1.5),
 117                                                                         (int)(10+norm*3),
 118                                                                         (int)(10+norm*1.5),
 119                                                                         paint);
 120                                         canvas.drawLine((int)(10+norm*1.5),
 121                                                                         (int)(10),
 122                                                                         (int)(10+norm*1.5),
 123                                                                         (int)(10+norm*3),
 124                                                                         paint);
 125                                         paint.setStrokeWidth(4);
 126                                         paint.setColor(Color.BLUE);
 127                                         canvas.drawLine((int)(10+norm*1.5),
 128                                                                         (int)(10+norm*1.5),
 129                                                                         (int)(speedCo[0]*norm+10+norm*1.5),
 130                                                                         (int)(speedCo[1]*norm+10+norm*1.5),
 131                                                                         paint);
 132                                 } finally {
 133                                         lockMotion.unlock();
 134                                 }
 135                         }
 136                 } finally {
 137                         rcLock.unlock();
 138                 }
 139         }
 140
 141         private void calculateCoordinates(short[] speed) {
 142                 double x, y;
 143                 double temp[] = new double[2];
 144
 145                 temp[0] = (double)speed[0]/16000;
 146                 temp[1] = (double)speed[1]/16000;
 147
 148                 y = (temp[0]+temp[1])/2;
 149                 x = (temp[0]-y)/0.30;
 150
 151                 speedCo[1] = -y;
 152                 speedCo[0] = speedCo[1]>0 ? -x : x;
 153         }
 154
 155         public void run(boolean run) {
 156                 wcLock.lock();
 157                 try {
 158                         isRunning = run;
 159                 } finally {
 160                         wcLock.unlock();
 161                 }
 162         }
 163
 164         public void runMotion(boolean run) {
 165                 wcLock.lock();
 166                 try {
 167                         isMonitoring = run;
 168                 } finally {
 169                         wcLock.unlock();
 170                 }
 171         }
 172
 173         public void setData(int[] data) {
 174                 if (lock.tryLock()) {
 175                         try {
 176                                 this.data = data.clone();
 177                                 hasBeenDrawn = false;
 178                         }
 179                         finally {
 180                                 lock.unlock();
 181                         }
 182                         postInvalidate();
 183                 }
 184         }
 185
 186         public void setDataMotion(short[] speed) {
 187                 if (lockMotion.tryLock()) {
 188                         try {
 189                                 short[] speedCl = speed.clone();
 190                                 calculateCoordinates(speedCl);
 191                         }
 192                         finally {
 193                                 lockMotion.unlock();
 194                         }
 195                         postInvalidate();
 196                 }
 197         }
 198
 199         public static double HOKUYO_INDEX_TO_DEG(int index) {
 200                 return ((HOKUYO_START_ANGLE-index*360.0/HOKUYO_SPLIT_DIVISION) * HOKUYO_ORIENTATION);
 201         }
 202
 203         public static double HOKUYO_INDEX_TO_RAD(int index) {
 204                 return (HOKUYO_INDEX_TO_DEG(index)/180.0*Math.PI);
 205         }
 206
 207         public static int HOKUYO_DEG_TO_INDEX(double d) {
 208                 return (int)((HOKUYO_START_ANGLE-(d)/HOKUYO_ORIENTATION)/(360.0/HOKUYO_SPLIT_DIVISION));
 209         }
 210 }