@wemap/positioning 1.2.2 → 2.1.0

package/src/providers/pose/PoseProvider.js

@@ -0,0 +1,90 @@
+import Provider from '../Provider';
+import EventType from '../../events/EventType';
+
+import GnssWifiProvider from '../position/GnssWifiProvider';
+import AbsoluteAttitudeProvider from '../attitude/AbsoluteAttitudeProvider';
+import ProviderError from '../../events/ProviderError';
+
+/**
+ * Pose provider is the provider used by the PositioningHandler. It uses the best fusion
+ * of what he can and provides an AbsoluteAttitude and an AbsolutePosition as an output.
+ */
+class PoseProvider extends Provider {
+
+    /**
+    * @override
+    */
+    constructor(onEvent, onError, options) {
+        super(onEvent, onError, options);
+
+        this.absoluteAttitudeProvider = new AbsoluteAttitudeProvider(onEvent, e => this.onAbsoluteAttitudeError(e));
+        this.gnssWifiProvider = new GnssWifiProvider((events) => {
+            this.absoluteAttitudeProvider.setLocation(events[0].data);
+            onEvent(events);
+        }, e => this.onGnssWifiError(e));
+    }
+
+    /**
+    * @override
+    */
+    static get displayName() {
+        return 'Pose provider';
+    }
+
+    /**
+     * @override
+     */
+    static get eventsType() {
+        return [EventType.AbsoluteAttitude, EventType.AbsolutePosition];
+    }
+
+    /**
+     * Return the list of required providers
+     */
+    static get requiredProviders() {
+        return [AbsoluteAttitudeProvider, GnssWifiProvider];
+    }
+
+    /**
+     * @override
+     */
+    startInternal() {
+        this.absoluteAttitudeProvider.start();
+        this.gnssWifiProvider.start();
+    }
+
+    /**
+     * @override
+     */
+    stopInternal() {
+        this.absoluteAttitudeProvider.stop();
+        this.gnssWifiProvider.stop();
+    }
+
+    onAbsoluteAttitudeError(errors) {
+        this.notifyError(...ProviderError.modifyArrayDataType(errors, EventType.AbsoluteAttitude));
+    }
+
+    onGnssWifiError(errors) {
+        this.notifyError(...ProviderError.modifyArrayDataType(errors, EventType.AbsolutePosition));
+    }
+
+    /**
+     * @override
+     */
+    static checkAvailabilityErrors() {
+        const errorsAttitude = AbsoluteAttitudeProvider.checkAvailabilityErrors();
+        const errorsGnssWifi = GnssWifiProvider.checkAvailabilityErrors();
+        return ProviderError.modifyArrayDataType(
+            errorsGnssWifi,
+            EventType.AbsolutePosition
+        ).concat(
+            ProviderError.modifyArrayDataType(
+                errorsAttitude,
+                EventType.AbsoluteAttitude
+            )
+        );
+    }
+}
+
+export default PoseProvider;

package/src/providers/pose/pdr/PdrProvider.js

