diff --git a/sensors/video/sensorhub-driver-axis/build.gradle b/sensors/video/sensorhub-driver-axis/build.gradle index f727d22ee..58771fcdb 100644 --- a/sensors/video/sensorhub-driver-axis/build.gradle +++ b/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 diff --git a/sensors/video/sensorhub-driver-axis/src/main/java/org/sensorhub/impl/sensor/axis/AxisCameraDriver.java b/sensors/video/sensorhub-driver-axis/src/main/java/org/sensorhub/impl/sensor/axis/AxisCameraDriver.java index 68f395f84..4c19b3e7c 100644 --- a/sensors/video/sensorhub-driver-axis/src/main/java/org/sensorhub/impl/sensor/axis/AxisCameraDriver.java +++ b/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() { diff --git a/sensors/video/sensorhub-driver-axis/src/main/java/org/sensorhub/impl/sensor/axis/AxisPtzOutput.java b/sensors/video/sensorhub-driver-axis/src/main/java/org/sensorhub/impl/sensor/axis/AxisPtzOutput.java index 62e2894fd..631fc8508 100644 --- a/sensors/video/sensorhub-driver-axis/src/main/java/org/sensorhub/impl/sensor/axis/AxisPtzOutput.java +++ b/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()); } diff --git a/sensors/video/sensorhub-driver-axis/src/main/java/org/sensorhub/impl/sensor/axis/AxisSensorOrientationOutput.java b/sensors/video/sensorhub-driver-axis/src/main/java/org/sensorhub/impl/sensor/axis/AxisSensorOrientationOutput.java new file mode 100644 index 000000000..9502e9be8 --- /dev/null +++ b/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 { + + 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; + } + + + + +} \ No newline at end of file