@wemap/positioning 2.2.0 → 2.3.0

package/src/providers/pose/ArCoreAbsoluteProvider.js

@@ -0,0 +1,186 @@
+/* eslint-disable max-statements */
+import { Attitude } from '@wemap/geo';
+
+import Provider from '../Provider';
+import EventType from '../../events/EventType';
+import ArCoreProvider from './ArCoreProvider';
+import {
+    Quaternion, Vector3
+} from '@wemap/maths';
+import ImuProvider from '../others/ImuProvider';
+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 ArCoreAbsoluteProvider extends Provider {
+
+    /**
+    * @override
+    */
+    constructor(onEvent, onError, options) {
+        super(onEvent, onError, options);
+
+        this.arCoreProvider = new ArCoreProvider(
+            e => this.onArCoreEvent(e),
+            e => this.onArCoreError(e),
+            options);
+
+        this.imuProvider = new ImuProvider(
+            this.onImuEvent,
+            this.onImuError,
+            Object.assign(options || {}, { require: [EventType.AngularRate] })
+        );
+    }
+
+    /**
+    * @override
+    */
+    static get displayName() {
+        return 'ArCore absolute provider';
+    }
+
+    /**
+     * @override
+     */
+    static get eventsType() {
+        return [EventType.AbsoluteAttitude, EventType.AbsolutePosition];
+    }
+
+    /**
+     * Return the list of required providers
+     */
+    static get requiredProviders() {
+        return [ArCoreProvider, ImuProvider];
+    }
+
+    /**
+     * @override
+     */
+    startInternal() {
+        this.arCoreProvider.start();
+        this.imuProvider.start();
+    }
+
+    /**
+     * @override
+     */
+    stopInternal() {
+        this.arCoreProvider.stop();
+        this.imuProvider.stop();
+    }
+
+    /**
+     * @override
+     */
+    static checkAvailabilityErrors() {
+        return ArCoreProvider.checkAvailabilityErrors().concat(
+            ImuProvider.checkAvailabilityErrors()
+        );
+    }
+
+    onArCoreEvent = events => {
+        events.forEach(event => {
+            if (event.dataType === EventType.RelativeAttitude) {
+
+                // const previousAttitude = this.relativeAttitude;
+                this.relativeAttitude = event.data;
+
+                /**
+                 * The two following blocks cannot be called in the same call because this.waitingForceHeading
+                 * if exists, is called before the first this.relativeAttitude assigment. So, this.waitingForceHeading
+                 * and previousAttitude should not be defined in the same time.
+                 */
+
+                if (typeof this.waitingForceHeading !== 'undefined') {
+                    /**
+                     * waitingForceHeading is set by setHeading() if this.relativeAttitude does not exist
+                     * At this moment, this.relativeAttitude is defined, so we can call setHeading again
+                     */
+                    this.setHeading(this.waitingForceHeading);
+                    delete this.waitingForceHeading;
+
+                }
+
+                // if (previousAttitude && this.absoluteAttitude) {
+                //     /**
+                //      * Sometimes ArCore has "orientation jumps" around z-axis.
+                //      * Here we try to detect and remove them.
+                //      */
+                //     const dist = Quaternion.distance(event.data.quaternion, previousAttitude.quaternion);
+                //     if (typeof this.angularRate !== 'undefined') {
+                //         const velocity = Vector3.norm(this.angularRate);
+                //         if (dist > deg2rad(10) && velocity < deg2rad(4)) {
+                //             this.setHeading(this.absoluteAttitude.heading);
+                //         }
+                //     }
+                // }
+
+                if (this.offsetQuat) {
+                    const absoluteQuaternion = Quaternion.multiply(this.offsetQuat, this.relativeAttitude.quaternion);
+                    this.absoluteAttitude = new Attitude(absoluteQuaternion);
+                    this.notify(this.createEvent(EventType.AbsoluteAttitude, this.absoluteAttitude, event.timestamp));
+                }
+            }
+
+            if (event.dataType === EventType.RelativePosition) {
+
+                if (this.oldRelativePos && this.offsetAngle
+                    && !ArCoreAbsoluteProvider.positionEquals(this.oldRelativePos, event.data)) {
+
+                    const diffPos = Vector3.subtract(event.data, this.oldRelativePos);
+                    const dist = Math.sqrt(diffPos[0] ** 2 + diffPos[2] ** 2);
+                    const bearing = Math.atan2(diffPos[0], -diffPos[2]) - this.offsetAngle;
+                    this.position = this.position.destinationPoint(dist, bearing, diffPos[1]);
+                    this.notify(this.createEvent(EventType.AbsolutePosition, this.position, event.timestamp));
+                }
+
+                this.oldRelativePos = event.data;
+            }
+        });
+    }
+
+    static positionEquals(pos1, pos2) {
+        return pos1[0] === pos2[0] && pos1[1] === pos2[1] && pos1[2] === pos2[2];
+    }
+
+    onArCoreError = arCoreErrors => {
+        this.notifyError(...(
+            ProviderError.modifyArrayDataType(arCoreErrors, EventType.AbsoluteAttitude).concat(
+                ProviderError.modifyArrayDataType(arCoreErrors, EventType.AbsolutePosition)
+            ))
+        );
+    };
+
+    onImuEvent = events => {
+        this.angularRate = events[0].data;
+    }
+
+    onImuError = imuErrors => {
+        this.notifyError(...(
+            ProviderError.modifyArrayDataType(imuErrors, EventType.AbsoluteAttitude).concat(
+                ProviderError.modifyArrayDataType(imuErrors, EventType.AbsolutePosition)
+            ))
+        );
+    }
+
+    setHeading(heading) {
+
+        if (!this.relativeAttitude) {
+            this.waitingForceHeading = heading;
+            return;
+        }
+
+        this.offsetAngle = this.relativeAttitude.heading - heading;
+        this.offsetQuat = Quaternion.fromAxisAngle([0, 0, 1], this.offsetAngle);
+    }
+
+    setPosition(position) {
+        this.position = position.clone();
+        this.position.provider = this.constructor.name;
+        this.notify(this.createEvent(EventType.AbsolutePosition, this.position));
+    }
+}
+
+export default ArCoreAbsoluteProvider;

