@wemap/positioning 1.2.0

package/src/providers/PdrLocationSource.js

@@ -0,0 +1,312 @@
+import noop from 'lodash.noop';
+
+import {
+    Itinerary, WGS84UserPosition
+} from '@wemap/geo';
+import { Rotations, Utils as MathUtils } from '@wemap/maths';
+
+import LocationSource from './LocationSource';
+import AttitudeHandler from '../attitude/AttitudeHandler';
+import Constants from '../Constants';
+import SensorsLogger from '../sensors/SensorsLogger';
+import SensorsCompatibility from '../sensors/SensorsCompatibility';
+import StepDetection from './steps/StepDetection';
+import HeadingUnlocker from './pdr/HeadingUnlocker';
+import ThugDetector from './pdr/ThugDetector';
+import Smoother from './pdr/Smoother';
+
+
+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 PdrLocationSource extends LocationSource {
+
+    // pdrLocation can be different from this.pose.location
+    // pdrLocation is raw location calculated by PdrLocationSource where
+    //     this.pose.location is smoothed location.
+    pdrLocation = null;
+
+    constructor(callback, options = DEFAULT_OPTIONS) {
+        super(callback);
+
+        // Assign params
+        this.callback = callback;
+        this.stepDetectionLockerEnabled = options.stepdetectionlocker !== null ? options.stepdetectionlocker : DEFAULT_OPTIONS.stepdetectionlocker;
+        this.smootherEnabled = options.smoother !== null ? options.smoother : DEFAULT_OPTIONS.smoother;
+        this.onThugEvent = options.onThugEvent !== null ? options.onThugEvent : DEFAULT_OPTIONS.onThugEvent;
+
+        // Input data providers
+        this.attitudeHandler = new AttitudeHandler();
+        this.sensorsCompatibility = new SensorsCompatibility();
+
+        // Helpers for PDR
+        this.stepDetection = new StepDetection();
+        this.stepDetectionLocker = new HeadingUnlocker();
+        this.smoother = new Smoother();
+        this.thugDetector = new ThugDetector();
+    }
+
+    /**
+     * Start the location source algorithm
+     * @override
+     */
+    start() {
+        super.start();
+
+        return Promise.all([
+            this.sensorsCompatibility.startImu(this.onNewMotion, {
+                accelerometer: true,
+                gyroscope: true
+            }),
+            this.attitudeHandler.startRelative(this.onNewAttitude, AttitudeHandler.RelativeMethod.INTERNAL_EKF)
+        ]);
+    }
+
+    /**
+     * Stop the location source algorithm
+     * @override
+     */
+    stop() {
+        super.stop();
+
+        this.attitudeHandler.stopRelative();
+        this.sensorsCompatibility.stopImu();
+    }
+
+    /**
+     * Get provider name
+     * @override
+     */
+    get providerNameWithOptions() {
+        let name = this.providerName;
+        name += ' (locker: ';
+        name += this.stepDetectionLockerEnabled ? 'Y' : 'N';
+        name += ', ';
+        name += 'smoo: ';
+        name += this.smootherEnabled ? 'Y' : 'N';
+        name += ', ';
+        name += 'mm: ';
+        name += this.mapMatching ? 'Y' : 'N';
+        name += ')';
+        return name;
+    }
+
+    /**
+     * Get provider name
+     * @override
+     */
+    static get providerName() {
+        return Constants.LOCATION_SOURCE_PROVIDERS.PDR;
+    }
+
+    /**
+     * @override
+     */
+    notify() {
+        if (!this.pose.attitude) {
+            return;
+        }
+        super.notify();
+    }
+
+    setLogger(logger) {
+        super.setLogger(logger);
+        this.sensorsCompatibility.setLogger(logger);
+        this.attitudeHandler.setLogger(logger);
+    }
+
+    setHeading(heading) {
+        this.attitudeHandler.setRelativeHeading(heading);
+        if (this.logger) {
+            this.logger.feedWithCurrentTime(SensorsLogger.DataType.USER_INPUT_HEADING, heading);
+        }
+    }
+
+    setLocation(location) {
+        this.pdrLocation = location;
+
+        if (this.smootherEnabled) {
+            this.smoother.generateNextLocations(location, true);
+            // this.smoother.pullLocation(location.time) should never return null
+            this.pose.location = this.smoother.pullLocation(location.time);
+        } else {
+            this.pose.location = location;
+        }
+
+        this.notify();
+
+        if (this.logger) {
+            const data = [location.lat, location.lng];
+            if (location.altitude) {
+                data.push(location.altitude);
+            }
+            this.logger.feedWithCurrentTime(SensorsLogger.DataType.USER_INPUT_POSITION, data);
+        }
+    }
+
+    onNewAttitude = (attitude) => {
+        this.pose.attitude = attitude;
+    }
+
+    onNewMotion = (motionEvent) => {
+
+        if (!this.pose.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.stepDetectionLockerEnabled
+            && this.stepDetectionLocker.locked
+            && this.stepDetectionLocker.feedHeading(this.pose.attitude.heading)) {
+            // Step detection is locked by stepDetectionLocker
+            // We still need to notify for attitude
+            this.notify();
+            return;
+        }
+
+        const timestamp = motionEvent.timestamp;
+        const heading = this.pose.attitude.heading;
+        const acceleration = motionEvent.acc;
+
+        if (this.onThugEvent && this.thugDetector.compute(timestamp, acceleration)) {
+            this.onThugEvent();
+        }
+
+        /**
+         * Step Detection and Step Size Detection
+         */
+        const linearAcc = Rotations.computeLinearAcceleration(
+            this.pose.attitude.quaternion, acceleration);
+        const stepDetected = this.stepDetection.compute(timestamp, linearAcc, motionEvent.gyr);
+
+        if (stepDetected) {
+
+            this.pdrLocation = this.calculateNewLocation(this.pdrLocation, timestamp,
+                heading, this.stepDetection.lastStepSize);
+
+            if (this.smootherEnabled) {
+                this.smoother.generateNextLocations(this.pdrLocation);
+            }
+
+            /**
+             * Update current_position
+             */
+            this.pose.location = this.smootherEnabled
+                ? this.smoother.pullLocation(timestamp)
+                : this.pdrLocation;
+
+        } else if (this.smootherEnabled) {
+            // If no step is detected, we pull last known location from smoother until now (timestamp).
+            const smoothedLocation = this.smoother.pullLocation(timestamp);
+            if (smoothedLocation) {
+                this.pose.location = smoothedLocation;
+            }
+        }
+
+        this.notify();
+    }
+
+    calculateNewLocation(previousLocation, timestamp, heading, stepSize) {
+
+        /**
+         * Compute new_position
+         */
+        const newLocationWithoutMM = previousLocation.clone();
+        newLocationWithoutMM.move(stepSize, heading);
+        newLocationWithoutMM.bearing = MathUtils.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 = MM_PDR_DIST, maxAngleBearing = MM_PDR_ANGLE) {
+        super.enableMapMatching(network, maxDistance, maxAngleBearing);
+    }
+
+
+    /**
+     * Itinerary and PDR Locker
+     */
+
+    setItinerary(itinerary) {
+        super.setItinerary(itinerary);
+
+        if (this.stepDetectionLockerEnabled) {
+            this.stepDetectionLocker.setWaitingOrientation(
+                PdrLocationSource.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);
+    }
+
+}
+
+export default PdrLocationSource;

package/src/providers/ProvidersLogger.js

@@ -0,0 +1,77 @@
+import Logger from '@wemap/logger';
+
+let currentId = 0;
+const objectsIdMap = new WeakMap();
+
+let pushEvents = {};
+let pushEventsRef = {};
+
+let interval;
+let initDate;
+
+class ProvidersLogger {
+
+    static initializeInterval() {
+
+        if (interval) {
+            return;
+        }
+
+        interval = setInterval(() => {
+
+            for (const [key, value] of Object.entries(pushEvents)) {
+                Logger.debug('Received ' + value + ' values from ' + pushEventsRef[key].constructor.name + ' last second');
+            }
+
+            pushEvents = {};
+            pushEventsRef = {};
+        }, 1000);
+    }
+
+    static getObjectId(object) {
+        if (!objectsIdMap.has(object)) {
+            objectsIdMap.set(object, ++currentId);
+        }
+        return objectsIdMap.get(object);
+    }
+
+    static addEvent(object, method) {
+
+        if (!ProvidersLogger.enabled) {
+            return;
+        }
+
+        if (!initDate) {
+            initDate = Date.now();
+        }
+
+        ProvidersLogger.initializeInterval();
+
+        const objectId = ProvidersLogger.getObjectId(object);
+        const objectClassName = object.constructor.name;
+
+        Logger.debug(objectClassName + '[' + objectId + '].' + method);
+    }
+
+    static addPushEvent(object) {
+
+        if (!ProvidersLogger.enabled) {
+            return;
+        }
+
+        const objectId = ProvidersLogger.getObjectId(object);
+
+        let counter = pushEvents[objectId];
+        if (!counter) {
+            counter = 0;
+            pushEventsRef[objectId] = object;
+        }
+        pushEvents[objectId] = counter + 1;
+    }
+
+    static get enabled() {
+        return Logger.LOG_LEVEL === Logger.DEBUG;
+    }
+
+}
+export default ProvidersLogger;

package/src/providers/pdr/HeadingUnlocker.js

@@ -0,0 +1,41 @@
+import isnumber from 'lodash.isnumber';
+
+import {Utils as MathUtils} 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(MathUtils.diffAngle(heading, this.waitingOrientation)) < LO_ANGLE) {
+            this.locked = false;
+        }
+
+        return this.locked;
+    }
+
+}
+
+export default HeadingUnlocker;

package/src/providers/pdr/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/pdr/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;