ROBOT_DEMO: fix thread sync. problems better

Use ReentrantLock and ReentrantReadWriteLock for thread synchronization.

diff --git a/orte/Robot_Demo/src/org/ocera/orte/demo/HokuyoScanSubscribe.java b/orte/Robot_Demo/src/org/ocera/orte/demo/HokuyoScanSubscribe.java
index 97b5cd58fe112eb69a0754a6b76c41b99664afa4..485c67289d1f42ffa580569a2a9bf2f3fe8f99be 100644 (file)
--- a/orte/Robot_Demo/src/org/ocera/orte/demo/HokuyoScanSubscribe.java
+++ b/orte/Robot_Demo/src/org/ocera/orte/demo/HokuyoScanSubscribe.java
@@ -1,5 +1,9 @@
 package org.ocera.orte.demo;
 
+import java.util.concurrent.locks.ReentrantReadWriteLock;
+import java.util.concurrent.locks.ReentrantReadWriteLock.ReadLock;
+import java.util.concurrent.locks.ReentrantReadWriteLock.WriteLock;
+
 import org.ocera.orte.DomainApp;
 import org.ocera.orte.Subscription;
 import org.ocera.orte.SubscriptionCallback;
@@ -18,7 +22,11 @@ public class HokuyoScanSubscribe extends SubscriptionCallback{
        private HokuyoScanType hokuyomsg;
        private DomainApp domainApp;
        private SubsProp subProps;
+       
        private boolean isCancelled = true;
+       private final ReentrantReadWriteLock controlRrwl = new ReentrantReadWriteLock(true);
+       private final ReadLock rcLock = controlRrwl.readLock();
+       private final WriteLock wcLock = controlRrwl.writeLock();
        
        public HokuyoScanSubscribe(DomainApp domainApp, HokuyoView view) {
                this.view = view;
@@ -39,17 +47,32 @@ public class HokuyoScanSubscribe extends SubscriptionCallback{
        }
        
        public void start() {
-           isCancelled = false;
-               sub = domainApp.createSubscription(subProps, hokuyomsg, this);
+               wcLock.lock();
+               try {
+                       isCancelled = false;
+                       sub = domainApp.createSubscription(subProps, hokuyomsg, this);
+               } finally {
+                       wcLock.unlock();
+               }
        }
        
        public void cancel() {
-               sub.destroy();
-               isCancelled = true;
+               wcLock.lock();
+               try {
+                       isCancelled = true;
+                       sub.destroy();
+               } finally {
+                       wcLock.unlock();
+               }
        }
        
        public boolean isCancelled() {
-               return isCancelled;
+               rcLock.lock();
+               try {
+                       return isCancelled;
+               } finally {
+                       rcLock.unlock();
+               }
        }
        
     public void callback(RecvInfo info, MessageData msg) {

diff --git a/orte/Robot_Demo/src/org/ocera/orte/demo/HokuyoView.java b/orte/Robot_Demo/src/org/ocera/orte/demo/HokuyoView.java
index 3351b81f54693b1c243ec6bfb97891aa42fdb957..dad1301d80c2af772ad158ddd47b07e7d9dfd303 100644 (file)
--- a/orte/Robot_Demo/src/org/ocera/orte/demo/HokuyoView.java
+++ b/orte/Robot_Demo/src/org/ocera/orte/demo/HokuyoView.java
@@ -1,5 +1,10 @@
 package org.ocera.orte.demo;
 
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.concurrent.locks.ReentrantReadWriteLock;
+import java.util.concurrent.locks.ReentrantReadWriteLock.ReadLock;
+import java.util.concurrent.locks.ReentrantReadWriteLock.WriteLock;
+
 import android.content.Context;
 import android.graphics.Canvas;
 import android.graphics.Color;
@@ -20,48 +25,79 @@ public class HokuyoView extends View {
        private int[] data = new int[681];
        private Paint paint = new Paint();
        private Path path = new Path();
+       
        private boolean isRunning = false;
        private boolean hasBeenDrawn = true;
+       private final ReentrantLock lock = new ReentrantLock();
+       private final ReentrantReadWriteLock controlRrwl = new ReentrantReadWriteLock(true);
+       private final ReadLock rcLock = controlRrwl.readLock();
+       private final WriteLock wcLock = controlRrwl.writeLock();
 
        public HokuyoView(Context context, AttributeSet attrs) {
                super(context, attrs);
+               
+               paint.setStyle(Style.STROKE);
+               paint.setStrokeWidth(4);
+               paint.setColor(Color.BLACK);
+               paint.setAntiAlias(false);
        }
 
        @Override
        protected void onDraw(Canvas canvas) {
-               paint.setStyle(Style.STROKE);
-               paint.setStrokeWidth(3);
-               paint.setColor(Color.BLACK);
-               paint.setAntiAlias(true);
-               
-               path.reset();
-               if (isRunning) {
-                       path.moveTo(getWidth()/2, getHeight());
-                       for(int i = 0; i < data.length; i++) {
-                               double ang = HOKUYO_INDEX_TO_RAD(i);
-                       if((ang<(-HOKUYO_RANGE_ANGLE_LEFT/180.0*Math.PI))||((ang>(HOKUYO_RANGE_ANGLE_RIGHT/180.0*Math.PI)))) {
-                       continue;
-                       }
-       
-                       data[i] /= 10;
-                   int x = (int)(getWidth()/2) - (int)(data[i] * Math.sin(HOKUYO_INDEX_TO_RAD(i)));
-                   int y = getHeight() -(int)(data[i] * Math.cos(HOKUYO_INDEX_TO_RAD(i)));
-                               path.lineTo(x, y);
+               rcLock.lock();
+               try {
+                       if (isRunning) {
+                               lock.lock();
+                               try {
+                                       if (!hasBeenDrawn) {
+                                               path.reset();
+                                               path.moveTo(getWidth()/2, getHeight());
+                                               for(int i = 0; i < data.length; i++) {
+                                                       double ang = HOKUYO_INDEX_TO_RAD(i);
+                                               if((ang<(-HOKUYO_RANGE_ANGLE_LEFT/180.0*Math.PI))||((ang>(HOKUYO_RANGE_ANGLE_RIGHT/180.0*Math.PI)))) {
+                                               continue;
+                                               }
+                               
+                                               data[i] /= 10;
+                                           int x = (int)(getWidth()/2) - (int)(data[i] * Math.sin(HOKUYO_INDEX_TO_RAD(i)));
+                                           int y = getHeight() -(int)(data[i] * Math.cos(HOKUYO_INDEX_TO_RAD(i)));
+                                                       path.lineTo(x, y);
+                                               }
+                                               path.close();
+                                               hasBeenDrawn = true;
+                                       }
+                               }
+                               finally {
+                                       lock.unlock();
+                               }
+                       }
+                       else {
+                               path.reset();
                        }
-                       path.close();
+                       canvas.drawPath(path, paint);
+               } finally {
+                       rcLock.unlock();
                }
-               canvas.drawPath(path, paint);
-               hasBeenDrawn = true;
        }
        
        public void run(boolean run) {
-               isRunning = run;
+               wcLock.lock();
+               try {
+                       isRunning = run;
+               } finally {
+                       wcLock.unlock();
+               }
        }
        
        public void setData(int[] data) {
-               if (hasBeenDrawn) {
-                       this.data = data.clone();
-                       hasBeenDrawn = false;
+               if (lock.tryLock()) {
+                       try {
+                               this.data = data.clone();
+                               hasBeenDrawn = false;
+                       }
+                       finally {
+                               lock.unlock();
+                       }
                        postInvalidate();
                }
        }

diff --git a/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedPublish.java b/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedPublish.java
index af04f65aed54266aceae15561aa4451a39580875..dbe102131f54b01a0b4a3d448194e1217e530307 100644 (file)
--- a/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedPublish.java
+++ b/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedPublish.java
@@ -1,7 +1,11 @@
 package org.ocera.orte.demo;
 
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.concurrent.locks.ReentrantReadWriteLock;
+import java.util.concurrent.locks.ReentrantReadWriteLock.ReadLock;
+import java.util.concurrent.locks.ReentrantReadWriteLock.WriteLock;
+
 import org.ocera.orte.DomainApp;
-import org.ocera.orte.JOrte;
 import org.ocera.orte.Publication;
 import org.ocera.orte.types.NtpTime;
 import org.ocera.orte.types.PublProp;
@@ -12,8 +16,7 @@ public class MotionSpeedPublish implements Runnable {
        public static final double r = 0.15;
        
        private short[] speed = new short[2];
-       private boolean isCancelled = false;
-       private boolean isSent = true;
+       private boolean isCancelled = true;
        private float maxRange;
        private float[] accelData = new float[2];
        private SpeedMotionType speedmsg;
@@ -22,6 +25,11 @@ public class MotionSpeedPublish implements Runnable {
        private PublProp publProp;
        private DomainApp appDomain;
        
+       private final ReentrantLock dataLock = new ReentrantLock();
+       private final ReentrantReadWriteLock controlRrwl = new ReentrantReadWriteLock(true);
+       private final ReadLock rcLock = controlRrwl.readLock();
+       private final WriteLock wcLock = controlRrwl.writeLock();
+       
        public MotionSpeedPublish(float maxRange, DomainApp appDomain) {
                this.maxRange = maxRange;
                this.appDomain = appDomain;
@@ -35,25 +43,16 @@ public class MotionSpeedPublish implements Runnable {
                                                                         "motion_speed",                                
                                                                         persistence,
                                                                         strength);
-           
-
-       }
-       
-       public void start() {
-               isCancelled = false;
-           pub = appDomain.createPublication(publProp, speedmsg);
-           thread = new Thread(this);
-           thread.start();
        }
        
        private void calculateSpeed(float[] mAccel) {
 
-               double angle = 0, lenght, v, omega; 
+               double angle = 0, length, v, omega;
 
                v = (double)mAccel[1]/maxRange;
                omega = (double)mAccel[0]/maxRange;
-               lenght = Math.sqrt(Math.pow(v,2) + Math.pow(omega,2));
-               if (lenght >= 1) {
+               length = Math.sqrt(Math.pow(v,2) + Math.pow(omega,2));
+               if (length >= 1) {
                        angle = Math.atan2(mAccel[1], mAccel[0]);       
                        v = Math.sin(angle);
                        omega = Math.cos(angle);
@@ -64,37 +63,84 @@ public class MotionSpeedPublish implements Runnable {
                speed[1] = (short)(-(v - (v>0 ? -r*omega : r*omega))*VMAX);
        }
        
+       public void start() {
+               wcLock.lock();
+               try {
+                       isCancelled = false;
+                   pub = appDomain.createPublication(publProp, speedmsg);
+                   thread = new Thread(this);
+                   thread.start();
+               }
+               finally {
+                       wcLock.unlock();
+               }
+       }
+       
        public void cancel() {
-               isCancelled = true;
+               wcLock.lock();
+               try {
+                       isCancelled = true;
+                       rcLock.lock();
+               } finally {
+                       wcLock.unlock();
+               }
                try {
                        thread.join();
+                   pub.destroy();
                } catch (InterruptedException e) {
                        e.printStackTrace();
+               } finally {
+                       rcLock.unlock();
                }
-           pub.destroy();
        }
        
        public boolean isCancelled() {
-               return isCancelled;
+               rcLock.lock();
+               try {
+                       return isCancelled;
+               } finally {
+                       rcLock.unlock();
+               }
        }
        
        @Override
        public void run() {
-           while(!isCancelled) {
-             calculateSpeed(accelData.clone());
-             speedmsg.speed = this.speed.clone();  
-             pub.send(speedmsg);
-             isSent = true;
-         JOrte.sleepMs(100);
+               while(true) {
+                       rcLock.lock();
+                       try {
+                           if (!isCancelled) {
+                             dataLock.lock();
+                             try {
+                                 calculateSpeed(accelData);
+                             }
+                             finally {
+                                 dataLock.unlock();
+                             }
+                         speedmsg.speed = this.speed;  
+                             pub.send(speedmsg);
+                               }
+                           else
+                               break;
+                       } finally {
+                               rcLock.unlock();
+                       }
+                       try {
+                               Thread.sleep(100);
+                       } catch (InterruptedException e) {
+                               e.printStackTrace();
+                       }
                }
        }
        
        public void setSpeed(float accelX, float accelY) {
-               if(isSent) {
-                       isSent = false;
-                       this.accelData[0] = accelX;
-                       this.accelData[1] = accelY;
+               if (dataLock.tryLock()) {
+                       try {
+                               this.accelData[0] = accelX;
+                               this.accelData[1] = accelY;
+                       }
+                       finally {
+                               dataLock.unlock();
+                       }
                }
        }
-       
 }