@wemap/providers 3.0.0

package/src/providers/legacy/PdrProvider.old.js

@@ -0,0 +1,300 @@
+/* eslint-disable max-statements */
+import {
+    Coordinates, UserPosition
+} from '@wemap/geo';
+
+import { Itinerary } from '@wemap/graph';
+import {deg2rad} from '@wemap/maths';
+import { TimeUtils } from '@wemap/utils';
+
+import StepDetectionProvider from '../steps/StepDetectionProvider';
+import HeadingUnlocker from './helpers/HeadingUnlocker';
+import ThugDetector from './helpers/ThugDetector';
+import Smoother from './helpers/Smoother';
+import Availability from '../../events/Availability';
+import EventType from '../../events/EventType';
+import MapMatchingProvider from '../others/MapMatchingProvider';
+import AbsoluteAttitudeProvider from '../attitude/absolute/AbsoluteAttitudeProvider';
+
+
+const MM_PDR_ANGLE = deg2rad(20);
+const MM_PDR_DIST = 15;
+const MM_CONV_SPEED = 0.7;
+
+const LO_SEGMENT_SIZE = 1.5;
+
+class AbsolutePdrProvider extends MapMatchingProvider {
+
+    // pdrPosition can be different from this.position
+    // pdrPosition is raw position calculated by PdrProvider where
+    //     this.position is smoothed position.
+    pdrPosition = null;
+
+    static MM_PDR_ANGLE = MM_PDR_ANGLE;
+    static MM_PDR_DIST = MM_PDR_DIST;
+
+    /**
+    * @override
+    */
+    constructor() {
+        super();
+
+        // Helpers for PDR
+        this.stepDetectionLocker = new HeadingUnlocker();
+        this.smoother = new Smoother();
+        this.thugDetector = new ThugDetector();
+
+        if (this.options.useMapMatching) {
+            this.enableMapMatching();
+        }
+    }
+
+    /**
+    * @override
+    */
+    static get displayName() {
+        return 'PDR';
+    }
+
+    /**
+     * @override
+     */
+    static get eventsType() {
+        return [EventType.AbsoluteAttitude, EventType.AbsolutePosition];
+    }
+
+    /**
+     * @override
+     */
+    static get _availability() {
+        return Availability.merge(
+            StepDetectionProvider.availability,
+            AbsoluteAttitudeProvider.availability
+        );
+    }
+
+
+    /**
+     * @override
+     */
+    _start() {
+
+        this.stepDetectionProviderId = this.scheduler.use(StepDetectionProvider,
+            events => this.onStepEvent(events[0]),
+            error => this.notifyError(error),
+            this.name);
+
+        this.absoluteAttitudeProviderId = this.scheduler.use(AbsoluteAttitudeProvider,
+            events => this.onAbsoluteAttitudeEvent(events),
+            error => this.notifyError(error),
+            this.name);
+
+        this.dataStore.addEventListener(EventType.AbsolutePosition, this.setPosition);
+    }
+
+    /**
+     * @override
+     */
+    _stop() {
+        this.dataStore.removeEventListener(EventType.AbsolutePosition, this.setPosition);
+        this.scheduler.release(this.stepDetectionProviderId, this.name);
+        this.scheduler.release(this.absoluteAttitudeProviderId, this.name);
+    }
+
+    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;
+    }
+
+    setPosition = (position) => {
+        if (position instanceof Coordinates) {
+            this.pdrPosition = UserPosition.fromCoordinates(position);
+            if (window && window.performance) {
+                this.pdrPosition.time = TimeUtils.preciseTime / 1e3;
+            }
+        } else {
+            this.pdrPosition = position;
+        }
+
+        // if (this.options.smoother) {
+        //     this.smoother.generateNextPositions(this.pdrPosition, true);
+        //     // this.smoother.pullPosition(position.time) should never return null
+        //     this.position = this.smoother.pullPosition(this.pdrPosition.time);
+        // } else {
+        this.position = this.pdrPosition.clone();
+        // }
+
+        this.notify(this.createEvent(EventType.AbsolutePosition, this.position));
+    };
+
+    /**
+     * @override
+     */
+    onAbsoluteAttitudeEvent = events => {
+        const attitudeEvent = events[0];
+        this.attitude = attitudeEvent.data;
+
+        const newEvent = attitudeEvent.clone();
+        newEvent.dataType = EventType.AbsoluteAttitude;
+        this.notify(newEvent);
+    }
+
+    /**
+     * @private
+     */
+    onStepEvent = stepEvent => {
+
+        const timestamp = stepEvent.timestamp;
+
+        if (!this.pdrPosition) {
+            // We should wait at least an input position for PDR
+            return;
+        }
+
+        if (this.options.stepdetectionlocker
+            && this.stepDetectionLocker.locked
+            && this.stepDetectionLocker.feedHeading(this.attitude.heading)) {
+            // Step detection is locked by stepDetectionLocker
+            return;
+        }
+
+        const heading = this.attitude.heading;
+
+        this.pdrPosition = this.calculateNewPosition(this.pdrPosition,
+            stepEvent.timestamp, heading, stepEvent.data.size);
+
+        /**
+         * Update current_position
+         */
+        if (this.options.smoother) {
+            this.smoother.generateNextPositions(this.pdrPosition);
+            const newPosition = this.smoother.pullPosition(timestamp);
+            // At this time, newPosition can be null if a new step has been detected
+            //  and smoother has not been completely consumed.
+            if (newPosition) {
+                this.position = newPosition;
+            }
+        } else {
+            this.position = this.pdrPosition;
+        }
+
+        // If no step is detected, we pull last known position from smoother until now (timestamp).
+        const smoothedPosition = this.smoother.pullPosition(timestamp);
+        if (!smoothedPosition) {
+            return;
+        }
+        this.position = smoothedPosition;
+
+        this.notify(this.createEvent(EventType.AbsolutePosition, this.position));
+    }
+
+    calculateNewPosition(previousPosition, timestamp, heading, stepSize) {
+
+        /**
+         * Compute new_position
+         */
+        const newPositionWithoutMM = previousPosition.clone();
+        newPositionWithoutMM.move(stepSize, heading);
+        newPositionWithoutMM.bearing = heading;
+        newPositionWithoutMM.time = timestamp;
+
+        if (!this.mapMatching) {
+            return newPositionWithoutMM;
+        }
+
+        /**
+         * Try map-matching
+         */
+        const projection = this.mapMatching.getProjection(newPositionWithoutMM, true, true);
+        if (!projection || !projection.projection) {
+            return newPositionWithoutMM;
+        }
+
+        const projectedPosition = newPositionWithoutMM;
+        projectedPosition.level = projection.projection.level;
+
+        if (projection.distanceFromNearestElement < stepSize) {
+            projectedPosition.lat = projection.projection.lat;
+            projectedPosition.lng = projection.projection.lng;
+            return projectedPosition;
+        }
+
+        /**
+         * Adapt map-matching to not stick corridor directly
+         * /!\ This smoothed position is different from the one of Smoother
+         */
+        const smoothedPosition = projectedPosition;
+        smoothedPosition.lat = previousPosition.lat;
+        smoothedPosition.lng = previousPosition.lng;
+
+        const smoothedDistance = projection.distanceFromNearestElement * MM_CONV_SPEED;
+        const smoothedBearing = previousPosition.bearingTo(projection.projection);
+        smoothedPosition.move(smoothedDistance, smoothedBearing);
+
+        return smoothedPosition;
+    }
+
+
+    /**
+     * 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(
+                this.constructor.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');
+        }
+
+        const edgeAt = itinerary.getEdgeAt(LO_SEGMENT_SIZE);
+        if (edgeAt) {
+            return edgeAt.bearing;
+        }
+
+        if (itinerary.length < 2) {
+            throw new Error('Itinerary is too short');
+        }
+        return itinerary.secondEdge.bearing;
+    }
+
+    setStepDetectionLockerOrientation(orientation) {
+        this.stepDetectionLocker.setWaitingOrientation(orientation);
+    }
+}
+
+export default AbsolutePdrProvider;

package/src/providers/legacy/PoseProvider.js

@@ -0,0 +1,68 @@
+import Provider from '../Provider';
+import Availability from '../../events/Availability';
+import EventType from '../../events/EventType';
+
+import GnssWifiProvider from '../position/GnssWifiProvider';
+import AbsoluteAttitudeProvider from '../attitude/absolute/AbsoluteAttitudeProvider';
+
+/**
+ * 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, onError);
+        this.gnssWifiProvider = new GnssWifiProvider((events) => {
+            this.absoluteAttitudeProvider.setPosition(events[0].data);
+            onEvent(events);
+        }, onError);
+    }
+
+    /**
+    * @override
+    */
+    static get displayName() {
+        return 'Pose provider';
+    }
+
+    /**
+     * @override
+     */
+    static get eventsType() {
+        return [EventType.AbsoluteAttitude, EventType.AbsolutePosition];
+    }
+
+    /**
+     * Return the list of required providers
+     */
+    static get _availability() {
+        return Availability.merge(
+            AbsoluteAttitudeProvider.availability,
+            GnssWifiProvider.availability
+        );
+    }
+
+    /**
+     * @override
+     */
+    _start() {
+        this.absoluteAttitudeProvider.start();
+        this.gnssWifiProvider.start();
+    }
+
+    /**
+     * @override
+     */
+    _stop() {
+        this.absoluteAttitudeProvider.stop();
+        this.gnssWifiProvider.stop();
+    }
+}
+
+export default PoseProvider;

