@wemap/positioning 2.0.0 → 2.2.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.old/providers/PdrLocationSource.js → src/providers/pose/pdr/PdrProvider.js}

@@ -7,15 +7,15 @@ import {
     rad2deg, Quaternion
 } 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';
+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;
@@ -30,138 +30,135 @@ const DEFAULT_OPTIONS = {
     onThugEvent: noop
 };
 
-class PdrLocationSource extends LocationSource {
+class PdrProvider extends MapMatchingProvider {
 
-    // pdrLocation can be different from this.pose.location
-    // pdrLocation is raw location calculated by PdrLocationSource where
-    //     this.pose.location is smoothed location.
+    // pdrLocation can be different from this.location
+    // pdrLocation is raw location calculated by PdrProvider where
+    //     this.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;
+    /**
+    * @override
+    */
+    constructor(onEvent, onError, options) {
+        super(onEvent, onError, Object.assign(DEFAULT_OPTIONS, options || {}));
 
         // Input data providers
-        this.attitudeHandler = new AttitudeHandler();
-        this.sensorsCompatibility = new SensorsCompatibility();
+        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 (this.options.useMapMatching) {
+            this.enableMapMatching();
+        }
     }
 
+
     /**
-     * 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)
-        ]);
+    * @override
+    */
+    static get displayName() {
+        return 'PDR';
     }
 
     /**
-     * Stop the location source algorithm
      * @override
      */
-    stop() {
-        super.stop();
-
-        this.attitudeHandler.stopRelative();
-        this.sensorsCompatibility.stopImu();
+    static get eventsType() {
+        return [EventType.AbsoluteAttitude, EventType.AbsolutePosition];
     }
 
     /**
-     * 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;
+    static get requiredProviders() {
+        return [ImuProvider, RelativeAttitudeProvider];
     }
 
+
     /**
-     * Get provider name
      * @override
      */
-    static get providerName() {
-        return Constants.LOCATION_SOURCE_PROVIDERS.PDR;
+    startInternal() {
+        this.imuProvider.start();
+        this.relativeAttitudeProvider.start();
     }
 
     /**
      * @override
      */
-    notify() {
-        if (!this.pose.attitude) {
-            return;
-        }
-        super.notify();
+    stopInternal() {
+        this.imuProvider.stop();
+        this.relativeAttitudeProvider.stop();
     }
 
-    setLogger(logger) {
-        super.setLogger(logger);
-        this.sensorsCompatibility.setLogger(logger);
-        this.attitudeHandler.setLogger(logger);
+    /**
+     * 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.attitudeHandler.setRelativeHeading(heading);
-        if (this.logger) {
-            this.logger.feedWithCurrentTime(SensorsLogger.DataType.USER_INPUT_HEADING, heading);
-        }
+        this.relativeAttitudeProvider.setOffset(heading);
     }
 
     setLocation(location) {
         this.pdrLocation = location;
 
-        if (this.smootherEnabled) {
+        if (this.options.smoother) {
             this.smoother.generateNextLocations(location, true);
             // this.smoother.pullLocation(location.time) should never return null
-            this.pose.location = this.smoother.pullLocation(location.time);
+            this.location = this.smoother.pullLocation(location.time);
         } else {
-            this.pose.location = location;
+            this.location = location.clone();
         }
 
-        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);
-        }
+        this.notify(this.createEvent(EventType.AbsolutePosition, this.location));
     }
 
-    onNewAttitude = (attitude) => {
-        this.pose.attitude = attitude;
+    /**
+     * @override
+     */
+    onRelativeAttitudeEvent = events => {
+        const attitudeEvent = events[0];
+        this.attitude = attitudeEvent.data;
+
+        const newEvent = attitudeEvent.clone();
+        newEvent.dataType = EventType.AbsoluteAttitude;
+        this.notify(newEvent);
     }
 
-    onNewMotion = (motionEvent) => {
+    /**
+     * @private
+     */
+    onImuEvent = imuEvent => {
 
-        if (!this.pose.attitude) {
+        if (!this.attitude) {
             // We should wait attitude for stepDetectionLocker and linear acceleration
             return;
         }
@@ -171,18 +168,25 @@ class PdrLocationSource extends LocationSource {
             return;
         }
 
-        if (this.stepDetectionLockerEnabled
+        if (this.options.stepdetectionlocker
             && this.stepDetectionLocker.locked
-            && this.stepDetectionLocker.feedHeading(this.pose.attitude.heading)) {
+            && this.stepDetectionLocker.feedHeading(this.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;
+
+        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();
@@ -191,35 +195,63 @@ class PdrLocationSource extends LocationSource {
         /**
          * Step Detection and Step Size Detection
          */
-        const linearAcc = PdrLocationSource.computeLinearAcceleration(
-            this.pose.attitude.quaternion, acceleration);
-        const stepDetected = this.stepDetection.compute(timestamp, linearAcc, motionEvent.gyr);
+        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);
 
-            if (this.smootherEnabled) {
+            /**
+            * 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;
             }
 
-            /**
-             * Update current_position
-             */
-            this.pose.location = this.smootherEnabled
-                ? this.smoother.pullLocation(timestamp)
-                : this.pdrLocation;
-
-        } else if (this.smootherEnabled) {
+        } 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) {
-                this.pose.location = smoothedLocation;
+            if (!smoothedLocation) {
+                return;
             }
+            this.location = smoothedLocation;
+        } else {
+            return;
         }
 
-        this.notify();
+        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) {
@@ -262,8 +294,8 @@ class PdrLocationSource extends LocationSource {
      * MapMatching
      */
 
-    enableMapMatching(network = [], maxDistance = MM_PDR_DIST, maxAngleBearing = MM_PDR_ANGLE) {
-        super.enableMapMatching(network, maxDistance, maxAngleBearing);
+    enableMapMatching(network, maxDistance, maxAngleBearing) {
+        super.enableMapMatching(network, maxDistance || MM_PDR_DIST, maxAngleBearing || MM_PDR_ANGLE);
     }
 
 
@@ -274,9 +306,9 @@ class PdrLocationSource extends LocationSource {
     setItinerary(itinerary) {
         super.setItinerary(itinerary);
 
-        if (this.stepDetectionLockerEnabled) {
+        if (this.options.stepdetectionlocker) {
             this.stepDetectionLocker.setWaitingOrientation(
-                PdrLocationSource.retrieveWaitingOrientationFromItinerary(itinerary)
+                PdrProvider.retrieveWaitingOrientationFromItinerary(itinerary)
             );
         }
     }
@@ -317,4 +349,4 @@ class PdrLocationSource extends LocationSource {
     }
 }
 
-export default PdrLocationSource;
+export default PdrProvider;