From ddc356ef6788e1238435ad8303517a2fbb631b6f Mon Sep 17 00:00:00 2001 From: Martin Vajnar Date: Wed, 21 Aug 2013 23:03:31 +0200 Subject: [PATCH] ROBOT_DEMO: add motion speed subscriber Add motion speed monitor. --- .../Robot_Demo/gen/org/ocera/orte/demo/R.java | 9 +- orte/Robot_Demo/res/menu/activity_main.xml | 22 +++-- .../src/org/ocera/orte/demo/HokuyoView.java | 70 ++++++++++++++++ .../src/org/ocera/orte/demo/MainActivity.java | 69 +++++++++++----- .../ocera/orte/demo/MotionSpeedSubscribe.java | 82 +++++++++++++++++++ .../org/ocera/orte/demo/SpeedMotionType.java | 6 +- 6 files changed, 223 insertions(+), 35 deletions(-) create mode 100644 orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedSubscribe.java diff --git a/orte/Robot_Demo/gen/org/ocera/orte/demo/R.java b/orte/Robot_Demo/gen/org/ocera/orte/demo/R.java index b0bd395..db9b42a 100644 --- a/orte/Robot_Demo/gen/org/ocera/orte/demo/R.java +++ b/orte/Robot_Demo/gen/org/ocera/orte/demo/R.java @@ -14,12 +14,13 @@ public final class R { public static final int ic_launcher=0x7f020000; } public static final class id { - public static final int exit=0x7f070005; - public static final int hokuyo=0x7f070002; + public static final int exit=0x7f070006; + public static final int hokuyo=0x7f070003; public static final int hokuyo_view=0x7f070000; - public static final int lift=0x7f070003; - public static final int manget=0x7f070004; + public static final int lift=0x7f070004; + public static final int magnet=0x7f070005; public static final int speed=0x7f070001; + public static final int speed_mon=0x7f070002; } public static final class layout { public static final int hokuyo_view=0x7f030000; diff --git a/orte/Robot_Demo/res/menu/activity_main.xml b/orte/Robot_Demo/res/menu/activity_main.xml index 307b1dd..d111b50 100644 --- a/orte/Robot_Demo/res/menu/activity_main.xml +++ b/orte/Robot_Demo/res/menu/activity_main.xml @@ -1,31 +1,37 @@ - - + + + - \ No newline at end of file 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 8ada9ae..185805e 100644 --- a/orte/Robot_Demo/src/org/ocera/orte/demo/HokuyoView.java +++ b/orte/Robot_Demo/src/org/ocera/orte/demo/HokuyoView.java @@ -26,12 +26,16 @@ public class HokuyoView extends View { public static final double COSINUS = Math.cos((90-HOKUYO_RANGE_ANGLE_LEFT)/180.0*Math.PI); private int[] data = new int[681]; + private double[] speedCo = new double[2]; private Paint paint = new Paint(); private Path path = new Path(); private boolean isRunning = false; + private boolean isMonitoring = false; private boolean hasBeenDrawn = true; + private final ReentrantLock lock = new ReentrantLock(); + private final ReentrantLock lockMotion = new ReentrantLock(); private final ReentrantReadWriteLock controlRrwl = new ReentrantReadWriteLock(true); private final ReadLock rcLock = controlRrwl.readLock(); private final WriteLock wcLock = controlRrwl.writeLock(); @@ -84,11 +88,55 @@ public class HokuyoView extends View { path.reset(); } canvas.drawPath(path, paint); + + if (isMonitoring) { + lockMotion.lock(); + try { + double norm; + if (getHeight() < getWidth()) + norm = getHeight()*0.125; + else + norm = getWidth()*0.125; + paint.setStrokeWidth(1); + canvas.drawLine((int)(10), + (int)(10+norm*1.5), + (int)(10+norm*3), + (int)(10+norm*1.5), + paint); + canvas.drawLine((int)(10+norm*1.5), + (int)(10), + (int)(10+norm*1.5), + (int)(10+norm*3), + paint); + paint.setStrokeWidth(4); + canvas.drawLine((int)(10+norm*1.5), + (int)(10+norm*1.5), + (int)(speedCo[0]*norm+10+norm*1.5), + (int)(speedCo[1]*norm+10+norm*1.5), + paint); + } finally { + lockMotion.unlock(); + } + } } finally { rcLock.unlock(); } } + private void calculateCoordinates(short[] speed) { + double x, y; + double temp[] = new double[2]; + + temp[0] = (double)speed[0]/16000; + temp[1] = (double)speed[1]/16000; + + y = (temp[0]+temp[1])/2; + x = (temp[0]-y)/0.30; + + speedCo[1] = -y; + speedCo[0] = speedCo[1]>0 ? -x : x; + } + public void run(boolean run) { wcLock.lock(); try { @@ -98,6 +146,15 @@ public class HokuyoView extends View { } } + public void runMotion(boolean run) { + wcLock.lock(); + try { + isMonitoring = run; + } finally { + wcLock.unlock(); + } + } + public void setData(int[] data) { if (lock.tryLock()) { try { @@ -111,6 +168,19 @@ public class HokuyoView extends View { } } + public void setDataMotion(short[] speed) { + if (lockMotion.tryLock()) { + try { + short[] speedCl = speed.clone(); + calculateCoordinates(speedCl); + } + finally { + lockMotion.unlock(); + } + postInvalidate(); + } + } + public static double HOKUYO_INDEX_TO_DEG(int index) { return ((HOKUYO_START_ANGLE-index*360.0/HOKUYO_SPLIT_DIVISION) * HOKUYO_ORIENTATION); } diff --git a/orte/Robot_Demo/src/org/ocera/orte/demo/MainActivity.java b/orte/Robot_Demo/src/org/ocera/orte/demo/MainActivity.java index 0b23132..d05df26 100644 --- a/orte/Robot_Demo/src/org/ocera/orte/demo/MainActivity.java +++ b/orte/Robot_Demo/src/org/ocera/orte/demo/MainActivity.java @@ -19,8 +19,9 @@ import android.view.MenuItem; public class MainActivity extends Activity { private Manager manager = null; - private String[] mgrs = {"192.168.1.5","192.168.1.8","10.1.1.1"}; - private MotionSpeedPublish motion_speed = null; + private String[] mgrs = {"192.168.1.5","192.168.1.8","192.168.1.29","10.1.1.1"}; + private MotionSpeedPublish motion_speed_publ = null; + private MotionSpeedSubscribe motion_speed_subs = null; private HokuyoScanSubscribe hokuyo_scan = null; private SensorManager mSensorManager = null; private Sensor mGravity = null; @@ -29,7 +30,8 @@ public class MainActivity extends Activity { private WakeLock mWakeLock = null; private DomainApp appDomain = null; private HokuyoView hokuyo_view = null; - private MenuItem speed_item = null; + private MenuItem speed_publ_item = null; + private MenuItem speed_subs_item = null; private MenuItem hokuyo_item = null; static { @@ -47,18 +49,25 @@ public class MainActivity extends Activity { protected void onPause() { super.onPause(); - if (motion_speed != null && !motion_speed.isCancelled()) { - motion_speed.cancel(); + if (motion_speed_publ != null && !motion_speed_publ.isCancelled()) { + motion_speed_publ.cancel(); mSensorManager.unregisterListener(accel); this.setRequestedOrientation(ActivityInfo.SCREEN_ORIENTATION_SENSOR); - speed_item.setTitle("Start speed"); + speed_publ_item.setTitle("Start motion control"); + } + + if (motion_speed_subs != null && !motion_speed_subs.isCancelled()) { + hokuyo_view.runMotion(false); + motion_speed_subs.cancel(); + hokuyo_view.invalidate(); + speed_subs_item.setTitle("Start motion monitor"); } if (hokuyo_scan != null && !hokuyo_scan.isCancelled()) { hokuyo_view.run(false); hokuyo_scan.cancel(); hokuyo_view.invalidate(); - hokuyo_item.setTitle("Start hokuyo"); + hokuyo_item.setTitle("Start LRF"); } mWakeLock.release(); @@ -108,35 +117,51 @@ public class MainActivity extends Activity { public boolean onOptionsItemSelected (MenuItem item) { System.out.println(item.getTitle()); - if(item.getTitle().equals("Start speed")) { + if(item.getTitle().equals("Start motion control")) { accel = new HandleAccelerometer(); mSensorManager.registerListener(accel, mGravity, SensorManager.SENSOR_DELAY_GAME); - if (motion_speed == null) - motion_speed = new MotionSpeedPublish(mGravity.getMaximumRange(),appDomain); - motion_speed.start(); - speed_item = item; + if (motion_speed_publ == null) + motion_speed_publ = new MotionSpeedPublish(mGravity.getMaximumRange(),appDomain); + motion_speed_publ.start(); + speed_publ_item = item; this.setRequestedOrientation(ActivityInfo.SCREEN_ORIENTATION_PORTRAIT); - item.setTitle("Stop speed"); + item.setTitle("Stop motion control"); } - else if (item.getTitle().equals("Stop speed")) { + else if (item.getTitle().equals("Stop motion control")) { mSensorManager.unregisterListener(accel); - motion_speed.cancel(); + motion_speed_publ.cancel(); this.setRequestedOrientation(ActivityInfo.SCREEN_ORIENTATION_SENSOR); - item.setTitle("Start speed"); + item.setTitle("Start motion control"); + } + else if (item.getTitle().equals("Start motion monitor")) { + if (motion_speed_subs == null) + motion_speed_subs = new MotionSpeedSubscribe(appDomain, hokuyo_view); + motion_speed_subs.start(); + hokuyo_view.runMotion(true); + hokuyo_view.invalidate(); + speed_subs_item = item; + item.setTitle("Stop motion monitor"); } - else if (item.getTitle().equals("Start hokuyo")) { + else if (item.getTitle().equals("Stop motion monitor")) { + hokuyo_view.runMotion(false); + motion_speed_subs.cancel(); + hokuyo_view.invalidate(); + item.setTitle("Start motion monitor"); + } + else if (item.getTitle().equals("Start LRF")) { if (hokuyo_scan == null) hokuyo_scan = new HokuyoScanSubscribe(appDomain, hokuyo_view); hokuyo_scan.start(); hokuyo_view.run(true); + hokuyo_view.invalidate(); hokuyo_item = item; - item.setTitle("Stop hokuyo"); + item.setTitle("Stop LRF"); } - else if (item.getTitle().equals("Stop hokuyo")) { + else if (item.getTitle().equals("Stop LRF")) { hokuyo_view.run(false); hokuyo_scan.cancel(); hokuyo_view.invalidate(); - item.setTitle("Start hokuyo"); + item.setTitle("Start LRF"); } else if (item.getTitle().equals("Lift up")) { @@ -165,8 +190,8 @@ public class MainActivity extends Activity { @Override public void onSensorChanged(SensorEvent event) { if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) { - if (motion_speed != null) - motion_speed.setSpeed(event.values[0], event.values[1]); + if (motion_speed_publ != null) + motion_speed_publ.setSpeed(event.values[0], event.values[1]); } } } diff --git a/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedSubscribe.java b/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedSubscribe.java new file mode 100644 index 0000000..524e954 --- /dev/null +++ b/orte/Robot_Demo/src/org/ocera/orte/demo/MotionSpeedSubscribe.java @@ -0,0 +1,82 @@ +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; +import org.ocera.orte.types.MessageData; +import org.ocera.orte.types.NtpTime; +import org.ocera.orte.types.RecvInfo; +import org.ocera.orte.types.SubsProp; + +public class MotionSpeedSubscribe extends SubscriptionCallback{ + + public final static int IMMEDIATE = 0x02; + public final static int BEST_EFFORTS = 0x01; + + private Subscription sub; + private HokuyoView view; + private SpeedMotionType speedmsg; + 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 MotionSpeedSubscribe(DomainApp domainApp, HokuyoView view) { + this.view = view; + this.domainApp = domainApp; + + NtpTime deadline = new NtpTime(0,1288490189); + NtpTime minSeparation = new NtpTime(0); + + speedmsg = new SpeedMotionType(domainApp, "motion_speed"); + + subProps = new SubsProp(speedmsg.getTopic(), + "motion_speed", + minSeparation, + deadline, + IMMEDIATE, + BEST_EFFORTS, + 0); + } + + public void start() { + wcLock.lock(); + try { + isCancelled = false; + sub = domainApp.createSubscription(subProps, speedmsg, this); + } finally { + wcLock.unlock(); + } + } + + public void cancel() { + wcLock.lock(); + try { + isCancelled = true; + sub.destroy(); + } finally { + wcLock.unlock(); + } + } + + public boolean isCancelled() { + rcLock.lock(); + try { + return isCancelled; + } finally { + rcLock.unlock(); + } + } + + public void callback(RecvInfo info, MessageData msg) { + view.setDataMotion(((SpeedMotionType)msg).speed); + //System.out.println(msg); + } +} \ No newline at end of file diff --git a/orte/Robot_Demo/src/org/ocera/orte/demo/SpeedMotionType.java b/orte/Robot_Demo/src/org/ocera/orte/demo/SpeedMotionType.java index de9f8e6..184280f 100644 --- a/orte/Robot_Demo/src/org/ocera/orte/demo/SpeedMotionType.java +++ b/orte/Robot_Demo/src/org/ocera/orte/demo/SpeedMotionType.java @@ -25,7 +25,11 @@ public class SpeedMotionType extends MessageData } @Override - public void read() {} + public void read() { + buffer.rewind(); + this.speed[0] = buffer.getShort(); + this.speed[1] = buffer.getShort(); + } @Override public int getMaxDataLength() -- 2.39.2