package/src/providers/legacy/helpers/HeadingUnlocker.js

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

package/src/providers/legacy/helpers/HeadingUnlocker.spec.js

@@ -0,0 +1,53 @@
+import chai from 'chai';
+import chaiAlmost from 'chai-almost';
+
+import { deg2rad } from '@wemap/maths';
+
+import HeadingUnlocker from './HeadingUnlocker';
+
+const expect = chai.expect;
+chai.use(chaiAlmost());
+
+describe('HeadingUnlocker', () => {
+
+    it('lock & feed', () => {
+
+        const headingUnlocker = new HeadingUnlocker();
+        headingUnlocker.lock(deg2rad(102));
+
+        headingUnlocker.feedHeading(deg2rad(0));
+        expect(headingUnlocker.locked).to.be.true;
+
+        headingUnlocker.feedHeading(deg2rad(130));
+        expect(headingUnlocker.locked).to.be.true;
+
+        headingUnlocker.feedHeading(deg2rad(100));
+        expect(headingUnlocker.locked).to.be.false;
+
+    });
+
+    it('lock & unlock', () => {
+
+        const headingUnlocker = new HeadingUnlocker();
+        headingUnlocker.lock(deg2rad(102));
+
+        expect(headingUnlocker.locked).to.be.true;
+
+        headingUnlocker.unlock();
+        expect(headingUnlocker.locked).to.be.false;
+
+    });
+
+    it('fails', () => {
+
+        const headingUnlocker = new HeadingUnlocker();
+
+        expect(() => headingUnlocker.feedHeading(0)).throw(Error);
+
+        headingUnlocker.lock(deg2rad(10));
+        headingUnlocker.feedHeading(deg2rad(20));
+        expect(headingUnlocker.locked).to.be.false;
+        expect(() => headingUnlocker.feedHeading(0)).throw(Error);
+
+    });
+});

