@wemap/positioning 2.0.0 → 2.1.0

package/src/providers/pose/GnssWifiPdrProvider.js

@@ -0,0 +1,233 @@
+/* eslint max-statements: ["error", 40]*/
+
+import { WGS84UserPosition } from '@wemap/geo';
+
+import MapMatchingProvider from '../others/MapMatchingProvider';
+import PdrProvider from './pdr/PdrProvider';
+import GnssWifiProvider from '../position/GnssWifiProvider';
+import EventType from '../../events/EventType';
+import AbsoluteAttitudeProvider from '../attitude/AbsoluteAttitudeProvider';
+import Constants from '../Constants';
+import ProviderError from '../../events/ProviderError';
+
+const GPF_ACCURACY = 25;
+const GPF_DISTANCE = 25;
+
+const MM_GNSS_DIST = 20;
+const MM_GNSS_ANGLE = 20;
+
+class GnssWifiPdrProvider extends MapMatchingProvider {
+
+    /**
+    * @override
+    */
+    constructor(onEvent, onError, options) {
+        super(onEvent, onError, options);
+
+        this.pdrProvider = new PdrProvider(e => this.onPdrEvent(e), e => this.onProviderError(e), options);
+        this.gnssWifiProvider = new GnssWifiProvider(e => this.onGnssWifiEvent(e), e => this.onProviderError(e), options);
+        this.absoluteAttitudeProvider = new AbsoluteAttitudeProvider(
+            e => this.onAbsoluteAttitudeEvent(e),
+            e => this.onProviderError(e),
+            options
+        );
+
+        this.gpsLastUpdate = 0;
+        this.isFirstGnssUpdate = true;
+        this.isFirstAttitudeUpdate = true;
+    }
+
+
+    /**
+    * @override
+    */
+    static get displayName() {
+        return 'GnssWifiPdr';
+    }
+
+    /**
+     * @override
+     */
+    static get eventsType() {
+        return [EventType.AbsoluteAttitude, EventType.AbsolutePosition];
+    }
+
+    /**
+     * @override
+     */
+    static get requiredProviders() {
+        return [PdrProvider, GnssWifiProvider, AbsoluteAttitudeProvider];
+    }
+
+
+    /**
+     * @override
+     */
+    startInternal() {
+        this.pdrProvider.start();
+        this.gnssWifiProvider.start();
+        this.absoluteAttitudeProvider.start();
+    }
+
+    /**
+     * @override
+     */
+    stop() {
+        this.imuProvider.stop();
+        this.gnssWifiProvider.stop();
+        this.absoluteAttitudeProvider.stop();
+    }
+
+    /**
+     * @private
+     */
+    onPdrEvent(pdrEvent) {
+        pdrEvent.forEach(event => {
+            if (event.dataType === EventType.AbsolutePosition) {
+                this.location = event.data;
+            } else if (event.dataType === EventType.AbsoluteAttitude) {
+                this.attitude = event.data;
+            }
+        });
+        this.notify(...pdrEvent);
+    }
+
+    /**
+     * @private
+     */
+    onGnssWifiEvent(events) {
+
+        const gnssWifiEvent = events[0];
+        const location = gnssWifiEvent.data;
+
+        // This should be called to update True North / Magnetic North declination
+        this.absoluteAttitudeProvider.setLocation(location);
+
+        if (location.accuracy > GPF_ACCURACY) {
+            return;
+        }
+
+        this.gnssLocation = location.clone();
+        this.gnssLocation.alt = Constants.DEFAULT_ALTITUDE;
+
+        if (!this.location || this.location
+            && this.location.distanceTo(this.gnssLocation) > GPF_DISTANCE) {
+
+            if (!this.mapMatching || !this.attitude) {
+                this.pdrProvider.setLocation(this.gnssLocation);
+            } else {
+
+                this.gnssLocation.bearing = this.attitude.headingDegrees;
+                const projection = this.mapMatching.getProjection(this.gnssLocation);
+
+                if (projection && projection.projection) {
+
+                    // Create a new location from projection and new GNSS location.
+                    const projectedLocation = WGS84UserPosition.fromWGS84(projection.projection, this.gnssLocation);
+                    this.pdrProvider.setLocation(projectedLocation);
+
+                    // // If nearest element is an edge, use its orientation to set heading
+                    // if (projection.nearestElement instanceof Edge) {
+                    //     const edgeBearing = projection.nearestElement.bearing;
+                    //     const diff1 = MathUtils.diffAngle(MathUtils.deg2rad(this.gnssLocation.bearing), edgeBearing);
+                    //     const diff2 = MathUtils.diffAngle(MathUtils.deg2rad(this.gnssLocation.bearing), edgeBearing + Math.PI);
+                    //     this.pdrProvider.setHeading(diff1 < diff2 ? edgeBearing : edgeBearing + Math.PI);
+                    // }
+
+                    if (this.lastAttitude) {
+                        this.pdrProvider.setHeading(this.lastAttitude.heading);
+                    }
+
+                } else {
+                    this.pdrProvider.setLocation(this.gnssLocation);
+                }
+            }
+        }
+
+    }
+
+    onAbsoluteAttitudeEvent(events) {
+
+        const attitude = events[0].data;
+
+        if (this.isFirstAttitudeUpdate) {
+            this.pdrProvider.setHeading(attitude.heading);
+            this.isFirstAttitudeUpdate = false;
+        }
+
+        this.lastAttitude = attitude;
+    }
+
+    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
+            )
+        );
+    }
+
+    /**
+     * MapMatching
+     */
+
+    enableMapMatching(network = null, maxDistance = MM_GNSS_DIST, maxAngleBearing = MM_GNSS_ANGLE) {
+        this.pdrProvider.enableMapMatching(network, maxDistance, maxAngleBearing);
+        super.enableMapMatching(network, maxDistance, maxAngleBearing);
+    }
+
+
+    setNetwork(network) {
+        this.pdrProvider.setNetwork(network);
+        super.setNetwork(network);
+    }
+
+    /**
+     * Itinerary
+     */
+
+    setItinerary(itinerary) {
+
+        const isFirstItinerary = !this.itinerary;
+
+        super.setItinerary(itinerary);
+
+        if (isFirstItinerary && itinerary.length > 0) {
+
+            // When the first itinerary is received, first or second node (depending on MM_GNSS_DIST) is used as a starting point. No map-matching is needed here as router already provide the projection in the itinerary (node2 is node1 projection on OSRM network).
+
+            if (!this.gnssLocation
+                || itinerary.length < 2
+                || !itinerary.points[0].equalsTo(this.gnssLocation)) {
+                console.warn('Itinerary has not been calculated from GnssWifiPdrProvider and these is not recommanded');
+            }
+
+            let startEdge;
+            if (itinerary.firstEdge.getLength() <= MM_GNSS_DIST) {
+                startEdge = itinerary.firstEdge;
+            } else {
+                startEdge = itinerary.secondEdge;
+            }
+            const startPoint = WGS84UserPosition.fromWGS84(startEdge.node1);
+            startPoint.alt = Constants.DEFAULT_ALTITUDE;
+            this.pdrProvider.setLocation(startPoint);
+            this.pdrProvider.setStepDetectionLockerOrientation(startEdge.getBearing());
+        }
+
+    }
+
+}
+
+export default GnssWifiPdrProvider;

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;