package/src/providers/pose/ArCoreProvider.js

@@ -3,6 +3,7 @@ import { Attitude } from '@wemap/geo';
 import Provider from '../Provider';
 import EventType from '../../events/EventType';
 import MissingArCoreError from '../../errors/MissingArCoreError';
+import MissingNativeInterfaceError from '../../errors/MissingNativeInterfaceError';
 
 /**
  * Pose provider is the provider used by the PositioningHandler. It uses the best fusion
@@ -10,14 +11,6 @@ import MissingArCoreError from '../../errors/MissingArCoreError';
  */
 class ArCoreProvider extends Provider {
 
-    /**
-    * @override
-    */
-    constructor(onEvent, onError, options) {
-        super(onEvent, onError, options);
-
-    }
-
     /**
     * @override
     */
@@ -43,12 +36,9 @@ class ArCoreProvider extends Provider {
      * @override
      */
     startInternal() {
-        const nativeProvider = this.constructor.getNativeProvider();
-        if (Array.isArray(nativeProvider)) {
-            this.notifyError(nativeProvider);
-            return;
+        if (ArCoreProvider.nativeProvider) {
+            ArCoreProvider.nativeProvider.start();
         }
-        nativeProvider.start();
 
         this.pullDataLoop();
     }
@@ -57,12 +47,9 @@ class ArCoreProvider extends Provider {
      * @override
      */
     stopInternal() {
-        const nativeProvider = this.constructor.getNativeProvider();
-        if (Array.isArray(nativeProvider)) {
-            this.notifyError(nativeProvider);
-            return;
+        if (ArCoreProvider.nativeProvider) {
+            ArCoreProvider.nativeProvider.stop();
         }
-        nativeProvider.stop();
     }
 
     pullDataLoop = () => {
@@ -71,13 +58,7 @@ class ArCoreProvider extends Provider {
             return;
         }
 
-        const nativeProvider = this.constructor.getNativeProvider();
-        if (Array.isArray(nativeProvider)) {
-            this.notifyError(...nativeProvider);
-            return;
-        }
-
-        const pose = JSON.parse(nativeProvider.getInfo());
+        const pose = JSON.parse(ArCoreProvider.nativeProvider.getInfo());
         if (pose.length !== 0) {
             const attitude = new Attitude(pose.slice(0, 4));
             const position = pose.slice(4, 7);
@@ -90,17 +71,22 @@ class ArCoreProvider extends Provider {
         requestAnimationFrame(this.pullDataLoop);
     }
 
-    static getNativeProvider() {
-        const nativeInterface = this.getNativeInterface();
+    static get nativeProvider() {
+
+        if (!this._nativeProvider) {
+
+            if (!this.nativeInterface) {
+                throw new MissingNativeInterfaceError();
+            }
+
+            this._nativeProvider = this.nativeInterface.getArCoreProvider();
+            if (!this._nativeProvider) {
+                throw new MissingArCoreError();
+            }
 
-        if (!nativeInterface) {
-            return [
-                this.createMissingNativeInterfaceError(EventType.RelativeAttitude),
-                this.createMissingNativeInterfaceError(EventType.RelativePosition)
-            ];
         }
 
-        return nativeInterface.getArCoreProvider();
+        return this._nativeProvider;
     }
 
     /**
@@ -108,19 +94,21 @@ class ArCoreProvider extends Provider {
      */
     static checkAvailabilityErrors() {
 
-        const nativeProvider = this.getNativeProvider();
-        if (Array.isArray(nativeProvider)) {
-            return nativeProvider;
-        }
+        try {
+            const nativeProvider = this.nativeProvider;
+
+            if (!nativeProvider.checkAvailability()) {
+                throw new MissingArCoreError();
+            }
 
-        if (nativeProvider.checkAvailability()) {
-            return [];
+        } catch (e) {
+            return [
+                this.createError(EventType.RelativeAttitude, e),
+                this.createError(EventType.RelativePosition, e)
+            ];
         }
 
-        return [
-            this.createError(EventType.RelativeAttitude, new MissingArCoreError()),
-            this.createError(EventType.RelativePosition, new MissingArCoreError())
-        ];
+        return [];
     }
 }
 

package/src/providers/pose/GnssWifiPdrProvider.js

@@ -84,7 +84,7 @@ class GnssWifiPdrProvider extends MapMatchingProvider {
     onPdrEvent(pdrEvent) {
         pdrEvent.forEach(event => {
             if (event.dataType === EventType.AbsolutePosition) {
-                this.location = event.data;
+                this.position = event.data;
             } else if (event.dataType === EventType.AbsoluteAttitude) {
                 this.attitude = event.data;
             }
@@ -98,39 +98,39 @@ class GnssWifiPdrProvider extends MapMatchingProvider {
     onGnssWifiEvent(events) {
 
         const gnssWifiEvent = events[0];
-        const location = gnssWifiEvent.data;
+        const position = gnssWifiEvent.data;
 
         // This should be called to update True North / Magnetic North declination
-        this.absoluteAttitudeProvider.setLocation(location);
+        this.absoluteAttitudeProvider.setPosition(position);
 
-        if (location.accuracy > GPF_ACCURACY) {
+        if (position.accuracy > GPF_ACCURACY) {
             return;
         }
 
-        this.gnssLocation = location.clone();
-        this.gnssLocation.alt = Constants.DEFAULT_ALTITUDE;
+        this.gnssPosition = position.clone();
+        this.gnssPosition.alt = Constants.DEFAULT_ALTITUDE;
 
-        if (!this.location || this.location
-            && this.location.distanceTo(this.gnssLocation) > GPF_DISTANCE) {
+        if (!this.position || this.position
+            && this.position.distanceTo(this.gnssPosition) > GPF_DISTANCE) {
 
             if (!this.mapMatching || !this.attitude) {
-                this.pdrProvider.setLocation(this.gnssLocation);
+                this.pdrProvider.setPosition(this.gnssPosition);
             } else {
 
-                this.gnssLocation.bearing = this.attitude.headingDegrees;
-                const projection = this.mapMatching.getProjection(this.gnssLocation);
+                this.gnssPosition.bearing = this.attitude.headingDegrees;
+                const projection = this.mapMatching.getProjection(this.gnssPosition);
 
                 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);
+                    // Create a new position from projection and new GNSS position.
+                    const projectedPosition = WGS84UserPosition.fromWGS84(projection.projection, this.gnssPosition);
+                    this.pdrProvider.setPosition(projectedPosition);
 
                     // // 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);
+                    //     const diff1 = MathUtils.diffAngle(MathUtils.deg2rad(this.gnssPosition.bearing), edgeBearing);
+                    //     const diff2 = MathUtils.diffAngle(MathUtils.deg2rad(this.gnssPosition.bearing), edgeBearing + Math.PI);
                     //     this.pdrProvider.setHeading(diff1 < diff2 ? edgeBearing : edgeBearing + Math.PI);
                     // }
 
@@ -139,7 +139,7 @@ class GnssWifiPdrProvider extends MapMatchingProvider {
                     }
 
                 } else {
-                    this.pdrProvider.setLocation(this.gnssLocation);
+                    this.pdrProvider.setPosition(this.gnssPosition);
                 }
             }
         }
@@ -208,9 +208,9 @@ class GnssWifiPdrProvider extends MapMatchingProvider {
 
             // 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
+            if (!this.gnssPosition
                 || itinerary.length < 2
-                || !itinerary.points[0].equalsTo(this.gnssLocation)) {
+                || !itinerary.points[0].equalsTo(this.gnssPosition)) {
                 console.warn('Itinerary has not been calculated from GnssWifiPdrProvider and these is not recommanded');
             }
 
@@ -222,7 +222,7 @@ class GnssWifiPdrProvider extends MapMatchingProvider {
             }
             const startPoint = WGS84UserPosition.fromWGS84(startEdge.node1);
             startPoint.alt = Constants.DEFAULT_ALTITUDE;
-            this.pdrProvider.setLocation(startPoint);
+            this.pdrProvider.setPosition(startPoint);
             this.pdrProvider.setStepDetectionLockerOrientation(startEdge.getBearing());
         }
 

package/src/providers/pose/PoseProvider.js

@@ -19,7 +19,7 @@ class PoseProvider extends Provider {
 
         this.absoluteAttitudeProvider = new AbsoluteAttitudeProvider(onEvent, e => this.onAbsoluteAttitudeError(e));
         this.gnssWifiProvider = new GnssWifiProvider((events) => {
-            this.absoluteAttitudeProvider.setLocation(events[0].data);
+            this.absoluteAttitudeProvider.setPosition(events[0].data);
             onEvent(events);
         }, e => this.onGnssWifiError(e));
     }

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

@@ -32,10 +32,10 @@ const DEFAULT_OPTIONS = {
 
 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;
+    // pdrPosition can be different from this.position
+    // pdrPosition is raw position calculated by PdrProvider where
+    //     this.position is smoothed position.
+    pdrPosition = null;
 
     /**
     * @override
@@ -127,18 +127,18 @@ class PdrProvider extends MapMatchingProvider {
         this.relativeAttitudeProvider.setOffset(heading);
     }
 
-    setLocation(location) {
-        this.pdrLocation = location;
+    setPosition(position) {
+        this.pdrPosition = position;
 
         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);
+            this.smoother.generateNextPositions(position, true);
+            // this.smoother.pullPosition(position.time) should never return null
+            this.position = this.smoother.pullPosition(position.time);
         } else {
-            this.location = location.clone();
+            this.position = position.clone();
         }
 
-        this.notify(this.createEvent(EventType.AbsolutePosition, this.location));
+        this.notify(this.createEvent(EventType.AbsolutePosition, this.position));
     }
 
     /**
@@ -163,8 +163,8 @@ class PdrProvider extends MapMatchingProvider {
             return;
         }
 
-        if (!this.pdrLocation) {
-            // We should wait at least an input location for PDR
+        if (!this.pdrPosition) {
+            // We should wait at least an input position for PDR
             return;
         }
 
@@ -201,36 +201,36 @@ class PdrProvider extends MapMatchingProvider {
 
         if (stepDetected) {
 
-            this.pdrLocation = this.calculateNewLocation(this.pdrLocation, timestamp,
+            this.pdrPosition = this.calculateNewPosition(this.pdrPosition, 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
+                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 (newLocation) {
-                    this.location = newLocation;
+                if (newPosition) {
+                    this.position = newPosition;
                 }
             } else {
-                this.location = this.pdrLocation;
+                this.position = this.pdrPosition;
             }
 
         } 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) {
+            // 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.location = smoothedLocation;
+            this.position = smoothedPosition;
         } else {
             return;
         }
 
-        this.notify(this.createEvent(EventType.AbsolutePosition, this.location));
+        this.notify(this.createEvent(EventType.AbsolutePosition, this.position));
     }
 
     onProviderError(errors) {
@@ -254,39 +254,39 @@ class PdrProvider extends MapMatchingProvider {
         );
     }
 
-    calculateNewLocation(previousLocation, timestamp, heading, stepSize) {
+    calculateNewPosition(previousPosition, timestamp, heading, stepSize) {
 
         /**
          * Compute new_position
          */
-        const newLocationWithoutMM = previousLocation.clone();
-        newLocationWithoutMM.move(stepSize, heading);
-        newLocationWithoutMM.bearing = rad2deg(heading);
-        newLocationWithoutMM.time = timestamp;
+        const newPositionWithoutMM = previousPosition.clone();
+        newPositionWithoutMM.move(stepSize, heading);
+        newPositionWithoutMM.bearing = rad2deg(heading);
+        newPositionWithoutMM.time = timestamp;
 
         if (!this.mapMatching) {
-            return newLocationWithoutMM;
+            return newPositionWithoutMM;
         }
 
-        const projection = this.mapMatching.getProjection(newLocationWithoutMM);
+        const projection = this.mapMatching.getProjection(newPositionWithoutMM);
 
         if (!projection || !projection.projection) {
-            return newLocationWithoutMM;
+            return newPositionWithoutMM;
         }
 
         if (projection.distanceFromNearestElement < stepSize) {
-            return WGS84UserPosition.fromWGS84(projection.projection, newLocationWithoutMM);
+            return WGS84UserPosition.fromWGS84(projection.projection, newPositionWithoutMM);
         }
 
         /**
          * Update new_position
          */
         const smoothedDistance = projection.distanceFromNearestElement * MM_CONV_SPEED;
-        const smoothedBearing = previousLocation.bearingTo(projection.projection);
-        const smoothedLocation = previousLocation.clone();
-        smoothedLocation.move(smoothedDistance, smoothedBearing);
+        const smoothedBearing = previousPosition.bearingTo(projection.projection);
+        const smoothedPosition = previousPosition.clone();
+        smoothedPosition.move(smoothedDistance, smoothedBearing);
 
-        return WGS84UserPosition.fromWGS84(smoothedLocation, newLocationWithoutMM);
+        return WGS84UserPosition.fromWGS84(smoothedPosition, newPositionWithoutMM);
     }