package/src/providers/legacy/helpers/Smoother.js

@@ -0,0 +1,92 @@
+/* eslint-disable max-statements */
+import { UserPosition } from '@wemap/geo';
+
+// Generated positions 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.positionsQueue = [];
+    }
+
+    /**
+     * Calculate smoothed positions for the next milliseconds given a new position
+     */
+    generateNextPositions(newPosition, flyby = false) {
+
+        if (!(newPosition instanceof UserPosition)) {
+            throw new TypeError('newPosition is not instance of UserPosition');
+        }
+
+        if (newPosition.time === null) {
+            throw new Error('newPosition does not have time property');
+        }
+
+        if (!this.previousPosition) {
+            this.previousPosition = newPosition;
+            this.positionsQueue.push(this.previousPosition);
+            return;
+        }
+
+        const distance = this.previousPosition.distanceTo(newPosition);
+        const azimuth = this.previousPosition.bearingTo(newPosition);
+
+        let refTimestamp = newPosition.time;
+
+        const queueLength = this.positionsQueue.length;
+        if (queueLength) {
+            refTimestamp = Math.max(refTimestamp, this.positionsQueue[queueLength - 1].time);
+        }
+
+        let diffTime = newPosition.time - this.previousPosition.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 smoothedPosition = this.previousPosition.destinationPoint(distance * i / nSamples, azimuth);
+            smoothedPosition.time = refTimestamp + (i - 1) / GEN_FREQUENCY;
+            smoothedPosition.bearing = newPosition.bearing;
+            this.positionsQueue.push(smoothedPosition);
+            i++;
+        }
+
+        this.positionsQueue[this.positionsQueue.length - 1].level = newPosition.level;
+
+        this.previousPosition = newPosition;
+    }
+
+    /**
+     * Pull positions until timestamp and return the last one.
+     * @param {*} timestamp position returned is the last before this timestamp
+     * @returns last known position before timestamp, undefined otherwise
+     */
+    pullPosition(timestamp) {
+
+        if ((typeof timestamp === 'undefined' || !timestamp)
+            && this.positionsQueue.length > 0) {
+            const output = this.positionsQueue[this.positionsQueue.length - 1];
+            this.positionsQueue = [];
+            return output;
+        }
+
+        let position;
+        while (this.positionsQueue.length && this.positionsQueue[0].time <= timestamp) {
+            position = this.positionsQueue.shift();
+        }
+        return position;
+    }
+
+}
+
+export default Smoother;