Add motion speed monitor.
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;
<menu xmlns:android="http://schemas.android.com/apk/res/android" >
<item android:id="@+id/speed"
- android:title="Start speed"
+ android:title="Start motion control"
android:showAsAction="never"
android:orderInCategory="1">
</item>
-
- <item android:id="@+id/hokuyo"
- android:title="Start hokuyo"
+
+ <item android:id="@+id/speed_mon"
+ android:title="Start motion monitor"
android:showAsAction="never"
android:orderInCategory="2">
</item>
+
+ <item android:id="@+id/hokuyo"
+ android:title="Start LRF"
+ android:showAsAction="never"
+ android:orderInCategory="3">
+ </item>
<item android:id="@+id/lift"
android:title="Lift up"
- android:orderInCategory="3"
+ android:orderInCategory="4"
android:showAsAction="never">
</item>
- <item android:id="@+id/manget"
+ <item android:id="@+id/magnet"
android:title="Magnet on"
- android:orderInCategory="4"
+ android:orderInCategory="5"
android:showAsAction="never">
</item>
<item android:id="@+id/exit"
android:title="Exit"
- android:orderInCategory="5"
+ android:orderInCategory="6"
android:showAsAction="never">
</item>
</menu>
\ No newline at end of file
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();
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 {
}
}
+ public void runMotion(boolean run) {
+ wcLock.lock();
+ try {
+ isMonitoring = run;
+ } finally {
+ wcLock.unlock();
+ }
+ }
+
public void setData(int[] data) {
if (lock.tryLock()) {
try {
}
}
+ 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);
}
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;
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 {
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();
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")) {
@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]);
}
}
}
--- /dev/null
+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
}
@Override
- public void read() {}
+ public void read() {
+ buffer.rewind();
+ this.speed[0] = buffer.getShort();
+ this.speed[1] = buffer.getShort();
+ }
@Override
public int getMaxDataLength()
projects / orte.git / commitdiff