From: Martin Vajnar Date: Sat, 24 Aug 2013 09:35:23 +0000 (+0200) Subject: ROBOT_DEMO: add publishers for "crane_cmd" and "magnet_cmd" X-Git-Url: http://rtime.felk.cvut.cz/gitweb/orte/eurobot.git/commitdiff_plain/fedecd00ea165c239ab8770c3d66410e86146bf4 ROBOT_DEMO: add publishers for "crane_cmd" and "magnet_cmd" --- diff --git a/orte/Robot_Demo/res/values/strings.xml b/orte/Robot_Demo/res/values/strings.xml index e495233..aacc9dd 100644 --- a/orte/Robot_Demo/res/values/strings.xml +++ b/orte/Robot_Demo/res/values/strings.xml @@ -1,6 +1,6 @@ - ORTE Demo + Robomon \ No newline at end of file diff --git a/orte/Robot_Demo/src/org/ocera/orte/demo/CraneCmdPublish.java b/orte/Robot_Demo/src/org/ocera/orte/demo/CraneCmdPublish.java new file mode 100644 index 0000000..d75d6ac --- /dev/null +++ b/orte/Robot_Demo/src/org/ocera/orte/demo/CraneCmdPublish.java @@ -0,0 +1,72 @@ +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.Publication; +import org.ocera.orte.types.NtpTime; +import org.ocera.orte.types.PublProp; + +public class CraneCmdPublish { + + private boolean isCancelled = true; + private CraneCmdType liftmsg; + private Publication pub; + private PublProp publProp; + private DomainApp appDomain; + + private final ReentrantReadWriteLock controlRrwl = new ReentrantReadWriteLock(true); + private final ReadLock rcLock = controlRrwl.readLock(); + private final WriteLock wcLock = controlRrwl.writeLock(); + + public CraneCmdPublish(DomainApp appDomain) { + this.appDomain = appDomain; + + NtpTime persistence = new NtpTime(3); + int strength = 100; + + liftmsg = new CraneCmdType(appDomain,"crane_cmd"); + + publProp = new PublProp(liftmsg.getTopic(), + "crane_cmd", + persistence, + strength); + } + + public void start() { + wcLock.lock(); + try { + isCancelled = false; + pub = appDomain.createPublication(publProp, liftmsg); + } + finally { + wcLock.unlock(); + } + } + + public void cancel() { + wcLock.lock(); + try { + isCancelled = true; + pub.destroy(); + } finally { + wcLock.unlock(); + } + } + + public boolean isCancelled() { + rcLock.lock(); + try { + return isCancelled; + } finally { + rcLock.unlock(); + } + } + + public void send(short magnet) { + liftmsg.position = magnet; + pub.send(liftmsg); + } +} diff --git a/orte/Robot_Demo/src/org/ocera/orte/demo/CraneCmdType.java b/orte/Robot_Demo/src/org/ocera/orte/demo/CraneCmdType.java new file mode 100644 index 0000000..154755a --- /dev/null +++ b/orte/Robot_Demo/src/org/ocera/orte/demo/CraneCmdType.java @@ -0,0 +1,35 @@ +package org.ocera.orte.demo; + +import org.ocera.orte.DomainApp; +import org.ocera.orte.types.MessageData; +import org.ocera.orte.types.ORTEConstant; + +public class CraneCmdType extends MessageData { + + public int position = 0; + + public CraneCmdType(DomainApp domainApp, String newTopic) { + super(); + this.setTopic(newTopic); + if (!domainApp.regNewDataType("crane_cmd",getMaxDataLength())) { + System.out.println(":j!: cannot register data type \"lift_cmd\"!"); + } + } + + @Override + public void read() { + } + + @Override + public void write() { + buffer.rewind(); + buffer.putShort((short) position); + buffer.putShort((short) 0); + } + + @Override + public int getMaxDataLength() { + return 2*ORTEConstant.SHORT_FIELD_SIZE; + } + +} diff --git a/orte/Robot_Demo/src/org/ocera/orte/demo/MagnetCmdPublish.java b/orte/Robot_Demo/src/org/ocera/orte/demo/MagnetCmdPublish.java new file mode 100644 index 0000000..f56d42f --- /dev/null +++ b/orte/Robot_Demo/src/org/ocera/orte/demo/MagnetCmdPublish.java @@ -0,0 +1,72 @@ +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.Publication; +import org.ocera.orte.types.NtpTime; +import org.ocera.orte.types.PublProp; + +public class MagnetCmdPublish { + + private boolean isCancelled = true; + private MagnetCmdType magnetmsg; + private Publication pub; + private PublProp publProp; + private DomainApp appDomain; + + private final ReentrantReadWriteLock controlRrwl = new ReentrantReadWriteLock(true); + private final ReadLock rcLock = controlRrwl.readLock(); + private final WriteLock wcLock = controlRrwl.writeLock(); + + public MagnetCmdPublish(DomainApp appDomain) { + this.appDomain = appDomain; + + NtpTime persistence = new NtpTime(3); + int strength = 100; + + magnetmsg = new MagnetCmdType(appDomain,"magnet_cmd"); + + publProp = new PublProp(magnetmsg.getTopic(), + "magnet_cmd", + persistence, + strength); + } + + public void start() { + wcLock.lock(); + try { + isCancelled = false; + pub = appDomain.createPublication(publProp, magnetmsg); + } + finally { + wcLock.unlock(); + } + } + + public void cancel() { + wcLock.lock(); + try { + isCancelled = true; + pub.destroy(); + } finally { + wcLock.unlock(); + } + } + + public boolean isCancelled() { + rcLock.lock(); + try { + return isCancelled; + } finally { + rcLock.unlock(); + } + } + + public void send(short magnet) { + magnetmsg.magnet = magnet; + pub.send(magnetmsg); + } +} diff --git a/orte/Robot_Demo/src/org/ocera/orte/demo/MagnetCmdType.java b/orte/Robot_Demo/src/org/ocera/orte/demo/MagnetCmdType.java new file mode 100644 index 0000000..3f4df5d --- /dev/null +++ b/orte/Robot_Demo/src/org/ocera/orte/demo/MagnetCmdType.java @@ -0,0 +1,34 @@ +package org.ocera.orte.demo; + +import org.ocera.orte.DomainApp; +import org.ocera.orte.types.MessageData; +import org.ocera.orte.types.ORTEConstant; + +public class MagnetCmdType extends MessageData { + + public int magnet = 0; + + public MagnetCmdType(DomainApp domainApp, String newTopic) { + super(); + this.setTopic(newTopic); + if (!domainApp.regNewDataType("magnet_cmd",getMaxDataLength())) { + System.out.println(":j!: cannot register data type \"magnet_cmd\"!"); + } + } + + @Override + public void read() { + } + + @Override + public void write() { + buffer.rewind(); + buffer.putShort((short) magnet); + } + + @Override + public int getMaxDataLength() { + return ORTEConstant.SHORT_FIELD_SIZE; + } + +} 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 36f38c0..b4f7f47 100644 --- a/orte/Robot_Demo/src/org/ocera/orte/demo/MainActivity.java +++ b/orte/Robot_Demo/src/org/ocera/orte/demo/MainActivity.java @@ -56,6 +56,8 @@ public class MainActivity extends Activity { private MotionSpeedSubscribe motion_speed_subs = null; private HokuyoScanSubscribe hokuyo_scan = null; private PwrVoltageSubscribe pwr_voltage = null; + private MagnetCmdPublish magnet_cmd = null; + private CraneCmdPublish crane_cmd = null; private SensorManager mSensorManager = null; private Sensor mGravity = null; private SensorEventListener accel = null; @@ -116,6 +118,14 @@ public class MainActivity extends Activity { protected void onDestroy() { super.onDestroy(); + if (crane_cmd != null && !crane_cmd.isCancelled()) { + crane_cmd.cancel(); + } + + if (magnet_cmd != null && !crane_cmd.isCancelled()) { + magnet_cmd.cancel(); + } + if (appDomain != null) { appDomain.destroy(); appDomain = null; @@ -147,7 +157,7 @@ public class MainActivity extends Activity { voltage50 = (EditText)voltageDialog.findViewById(R.id.editText2); voltage80 = (EditText)voltageDialog.findViewById(R.id.editText3); voltageBAT = (EditText)voltageDialog.findViewById(R.id.editText4); - + mSensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE); mGravity = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER); @@ -159,6 +169,10 @@ public class MainActivity extends Activity { manager = new Manager(mgrs); appDomain = new DomainApp(); + magnet_cmd = new MagnetCmdPublish(appDomain); + magnet_cmd.start(); + crane_cmd = new CraneCmdPublish(appDomain); + crane_cmd.start(); } @Override @@ -219,16 +233,20 @@ public class MainActivity extends Activity { item.setTitle("Start LRF"); } else if (item.getTitle().equals("Lift up")) { - + crane_cmd.send((short)0x100); + item.setTitle("Lift down"); } else if (item.getTitle().equals("Lift down")) { - + crane_cmd.send((short)0x190); + item.setTitle("Lift up"); } else if (item.getTitle().equals("Magnet on")) { - + magnet_cmd.send((short)1); + item.setTitle("Magnet off"); } else if (item.getTitle().equals("Magnet off")) { - + magnet_cmd.send((short)0); + item.setTitle("Magnet on"); } else if (item.getTitle().equals("Voltage monitor")) { if (pwr_voltage == null)