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