@@ -0,0 +1,352 @@
+import noop from 'lodash.noop';
+
+import {
+    Constants as GeoConstants, Itinerary, WGS84UserPosition
+} from '@wemap/geo';
+import {
+    rad2deg, Quaternion
+} from '@wemap/maths';
+
+import StepDetection from './steps/StepDetection';
+import HeadingUnlocker from './helpers/HeadingUnlocker';
+import ThugDetector from './helpers/ThugDetector';
+import Smoother from './helpers/Smoother';
+import RelativeAttitudeProvider from '../../attitude/RelativeAttitudeProvider';
+import ImuProvider from '../../others/ImuProvider';
+import EventType from '../../../events/EventType';
+import MapMatchingProvider from '../../others/MapMatchingProvider';
+import ProviderError from '../../../events/ProviderError';
+
+
+const MM_PDR_ANGLE = 20;
+const MM_PDR_DIST = 15;
+const MM_CONV_SPEED = 0.7;
+
+const LO_SEGMENT_SIZE = 1.5;
+
+const DEFAULT_OPTIONS = {
+    stepdetectionlocker: true,
+    smoother: true,
+    onThugEvent: noop
+};
+
+class PdrProvider extends MapMatchingProvider {
+
+    // pdrLocation can be different from this.location
+    // pdrLocation is raw location calculated by PdrProvider where
+    //     this.location is smoothed location.
+    pdrLocation = null;
+
+    /**
+    * @override
+    */
+    constructor(onEvent, onError, options) {
+        super(onEvent, onError, Object.assign(DEFAULT_OPTIONS, options || {}));
+
+        // Input data providers
+        this.relativeAttitudeProvider = new RelativeAttitudeProvider(
+            e => this.onRelativeAttitudeEvent(e),
+            e => this.onProviderError(e),
+            options
+        );
+        this.imuProvider = new ImuProvider(
+            e => this.onImuEvent(e),
+            e => this.onProviderError(e),
+            Object.assign(options || {}, { require: [EventType.Acceleration, EventType.AngularRate] })
+        );
+
+        // Helpers for PDR
+        this.stepDetection = new StepDetection();
+        this.stepDetectionLocker = new HeadingUnlocker();
+        this.smoother = new Smoother();
+        this.thugDetector = new ThugDetector();
+
+        if (options.useMapMatching) {
+            this.enableMapMatching();
+        }
+    }
+
+
+    /**
+    * @override
+    */
+    static get displayName() {
+        return 'PDR';
+    }
+
+    /**
+     * @override
+     */
+    static get eventsType() {
+        return [EventType.AbsoluteAttitude, EventType.AbsolutePosition];
+    }
+
+    /**
+     * @override
+     */
+    static get requiredProviders() {
+        return [ImuProvider, RelativeAttitudeProvider];
+    }
+
+
+    /**
+     * @override
+     */
+    startInternal() {
+        this.imuProvider.start();
+        this.relativeAttitudeProvider.start();
+    }
+
+    /**
+     * @override
+     */
+    stopInternal() {
+        this.imuProvider.stop();
+        this.relativeAttitudeProvider.stop();
+    }
+
+    /**
+     * Get provider name
+     * @override
+     */
+    get nameWithOptions() {
+        let name = this.name;
+        name += ' (locker: ';
+        name += this.options.stepdetectionlocker ? 'Y' : 'N';
+        name += ', ';
+        name += 'smoo: ';
+        name += this.options.smoother ? 'Y' : 'N';
+        name += ', ';
+        name += 'mm: ';
+        name += this.options.useMapMatching ? 'Y' : 'N';
+        name += ')';
+        return name;
+    }
+
+    setHeading(heading) {
+        this.relativeAttitudeProvider.setOffset(heading);
+    }
+
+    setLocation(location) {
+        this.pdrLocation = location;
+
+        if (this.options.smoother) {
+            this.smoother.generateNextLocations(location, true);
+            // this.smoother.pullLocation(location.time) should never return null
+            this.location = this.smoother.pullLocation(location.time);
+        } else {
+            this.location = location.clone();
+        }
+
+        this.notify(this.createEvent(EventType.AbsolutePosition, this.location));
+    }
+
+    /**
+     * @override
+     */
+    onRelativeAttitudeEvent = events => {
+        const attitudeEvent = events[0];
+        this.attitude = attitudeEvent.data;
+
+        const newEvent = attitudeEvent.clone();
+        newEvent.dataType = EventType.AbsoluteAttitude;
+        this.notify(newEvent);
+    }
+
+    /**
+     * @private
+     */
+    onImuEvent = imuEvent => {
+
+        if (!this.attitude) {
+            // We should wait attitude for stepDetectionLocker and linear acceleration
+            return;
+        }
+
+        if (!this.pdrLocation) {
+            // We should wait at least an input location for PDR
+            return;
+        }
+
+        if (this.options.stepdetectionlocker
+            && this.stepDetectionLocker.locked
+            && this.stepDetectionLocker.feedHeading(this.attitude.heading)) {
+            // Step detection is locked by stepDetectionLocker
+            return;
+        }
+
+
+        let timestamp, acceleration, angularRate;
+        imuEvent.forEach(event => {
+            if (event.dataType === EventType.Acceleration) {
+                acceleration = event.data;
+                timestamp = event.timestamp;
+            } else if (event.dataType === EventType.AngularRate) {
+                angularRate = event.data;
+            }
+        });
+
+        const heading = this.attitude.heading;
+
+        if (this.onThugEvent && this.thugDetector.compute(timestamp, acceleration)) {
+            this.onThugEvent();
+        }
+
+        /**
+         * Step Detection and Step Size Detection
+         */
+        const linearAcc = PdrProvider.computeLinearAcceleration(
+            this.attitude.quaternion, acceleration);
+        const stepDetected = this.stepDetection.compute(timestamp, linearAcc, angularRate);
+
+        if (stepDetected) {
+
+            this.pdrLocation = this.calculateNewLocation(this.pdrLocation, timestamp,
+                heading, this.stepDetection.lastStepSize);
+
+            /**
+            * Update current_position
+            */
+            if (this.options.smoother) {
+                this.smoother.generateNextLocations(this.pdrLocation);
+                const newLocation = this.smoother.pullLocation(timestamp);
+                // At this time, newLocation can be null if a new step has been detected
+                //  and smoother has not been completely consumed.
+                if (newLocation) {
+                    this.location = newLocation;
+                }
+            } else {
+                this.location = this.pdrLocation;
+            }
+
+        } else if (this.options.smoother) {
+            // If no step is detected, we pull last known location from smoother until now (timestamp).
+            const smoothedLocation = this.smoother.pullLocation(timestamp);
+            if (!smoothedLocation) {
+                return;
+            }
+            this.location = smoothedLocation;
+        } else {
+            return;
+        }
+
+        this.notify(this.createEvent(EventType.AbsolutePosition, this.location));
+    }
+
+    onProviderError(errors) {
+        this.notifyError(...ProviderError.modifyArrayDataType(errors, EventType.AbsoluteAttitude));
+        this.notifyError(...ProviderError.modifyArrayDataType(errors, EventType.AbsolutePosition));
+    }
+
+    /**
+     * @override
+     */
+    static checkAvailabilityErrors() {
+        const errors = super.checkAvailabilityErrors();
+        return ProviderError.modifyArrayDataType(
+            errors,
+            EventType.AbsolutePosition
+        ).concat(
+            ProviderError.modifyArrayDataType(
+                errors,
+                EventType.AbsoluteAttitude
+            )
+        );
+    }
+
+    calculateNewLocation(previousLocation, timestamp, heading, stepSize) {
+
+        /**
+         * Compute new_position
+         */
+        const newLocationWithoutMM = previousLocation.clone();
+        newLocationWithoutMM.move(stepSize, heading);
+        newLocationWithoutMM.bearing = rad2deg(heading);
+        newLocationWithoutMM.time = timestamp;
+
+        if (!this.mapMatching) {
+            return newLocationWithoutMM;
+        }
+
+        const projection = this.mapMatching.getProjection(newLocationWithoutMM);
+
+        if (!projection || !projection.projection) {
+            return newLocationWithoutMM;
+        }
+
+        if (projection.distanceFromNearestElement < stepSize) {
+            return WGS84UserPosition.fromWGS84(projection.projection, newLocationWithoutMM);
+        }
+
+        /**
+         * Update new_position
+         */
+        const smoothedDistance = projection.distanceFromNearestElement * MM_CONV_SPEED;
+        const smoothedBearing = previousLocation.bearingTo(projection.projection);
+        const smoothedLocation = previousLocation.clone();
+        smoothedLocation.move(smoothedDistance, smoothedBearing);
+
+        return WGS84UserPosition.fromWGS84(smoothedLocation, newLocationWithoutMM);
+    }
+
+
+    /**
+     * MapMatching
+     */
+
+    enableMapMatching(network, maxDistance, maxAngleBearing) {
+        super.enableMapMatching(network, maxDistance || MM_PDR_DIST, maxAngleBearing || MM_PDR_ANGLE);
+    }
+
+
+    /**
+     * Itinerary and PDR Locker
+     */
+
+    setItinerary(itinerary) {
+        super.setItinerary(itinerary);
+
+        if (this.options.stepdetectionlocker) {
+            this.stepDetectionLocker.setWaitingOrientation(
+                PdrProvider.retrieveWaitingOrientationFromItinerary(itinerary)
+            );
+        }
+    }
+
+    static retrieveWaitingOrientationFromItinerary(itinerary) {
+
+        if (!itinerary) {
+            throw new Error('Empty itinerary');
+        }
+
+        if (!(itinerary instanceof Itinerary)) {
+            throw new TypeError(itinerary + ' is not an instance of itinerary');
+        }
+
+        if (itinerary.length < 1) {
+            throw new Error('Empty itinerary');
+        }
+
+        if (itinerary.firstEdge.getLength() > LO_SEGMENT_SIZE) {
+            return itinerary.firstEdge.getBearing();
+        }
+
+        if (itinerary.length < 2) {
+            throw new Error('Itinerary is too short');
+        }
+        return itinerary.secondEdge.getBearing();
+    }
+
+    setStepDetectionLockerOrientation(orientation) {
+        this.stepDetectionLocker.setWaitingOrientation(orientation);
+    }
+
+    // Linear acceleration in ENU
+    static computeLinearAcceleration(quaternion, acc) {
+        const linearAcc = Quaternion.rotate(Quaternion.inverse(quaternion), acc);
+        linearAcc[2] -= GeoConstants.EARTH_GRAVITY;
+        return linearAcc;
+    }
+}
+
+export default PdrProvider;

