Add support for default orientation output

sensorhub-core/src/main/java/org/sensorhub/impl/sensor/AbstractSensorModule.java

@@ -93,6 +93,8 @@ public abstract class AbstractSensorModule<T extends SensorConfig> 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<T extends SensorConfig> 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</li>
      * <li>If location is provided in config, a generic feature interest
      * and a location output</li>
+     * <li>If orientation is provided in config, a generic orientation output</li>
      * </ul>
      * 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,25 @@ protected void afterInit() throws SensorHubException
                 foiMap.put(sf.getUniqueIdentifier(), sf);
             }
         }
+
+        // add orientation output if an orientation is set in config
+        if (config.getOrientation() != null)
+        {
+            if (orientationOutput == null)
+                addOrientationOutput(Double.NaN);
+        }
 
         super.afterInit();
     }
 
 
     /**
-     * This methods does the following:
+     * This method does the following:
      * <ul>
      * <li>Register the driver with the system registry if the driver is
      * connected to a hub (i.e. setParentHub() has been called)</li>
      * <li>Send a location data event if a location output has been created</li>
+     * <li>Send an orientation data event if an orientation output has been created</li>
      * </ul>
      * In most cases, derived classes overriding this method must call it
      * using the super keyword.
@@ -230,6 +243,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 +328,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
      */

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<ISensorDriver> {
+    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);
+}

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));
+        }
+    }
+}