RSDK-4434 Update get location wrapper (#160)

Author: maximpertsov

buf.lock

@@ -9,8 +9,8 @@ deps:
   - remote: buf.build
     owner: viamrobotics
     repository: api
-    commit: 567db2ef2dd04c548371f69ab31a1226
-    digest: shake256:a8bfb0b7ab720092320c8a302e89e65a4de510f8816ee18ae86673d157b70f2d3433c90dffce82ed7be2bc226781e41f3e558d9e89510a2ceccac001f7dc63cb
+    commit: 82d243f345744a249103db65f136f400
+    digest: shake256:4c885426d51826e69331f33f55ce02d6764509dc47b919790e36fad9401e2cf593e3a610506b4be144356fed045570e9933a8fcd3f3e704663ebfca5c33e7552
   - remote: buf.build
     owner: viamrobotics
     repository: goutils

src/main.ts

@@ -306,6 +306,7 @@ export { default as navigationApi } from './gen/service/navigation/v1/navigation
 export {
   type ModeMap,
   type Waypoint,
+  type NavigationPosition,
   NavigationClient,
 } from './services/navigation';
 

src/services/navigation.ts

@@ -1,3 +1,3 @@
 export type { Navigation } from './navigation/navigation';
-export type { ModeMap, Waypoint } from './navigation/types';
+export type { ModeMap, Waypoint, NavigationPosition } from './navigation/types';
 export { NavigationClient } from './navigation/client';

src/services/navigation/client.test.ts

@@ -0,0 +1,95 @@
+// @vitest-environment happy-dom
+
+import { type Mock, beforeEach, describe, expect, test, vi } from 'vitest';
+import { NavigationServiceClient } from '../../gen/service/navigation/v1/navigation_pb_service';
+vi.mock('../../gen/service/navigation/v1/navigation_pb_service');
+import { RobotClient } from '../../robot';
+vi.mock('../../robot');
+
+import { NavigationClient } from './client';
+
+const navigationClientName = 'test-navigation';
+
+let navigation: NavigationClient;
+
+beforeEach(() => {
+  RobotClient.prototype.createServiceClient = vi
+    .fn()
+    .mockImplementation(
+      () => new NavigationServiceClient(navigationClientName)
+    );
+
+  navigation = new NavigationClient(
+    new RobotClient('host'),
+    navigationClientName
+  );
+});
+
+const testLatitude = 50;
+const testLongitude = 75;
+const testCompassHeading = 90;
+
+describe('getLocation', () => {
+  let latitude: Mock<[], number>;
+  let longitude: Mock<[], number>;
+  let compassHeading: Mock<[], number>;
+  let location: Mock<[], { latitude: number; longitude: number }>;
+
+  beforeEach(() => {
+    location = vi.fn(() => ({
+      latitude: latitude(),
+      longitude: longitude(),
+    }));
+
+    NavigationServiceClient.prototype.getLocation = vi
+      .fn()
+      .mockImplementation((_req, _md, cb) => {
+        cb(null, {
+          toObject: () => ({
+            compassHeading: compassHeading(),
+            location: location(),
+          }),
+        });
+      });
+  });
+
+  test('null location', async () => {
+    location = vi.fn();
+    compassHeading = vi.fn();
+
+    await expect(navigation.getLocation()).rejects.toThrowError(
+      /^no location$/u
+    );
+
+    expect(location).toHaveBeenCalledOnce();
+    expect(compassHeading).toHaveBeenCalledOnce();
+  });
+
+  test('valid geopoint', async () => {
+    latitude = vi.fn(() => testLatitude);
+    longitude = vi.fn(() => testLongitude);
+    compassHeading = vi.fn(() => testCompassHeading);
+
+    const expected = {
+      location: { latitude: testLatitude, longitude: testLongitude },
+      compassHeading: testCompassHeading,
+    };
+
+    await expect(navigation.getLocation()).resolves.toStrictEqual(expected);
+
+    expect(location).toHaveBeenCalledOnce();
+    expect(compassHeading).toHaveBeenCalledOnce();
+  });
+
+  test('invalid geopoint', async () => {
+    latitude = vi.fn(() => Number.NaN);
+    longitude = vi.fn(() => Number.NaN);
+
+    await expect(navigation.getLocation()).rejects.toThrowError(
+      /^invalid location$/u
+    );
+
+    expect(location).toHaveBeenCalledOnce();
+    expect(compassHeading).toHaveBeenCalledOnce();
+  });
+});

src/services/navigation/client.ts

@@ -4,6 +4,7 @@ import { RobotClient } from '../../robot';
 import { NavigationServiceClient } from '../../gen/service/navigation/v1/navigation_pb_service';
 import { doCommandFromClient, encodeGeoPoint, promisify } from '../../utils';
 import type { GeoPoint, Options, StructType } from '../../types';
+import { isValidGeoPoint } from '../../types';
 import type { ModeMap } from './types';
 import type { Navigation } from './navigation';
 
@@ -74,11 +75,14 @@ export class NavigationClient implements Navigation {
       pb.GetLocationResponse
     >(service.getLocation.bind(service), request);
 
-    const result = response.getLocation();
-    if (!result) {
+    const result = response.toObject();
+    if (!result.location) {
       throw new Error('no location');
     }
-    return result.toObject();
+    if (!isValidGeoPoint(result.location)) {
+      throw new Error('invalid location');
+    }
+    return result;
   }
 
   async getWayPoints(extra = {}) {

src/services/navigation/navigation.ts

@@ -1,5 +1,5 @@
 import type { GeoObstacle, GeoPoint, Resource, StructType } from '../../types';
-import type { ModeMap, Waypoint } from './types';
+import type { ModeMap, Waypoint, NavigationPosition } from './types';
 
 /**
  * A service that uses GPS to automatically navigate a robot to user defined
@@ -17,10 +17,10 @@ export interface Navigation extends Resource {
   setMode: (mode: ModeMap[keyof ModeMap], extra?: StructType) => Promise<void>;
 
   /** Get the current location of the robot. */
-  getLocation: (extra?: StructType) => Promise<GeoPoint>;
+  getLocation: (extra?: StructType) => Promise<NavigationPosition>;
 
   /** Get an array of waypoints currently in the service's data storage. */
-  getWayPoints: (extra?: StructType) => Promise<Array<Waypoint>>;
+  getWayPoints: (extra?: StructType) => Promise<Waypoint[]>;
 
   /**
    * Add a waypoint to the service's data storage.
@@ -39,5 +39,5 @@ export interface Navigation extends Resource {
   removeWayPoint: (id: string, extra?: StructType) => Promise<void>;
 
   /** Get a list of obstacles. */
-  getObstacles: (extra?: StructType) => Promise<Array<GeoObstacle>>;
+  getObstacles: (extra?: StructType) => Promise<GeoObstacle[]>;
 }

src/services/navigation/types.ts

@@ -2,3 +2,4 @@ import pb from '../../gen/service/navigation/v1/navigation_pb';
 
 export type ModeMap = pb.ModeMap;
 export type Waypoint = pb.Waypoint.AsObject;
+export type NavigationPosition = pb.GetLocationResponse.AsObject;

src/types.ts

@@ -23,6 +23,17 @@ export type ResourceName = common.ResourceName.AsObject;
 export type GeoObstacle = common.GeoObstacle.AsObject;
 export type GeoPoint = common.GeoPoint.AsObject;
 
+export const isValidGeoPoint = (value: GeoPoint) => {
+  const { latitude, longitude } = value;
+
+  return !(
+    typeof latitude !== 'number' ||
+    typeof longitude !== 'number' ||
+    Number.isNaN(latitude) ||
+    Number.isNaN(longitude)
+  );
+};
+
 // Spatial Math
 export type Vector3 = common.Vector3.AsObject;
 export type Orientation = common.Orientation.AsObject;