addign a disconnect pass through for sub mobile bases

Author: Kevin Harrington

build.gradle

@@ -44,7 +44,7 @@ repositories {
 dependencies {
 	//TODO change as many of these as possible to Maven repositories
     compile fileTree (dir: 'libs', includes: ['*.jar'])
-	testCompile 'junit:junit:4.10'
+	testCompile 'junit:junit:4.12'
 	compile 'gov.nist.math:jama:1.0.2'
 	compile 'com.miglayout:miglayout-swing:4.1'
 	compile 'org.igniterealtime.smack:smack:3.2.1'

src/main/java/com/neuronrobotics/sdk/addons/kinematics/AbstractKinematicsNR.java

@@ -319,7 +319,7 @@ public void onConnect(BowlerAbstractDevice source) {
 					&& linkNode.getNodeName().contentEquals("ZframeToRAS")) {
 				Element eElement = (Element) linkNode;
 				try {
-					setZframeToGlobalTransform(new TransformNR(
+					setGlobalToFiducialTransform(new TransformNR(
 							Double.parseDouble(XmlFactory.getTagValue("x", eElement)),
 							Double.parseDouble(XmlFactory.getTagValue("y", eElement)),
 							Double.parseDouble(XmlFactory.getTagValue("z", eElement)),
@@ -329,13 +329,13 @@ public void onConnect(BowlerAbstractDevice source) {
 									Double.parseDouble(XmlFactory.getTagValue("rotz", eElement)) })));
 				} catch (Exception ex) {
 					ex.printStackTrace();
-					setZframeToGlobalTransform(new TransformNR());
+					setGlobalToFiducialTransform(new TransformNR());
 				}
 			} else if (linkNode.getNodeType() == Node.ELEMENT_NODE
 					&& linkNode.getNodeName().contentEquals("baseToZframe")) {
 				Element eElement = (Element) linkNode;
 				try {
-					setBaseToZframeTransform(new TransformNR(Double.parseDouble(XmlFactory.getTagValue("x", eElement)),
+					setRobotToFiducialTransform(new TransformNR(Double.parseDouble(XmlFactory.getTagValue("x", eElement)),
 							Double.parseDouble(XmlFactory.getTagValue("y", eElement)),
 							Double.parseDouble(XmlFactory.getTagValue("z", eElement)),
 							new RotationNR(new double[] { Double.parseDouble(XmlFactory.getTagValue("rotw", eElement)),
@@ -344,7 +344,7 @@ public void onConnect(BowlerAbstractDevice source) {
 									Double.parseDouble(XmlFactory.getTagValue("rotz", eElement)) })));
 				} catch (Exception ex) {
 					ex.printStackTrace();
-					setBaseToZframeTransform(new TransformNR());
+					setRobotToFiducialTransform(new TransformNR());
 				}
 			} else {
 				// System.err.println(linkNode.getNodeName());
@@ -902,15 +902,15 @@ public TransformNR getFiducialToGlobalTransform() {
 		return fiducial2RAS;
 	}
 
-	/**
-	 * Sets the base to zframe transform.
-	 *
-	 * @param baseToFiducial the new base to zframe transform
-	 */
-	@Deprecated
-	public void setBaseToZframeTransform(TransformNR baseToFiducial) {
-		setRobotToFiducialTransform(baseToFiducial);
-	}
+//	/**
+//	 * Sets the base to zframe transform.
+//	 *
+//	 * @param baseToFiducial the new base to zframe transform
+//	 */
+//	@Deprecated
+//	public void setBaseToZframeTransform(TransformNR baseToFiducial) {
+//		setRobotToFiducialTransform(baseToFiducial);
+//	}
 	public void setRobotToFiducialTransform(TransformNR baseToFiducial) {
 		if (baseToFiducial == null) {
 			Log.error("Fiducial can not be null " + baseToFiducial);
@@ -925,15 +925,15 @@ public void setRobotToFiducialTransform(TransformNR baseToFiducial) {
 
 		runRenderWrangler();
 	}
-	/**
-	 * Sets the zframe to global transform.
-	 *
-	 * @param fiducialToRAS the new zframe to global transform
-	 */
-	@Deprecated
-	private void setZframeToGlobalTransform(TransformNR fiducialToRAS) {
-		setGlobalToFiducialTransform(fiducialToRAS);
-	}
+//	/**
+//	 * Sets the zframe to global transform.
+//	 *
+//	 * @param fiducialToRAS the new zframe to global transform
+//	 */
+//	@Deprecated
+//	private void setZframeToGlobalTransform(TransformNR fiducialToRAS) {
+//		setGlobalToFiducialTransform(fiducialToRAS);
+//	}
 
 	/**
 	 * Gets the robot to fiducial transform.

src/main/java/com/neuronrobotics/sdk/addons/kinematics/DHParameterKinematics.java

@@ -68,6 +68,23 @@ public DHParameterKinematics(BowlerAbstractDevice bad, Element linkStream) {
 				getFactory().getDyio(lf).addConnectionEventListener(l);
 				return;
 			}
+		addConnectionEventListener(new IDeviceConnectionEventListener() {
+			
+			@Override
+			public void onDisconnect(BowlerAbstractDevice source) {
+				for(int i=0;i<getNumberOfLinks();i++) {
+					MobileBase m = getSlaveMobileBase(i);
+					if(m!=null)
+						m.disconnect();
+				}
+			}
+			
+			@Override
+			public void onConnect(BowlerAbstractDevice source) {
+				// TODO Auto-generated method stub
+				
+			}
+		});
 	}
 
 	/**
@@ -775,7 +792,7 @@ public Object getListener(int i) {
 	 */
 	@Override
 	public void setRobotToFiducialTransform(TransformNR newTrans) {
-		super.setBaseToZframeTransform(newTrans);
+		super.setRobotToFiducialTransform(newTrans);
 	}
 
 	public void refreshPose() {

src/main/java/com/neuronrobotics/sdk/addons/kinematics/MobileBase.java

@@ -613,7 +613,6 @@ public String getXml() {
 	 */
 	public String getEmbedableXml() {
 		TransformNR location = getFiducialToGlobalTransform();
-		setGlobalToFiducialTransform(new TransformNR());
 
 		String allVitamins = "";
 		for (String key : getVitamins().keySet()) {
@@ -871,8 +870,8 @@ public void setGlobalToFiducialTransform(TransformNR frameToBase) {
 	 * @param frameToBase the new global to fiducial transform
 	 */
 	@Override	
-	public void setBaseToZframeTransform(TransformNR baseToFiducial)  {
-		super.setBaseToZframeTransform( baseToFiducial);
+	public void setRobotToFiducialTransform(TransformNR baseToFiducial)  {
+		super.setRobotToFiducialTransform( baseToFiducial);
 		fireBaseUpdates();
 	}
 
@@ -910,16 +909,12 @@ public boolean connect() {
 		addRegistrationListener(new IRegistrationListenerNR() {
 			@Override
 			public void onFiducialToGlobalUpdate(AbstractKinematicsNR source, TransformNR regestration) {
-				for (DHParameterKinematics kin : getAllDHChains()) {
-					// Log.debug("Motion of mobile base event ");
-					// this represents motion of the mobile base
-					kin.setGlobalToFiducialTransform(regestration);
-				}
+				fireBaseUpdates();
 			}
 
 			@Override
 			public void onBaseToFiducialUpdate(AbstractKinematicsNR source, TransformNR regestration) {
-
+				fireBaseUpdates();
 			}
 		});
 		return isAvailable();