package/src/providers/pose/pdr/helpers/HeadingUnlocker.js

@@ -0,0 +1,41 @@
+import isnumber from 'lodash.isnumber';
+
+import { diffAngle } from '@wemap/maths';
+
+const LO_ANGLE = 15 * Math.PI / 180;
+
+/**
+ * HeadingUnlocker is designed to block Pdr Step Detection until user is aiming in the right direction
+ */
+class HeadingUnlocker {
+
+    constructor() {
+        // HeadingUnlocker status, by default it is unlocked
+        this.locked = false;
+    }
+
+    setWaitingOrientation(waitingOrientation) {
+        this.waitingOrientation = waitingOrientation;
+        this.locked = true;
+    }
+
+    feedHeading(heading) {
+
+        if (!isnumber(this.waitingOrientation)) {
+            throw new Error('HeadingUnlocker has not been initialized by setWaitingOrientation');
+        }
+
+        if (!this.locked) {
+            throw new Error('HeadingUnlocker is already unlocked');
+        }
+
+        if (Math.abs(diffAngle(heading, this.waitingOrientation)) < LO_ANGLE) {
+            this.locked = false;
+        }
+
+        return this.locked;
+    }
+
+}
+
+export default HeadingUnlocker;

package/src/providers/pose/pdr/helpers/HeadingUnlocker.spec.js

