From 1b4c6e767458fbef097e1a9ae9b9ea6ff084692c Mon Sep 17 00:00:00 2001 From: Michael Elmore <135364586+MichaelElmore1@users.noreply.github.com> Date: Tue, 4 Jun 2024 15:59:33 -0500 Subject: [PATCH 1/2] Add support for creating a default orientation output if orientation is provided in the config --- .../impl/sensor/AbstractSensorModule.java | 60 +++++++++++++++++- .../impl/sensor/DefaultOrientationOutput.java | 56 +++++++++++++++++ .../sensor/DefaultOrientationOutputEuler.java | 63 +++++++++++++++++++ 3 files changed, 177 insertions(+), 2 deletions(-) create mode 100644 sensorhub-core/src/main/java/org/sensorhub/impl/sensor/DefaultOrientationOutput.java create mode 100644 sensorhub-core/src/main/java/org/sensorhub/impl/sensor/DefaultOrientationOutputEuler.java diff --git a/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/AbstractSensorModule.java b/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/AbstractSensorModule.java index 0c684fc77..92fcfc523 100644 --- a/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/AbstractSensorModule.java +++ b/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/AbstractSensorModule.java @@ -93,6 +93,8 @@ public abstract class AbstractSensorModule extends Abstr public static final String DEFAULT_XMLID_PREFIX = "SENSOR_"; protected static final String LOCATION_OUTPUT_ID = "SENSOR_LOCATION"; protected static final String LOCATION_OUTPUT_NAME = "sensorLocation"; + protected static final String ORIENTATION_OUTPUT_ID = "SENSOR_ORIENTATION"; + protected static final String ORIENTATION_OUTPUT_NAME = "sensorOrientation"; protected static final String UUID_URI_PREFIX = "urn:uuid:"; protected static final String STATE_UNIQUE_ID = "UniqueID"; @@ -105,6 +107,7 @@ public abstract class AbstractSensorModule extends Abstr protected ISystemGroupDriver parentSystem; protected DefaultLocationOutput locationOutput; + protected DefaultOrientationOutput orientationOutput; protected AbstractProcess sensorDescription = new PhysicalSystemImpl(); protected volatile long lastUpdatedSensorDescription = Long.MIN_VALUE; protected final Object sensorDescLock = new Object(); @@ -123,6 +126,7 @@ protected void beforeInit() throws SensorHubException this.uniqueID = null; this.xmlID = null; this.locationOutput = null; + this.orientationOutput = null; this.sensorDescription = new PhysicalSystemImpl(); removeAllOutputs(); removeAllControlInputs(); @@ -139,6 +143,7 @@ protected void beforeInit() throws SensorHubException * provided in the driver configuration *
  • If location is provided in config, a generic feature interest * and a location output
    • + *
  • If orientation is provided in config, a generic orientation output
    • * * In most cases, derived classes overriding this method must call it * using the super keyword. @@ -147,7 +152,7 @@ protected void beforeInit() throws SensorHubException protected void afterInit() throws SensorHubException { // generate random unique ID in case sensor driver hasn't generate one - // if a random UUID has already been generated it will be restored by + // if a random UUID has already been generated, it will be restored by // loadState() method that is called after init() if (this.uniqueID == null) { @@ -190,17 +195,45 @@ protected void afterInit() throws SensorHubException foiMap.put(sf.getUniqueIdentifier(), sf); } } + + // add orientation output and foi if an orientation is set in config + if (config.getOrientation() != null) + { + EulerOrientation loc = config.getOrientation(); + + if (orientationOutput == null) + addOrientationOutput(Double.NaN); + + if (foiMap.isEmpty()) + { + // add + SamplingPoint sf = new SamplingPoint(); + sf.setId("FOI_" + xmlID); + sf.setUniqueIdentifier(uniqueID + ":foi"); + if (config.name != null) + sf.setName(config.name); + sf.setDescription("Sampling point for " + config.name); + sf.setHostedProcedureUID(uniqueID); + Point point = new GMLFactory(true).newPoint(); + point.setSrsName(SWEConstants.REF_FRAME_4979); + point.setSrsDimension(3); + point.setPos(new double[] {loc.heading, loc.pitch, loc.roll}); + sf.setShape(point); + foiMap.put(sf.getUniqueIdentifier(), sf); + } + } super.afterInit(); } /** - * This methods does the following: + * This method does the following: * * In most cases, derived classes overriding this method must call it * using the super keyword. @@ -230,6 +263,11 @@ protected void beforeStart() throws SensorHubException var loc = config.getLocation(); if (locationOutput != null && loc != null) locationOutput.updateLocation(System.currentTimeMillis()/1000., loc.lon, loc.lat, loc.alt, false); + + // Send new orientation event + var orient = config.getOrientation(); + if (orientationOutput != null && orient != null) + orientationOutput.updateOrientation(System.currentTimeMillis()/1000., orient.heading, orient.pitch, orient.roll, false); } @@ -310,6 +348,24 @@ protected void addLocationOutput(double updatePeriod) } + /** + * Helper method to add an orientation output so that all sensors can update their orientation + * in a consistent manner. + * @param updatePeriod estimated orientation update period or NaN if sensor is mostly static + */ + protected void addOrientationOutput(double updatePeriod) + { + synchronized(obsOutputs) + { + if (orientationOutput == null) + { + orientationOutput = new DefaultOrientationOutputEuler(this, getLocalFrameID(), updatePeriod); + addOutput(orientationOutput, true); + } + } + } + + /** * Removes all outputs previously added to this sensor */ diff --git a/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/DefaultOrientationOutput.java b/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/DefaultOrientationOutput.java new file mode 100644 index 000000000..b022519fb --- /dev/null +++ b/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/DefaultOrientationOutput.java @@ -0,0 +1,56 @@ +/***************************** BEGIN LICENSE BLOCK *************************** + The contents of this file are subject to the Mozilla Public License, v. 2.0. + If a copy of the MPL was not distributed with this file, You can obtain one + at http://mozilla.org/MPL/2.0/. + + Software distributed under the License is distributed on an "AS IS" basis, + WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License + for the specific language governing rights and limitations under the License. + + Copyright (C) 2024 Botts Innovative Research Inc. All Rights Reserved. + ******************************* END LICENSE BLOCK ***************************/ +package org.sensorhub.impl.sensor; + +import net.opengis.swe.v20.DataComponent; +import net.opengis.swe.v20.DataEncoding; +import org.sensorhub.api.sensor.ISensorDriver; + +/** + * Default orientation output for sensor drivers outputting their own orientation. + */ +public abstract class DefaultOrientationOutput extends AbstractSensorOutput { + protected DataComponent outputStruct; + DataEncoding outputEncoding; + protected double updatePeriod; + + protected DefaultOrientationOutput(ISensorDriver parentSensor, double updatePeriod) { + super(AbstractSensorModule.ORIENTATION_OUTPUT_NAME, parentSensor); + this.updatePeriod = updatePeriod; + } + + @Override + public DataComponent getRecordDescription() { + return outputStruct; + } + + @Override + public DataEncoding getRecommendedEncoding() { + return outputEncoding; + } + + @Override + public double getAverageSamplingPeriod() { + return updatePeriod; + } + + /** + * Update the orientation output with the given heading, pitch, and roll angles. + * + * @param time The time of the orientation update. + * @param heading The heading angle in degrees. + * @param pitch The pitch angle in degrees. + * @param roll The roll angle in degrees. + * @param forceUpdate If true, the orientation will be updated even if the angles have not changed. + */ + public abstract void updateOrientation(double time, double heading, double pitch, double roll, boolean forceUpdate); +} \ No newline at end of file diff --git a/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/DefaultOrientationOutputEuler.java b/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/DefaultOrientationOutputEuler.java new file mode 100644 index 000000000..2862e812b --- /dev/null +++ b/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/DefaultOrientationOutputEuler.java @@ -0,0 +1,63 @@ +/***************************** BEGIN LICENSE BLOCK *************************** + The contents of this file are subject to the Mozilla Public License, v. 2.0. + If a copy of the MPL was not distributed with this file, You can obtain one + at http://mozilla.org/MPL/2.0/. + + Software distributed under the License is distributed on an "AS IS" basis, + WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License + for the specific language governing rights and limitations under the License. + + Copyright (C) 2012-2015 Sensia Software LLC. All Rights Reserved. + ******************************* END LICENSE BLOCK ***************************/ +package org.sensorhub.impl.sensor; + +import net.opengis.swe.v20.DataBlock; +import org.sensorhub.api.data.DataEvent; +import org.sensorhub.api.sensor.ISensorDriver; +import org.vast.swe.SWEConstants; +import org.vast.swe.helper.GeoPosHelper; + +/** + * Default orientation output with heading, pitch, and roll angles. + */ +public class DefaultOrientationOutputEuler extends DefaultOrientationOutput { + public DefaultOrientationOutputEuler(ISensorDriver parentSensor, String sensorFrameID, double updatePeriod) { + super(parentSensor, updatePeriod); + + GeoPosHelper fac = new GeoPosHelper(); + + outputStruct = fac.createRecord() + .label("Sensor Orientation") + .addSamplingTimeIsoUTC("time") + .addField("orientation", fac.createVector() + .from(fac.newEulerOrientationNED(SWEConstants.DEF_SENSOR_ORIENT)) + .localFrame('#' + sensorFrameID)) + .build(); + + outputStruct.setName(getName()); + outputStruct.setId(AbstractSensorModule.ORIENTATION_OUTPUT_ID); + outputEncoding = fac.newTextEncoding(); + } + + @Override + public void updateOrientation(double time, double heading, double pitch, double roll, boolean forceUpdate) { + // Build new DataBlock + DataBlock dataBlock = (latestRecord == null) ? outputStruct.createDataBlock() : latestRecord.renew(); + dataBlock.setDoubleValue(0, time); + dataBlock.setDoubleValue(1, heading); + dataBlock.setDoubleValue(2, pitch); + dataBlock.setDoubleValue(3, roll); + + var changed = forceUpdate || latestRecord == null || + latestRecord.getDoubleValue(1) != dataBlock.getDoubleValue(1) || + latestRecord.getDoubleValue(2) != dataBlock.getDoubleValue(2) || + latestRecord.getDoubleValue(3) != dataBlock.getDoubleValue(3); + + // If the location has actually changed, update the latest record and send event + if (changed) { + latestRecord = dataBlock; + latestRecordTime = System.currentTimeMillis(); + eventHandler.publish(new DataEvent(latestRecordTime, this, dataBlock)); + } + } +} From acd90cd2a2301749f9ea91a8f61952fb5038ec5e Mon Sep 17 00:00:00 2001 From: Michael Elmore Date: Tue, 13 Aug 2024 12:18:18 -0500 Subject: [PATCH 2/2] Remove FOI for orientation --- .../impl/sensor/AbstractSensorModule.java | 22 +------------------ 1 file changed, 1 insertion(+), 21 deletions(-) diff --git a/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/AbstractSensorModule.java b/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/AbstractSensorModule.java index 92fcfc523..d2c85ddd4 100644 --- a/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/AbstractSensorModule.java +++ b/sensorhub-core/src/main/java/org/sensorhub/impl/sensor/AbstractSensorModule.java @@ -196,31 +196,11 @@ protected void afterInit() throws SensorHubException } } - // add orientation output and foi if an orientation is set in config + // add orientation output if an orientation is set in config if (config.getOrientation() != null) { - EulerOrientation loc = config.getOrientation(); - if (orientationOutput == null) addOrientationOutput(Double.NaN); - - if (foiMap.isEmpty()) - { - // add - SamplingPoint sf = new SamplingPoint(); - sf.setId("FOI_" + xmlID); - sf.setUniqueIdentifier(uniqueID + ":foi"); - if (config.name != null) - sf.setName(config.name); - sf.setDescription("Sampling point for " + config.name); - sf.setHostedProcedureUID(uniqueID); - Point point = new GMLFactory(true).newPoint(); - point.setSrsName(SWEConstants.REF_FRAME_4979); - point.setSrsDimension(3); - point.setPos(new double[] {loc.heading, loc.pitch, loc.roll}); - sf.setShape(point); - foiMap.put(sf.getUniqueIdentifier(), sf); - } } super.afterInit();