updated axis driver to have camera sensor orientation based on pan an…

sensors/video/sensorhub-driver-axis/build.gradle

@@ -1,6 +1,6 @@
 description = 'Axis Video Camera'
 ext.details = 'Driver for IP video cameras from Axis (with PTZ tasking support)'
-version = '1.1.0'
+version = '1.2.0'
 
 dependencies {
   implementation 'org.sensorhub:sensorhub-core:' + oshCoreVersion

sensors/video/sensorhub-driver-axis/src/main/java/org/sensorhub/impl/sensor/axis/AxisCameraDriver.java

@@ -62,6 +62,8 @@ public class AxisCameraDriver extends AbstractSensorModule < AxisCameraConfig >
     AxisPtzOutput ptzPosOutput;
     AxisVideoControl videoControlInterface;
     AxisPtzControl ptzControlInterface;
+    AxisSensorOrientationOutput sensorOrientationOutput;
+
 
     String hostUrl;
     String serialNumber;
@@ -71,11 +73,17 @@ public class AxisCameraDriver extends AbstractSensorModule < AxisCameraConfig >
 
     int vapixVersion = 0;
 
+    double initialHeading = 0.0;
+    double initialPitch = 0.0;
+    double initialRoll = 0.0;
+
+
     boolean ptzSupported = false;
     boolean mjpegSupported = false;
     boolean h264Supported = false;
     boolean h265Supported = false;
     boolean mpeg4Supported = false;
+    boolean initialOrientationCaptured = false;
 
     public AxisCameraDriver() {
 
@@ -111,6 +119,7 @@ protected void doInit() throws SensorHubException {
         ptzPosOutput = null;
         ptzControlInterface = null;
         ptzSupported = false;
+        initialOrientationCaptured = false;
 
         // create connection handler
         connection = new RobustHTTPConnection(this, config.connection, "Axis Camera") {
@@ -241,16 +250,41 @@ else if (tokens[0].trim().equalsIgnoreCase("root.Properties.API.HTTP.Version"))
             addOutput(h265VideoStream.getOutput(), false);
         }
 
+        if (config.position.orientation != null) {
+            initialHeading = config.position.orientation.heading;
+            initialPitch = config.position.orientation.pitch;
+            initialRoll = config.position.orientation.roll;
+            initialOrientationCaptured = true;
+
+            // add Static System orientation output
+            addOrientationOutput(Double.NaN);
+            orientationOutput.getRecordDescription().setLabel("Platform Orientation");
+            orientationOutput.getRecordDescription().setDescription(
+                "Static mount orientation of the camera platform at its installed position. " +
+                "Represents heading, pitch, and roll in the NED frame when the camera PTZ is at (0, 0, 0). " +
+                "Set via configuration and published once on start.");
+        }
+
         if (ptzSupported) {
             // add PTZ output
             ptzPosOutput = new AxisPtzOutput(this);
             addOutput(ptzPosOutput, false);
             ptzPosOutput.init();
 
+
+
             // add PTZ controller
             ptzControlInterface = new AxisPtzControl(this);
             addControlInput(ptzControlInterface);
             ptzControlInterface.init();
+
+            // add camera sensor orientation 
+            if (initialOrientationCaptured) {
+                sensorOrientationOutput = new AxisSensorOrientationOutput(this, getLocalFrameID());
+                addOutput(sensorOrientationOutput, false);
+
+
+            }
         }
     }
 
@@ -274,9 +308,31 @@ protected void doStart() throws SensorHubException {
         if (ptzSupported) {
             ptzPosOutput.start();
             ptzControlInterface.start();
+
         }
     }
 
+    protected void updateOrientationFromPtz(double pan, double tilt) {
+        if (!initialOrientationCaptured || sensorOrientationOutput == null)
+            return;
+
+        double newHeading = normalizeHeadingDeg(initialHeading + pan);
+        double newPitch = initialPitch + tilt;
+        double newRoll = initialRoll;
+
+        sensorOrientationOutput.updateOrientation(newHeading, newPitch, newRoll);
+    }
+
+    /** Wrap a heading angle into the range (-180, 180]. */
+    private static double normalizeHeadingDeg(double deg) {
+        double d = deg % 360.0;
+        if (d > 180.0)
+            d -= 360.0;
+        else if (d <= -180.0)
+            d += 360.0;
+        return d;
+    }
+
 
     @Override
     protected void updateSensorDescription() {

sensors/video/sensorhub-driver-axis/src/main/java/org/sensorhub/impl/sensor/axis/AxisPtzOutput.java

@@ -165,6 +165,13 @@ public void run()
 
                         String line;
 
+                        // track parsed pan/tilt so we can push them to the driver
+                        // for dynamic orientation updates
+                        float currentPan = 0f;
+                        float currentTilt = 0f;
+                        boolean gotPan = false;
+                        boolean gotTilt = false;
+
                         short lines = 0;
                         final byte totalReads = 3;
 
@@ -174,12 +181,17 @@ public void run()
                             String[] tokens = line.split("=");
                             if (tokens[0].trim().equalsIgnoreCase("pan"))
                             {
-                                dataStruct.getComponent("pan").getData().setFloatValue(Float.parseFloat(tokens[1]));
+                                currentPan = Float.parseFloat(tokens[1]);
+                                dataStruct.getComponent("pan").getData().setFloatValue(currentPan);
+                                gotPan = true;
                                 lines++;
+
                             }
                             else if (tokens[0].trim().equalsIgnoreCase("tilt"))
                             {
-                                dataStruct.getComponent("tilt").getData().setFloatValue(Float.parseFloat(tokens[1]));
+                                currentTilt = Float.parseFloat(tokens[1]);
+                                dataStruct.getComponent("tilt").getData().setFloatValue(currentTilt);
+                                gotTilt = true;
                                 lines++;
                             }
                             else if (tokens[0].trim().equalsIgnoreCase("zoom"))
@@ -194,6 +206,10 @@ else if (tokens[0].trim().equalsIgnoreCase("zoom"))
                             latestRecord = dataStruct.getData();
                             latestRecordTime = System.currentTimeMillis();
                             eventHandler.publish(new DataEvent(latestRecordTime, AxisPtzOutput.this, latestRecord));
+
+                            if(gotPan && gotTilt) {
+                                parentSensor.updateOrientationFromPtz(currentPan, currentTilt);
+                            }
                         } else {
                             throw new SensorException("Invalid sensor data from AXIS camera. AXIS url: " + ptzURL.toString());
                         }

sensors/video/sensorhub-driver-axis/src/main/java/org/sensorhub/impl/sensor/axis/AxisSensorOrientationOutput.java

@@ -0,0 +1,89 @@
+/***************************** 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.
+
+The Initial Developer is Botts Innovative Research Inc. Portions created by the Initial
+Developer are Copyright (C) 2014 the Initial Developer. All Rights Reserved.
+
+******************************* END LICENSE BLOCK ***************************/
+
+package org.sensorhub.impl.sensor.axis;
+
+import net.opengis.swe.v20.DataBlock;
+import net.opengis.swe.v20.DataComponent;
+import net.opengis.swe.v20.DataEncoding;
+import net.opengis.swe.v20.DataRecord;
+import org.sensorhub.api.data.DataEvent;
+import org.sensorhub.impl.sensor.AbstractSensorOutput;
+import org.vast.swe.SWEConstants;
+import org.vast.swe.helper.GeoPosHelper;
+
+public class AxisSensorOrientationOutput extends AbstractSensorOutput<AxisCameraDriver> {
+
+    private static final String OUTPUT_NAME = "sensorOrientationPtz";
+
+    DataRecord outputStruct;
+    DataEncoding outputEncoding;
+
+    public AxisSensorOrientationOutput(AxisCameraDriver driver, String sensorFrameID){
+        super(OUTPUT_NAME, driver);
+
+        GeoPosHelper fac = new GeoPosHelper();
+
+        outputStruct = fac.createRecord()
+            .name(OUTPUT_NAME)
+            .label("Sensor Orientation")
+            .description("Real-time orientation of the camera sensor computed from the current PTZ position. " +
+                    "Heading and pitch are derived by applying pan and tilt offsets to the platform mount " +
+                    "orientation. Updated each time the PTZ output polls the camera.")
+            .addSamplingTimeIsoUTC("time")
+            .addField("orientation", fac.createVector()
+                    .from(fac.newEulerOrientationNED(SWEConstants.DEF_SENSOR_ORIENT))
+                    .localFrame('#' + sensorFrameID))
+                .build();
+
+        outputEncoding = fac.newTextEncoding();
+    }
+
+    void updateOrientation(double heading, double pitch, double roll) {
+        double time = System.currentTimeMillis() / 1000.0;
+
+        DataBlock dataBlock = outputStruct.createDataBlock();
+        dataBlock.setDoubleValue(0, time);
+        dataBlock.setDoubleValue(1, heading);
+        dataBlock.setDoubleValue(2, pitch);
+        dataBlock.setDoubleValue(3, roll);
+
+        latestRecord = dataBlock;
+        latestRecordTime = System.currentTimeMillis();
+        eventHandler.publish(new DataEvent(latestRecordTime, this, dataBlock));
+    }
+
+
+    @Override
+    public DataComponent getRecordDescription() {
+        return outputStruct;
+    }
+
+
+    @Override
+    public DataEncoding getRecommendedEncoding() {
+        return outputEncoding;
+    }
+
+
+    @Override
+    public double getAverageSamplingPeriod() {
+        return 1.0;
+    }
+
+
+
+
+}