@@ -0,0 +1,26 @@
+import chai from 'chai';
+import chaiAlmost from 'chai-almost';
+
+import HeadingUnlocker from './HeadingUnlocker';
+
+const expect = chai.expect;
+chai.use(chaiAlmost());
+
+describe('HeadingUnlocker test short first edge', () => {
+
+    it('Should return the good value', () => {
+
+        const headingUnlocker = new HeadingUnlocker();
+        headingUnlocker.setWaitingOrientation(102 * Math.PI / 180);
+
+        headingUnlocker.feedHeading(0 * Math.PI / 180);
+        expect(headingUnlocker.locked).to.be.true;
+
+        headingUnlocker.feedHeading(130 * Math.PI / 180);
+        expect(headingUnlocker.locked).to.be.true;
+
+        headingUnlocker.feedHeading(100 * Math.PI / 180);
+        expect(headingUnlocker.locked).to.be.false;
+
+    });
+});

package/src/providers/pose/pdr/helpers/Smoother.js

@@ -0,0 +1,90 @@
+/* eslint max-statements: ["error", 27]*/
+import { WGS84UserPosition } from '@wemap/geo';
+
+// Generated locations by second
+const GEN_FREQUENCY = 60;
+
+// Min and max time for flyby (in second)
+const MIN_FLYBY_TIME = 0.25;
+const MAX_FLYBY_TIME = 1;
+
+class Smoother {
+
+    constructor() {
+        this.locationsQueue = [];
+    }
+
+    /**
+     * Calculate smoothed positions for the next milliseconds given a new location
+     */
+    generateNextLocations(_newLocation, flyby = false) {
+
+        if (!(_newLocation instanceof WGS84UserPosition)) {
+            throw new TypeError('newLocation is not instance of WGS84UserPosition');
+        }
+
+        const newLocation = _newLocation.clone();
+        if (!newLocation.hasOwnProperty('time')) {
+            throw new Error('newLocation does not have time property');
+        }
+
+        if (!this.previousLocation) {
+            this.previousLocation = newLocation;
+            this.locationsQueue.push(this.previousLocation);
+            return;
+        }
+
+        const distance = this.previousLocation.distanceTo(newLocation);
+        const azimuth = this.previousLocation.bearingTo(newLocation);
+
+        let refTimestamp = newLocation.time;
+
+        const queueLength = this.locationsQueue.length;
+        if (queueLength) {
+            refTimestamp = Math.max(refTimestamp, this.locationsQueue[queueLength - 1].time);
+        }
+
+        let diffTime = newLocation.time - this.previousLocation.time;
+
+        if (flyby) {
+            diffTime = MAX_FLYBY_TIME;
+        }
+
+        const nSamples = GEN_FREQUENCY * Math.min(Math.max(MIN_FLYBY_TIME, diffTime), MAX_FLYBY_TIME);
+
+        let i = 1;
+        while (i < nSamples + 1) {
+            i = Math.min(i, nSamples);
+            const smoothedLocation = this.previousLocation.destinationPoint(distance * i / nSamples, azimuth);
+            smoothedLocation.time = refTimestamp + (i - 1) / GEN_FREQUENCY;
+            this.locationsQueue.push(smoothedLocation);
+            i++;
+        }
+
+        this.previousLocation = newLocation;
+    }
+
+    /**
+     * Pull locations until timestamp and return the last one.
+     * @param {*} timestamp location returned is the last before this timestamp
+     * @returns last known location before timestamp, undefined otherwise
+     */
+    pullLocation(timestamp) {
+
+        if ((typeof timestamp === 'undefined' || !timestamp)
+            && this.locationsQueue.length > 0) {
+            const output = this.locationsQueue[this.locationsQueue.length - 1];
+            this.locationsQueue = [];
+            return output;
+        }
+
+        let location;
+        while (this.locationsQueue.length && this.locationsQueue[0].time <= timestamp) {
+            location = this.locationsQueue.shift();
+        }
+        return location;
+    }
+
+}
+
+export default Smoother;