[RSDK 4318] - Obstacle avoidance bugfixes (viamrobotics#2741)

etc/configs/fake.json

@@ -140,6 +140,28 @@
                 "movement_sensor": "movement_sensor1",
                 "base": "base1",
                 "obstacles": [
+                    {
+                        "location": {
+                            "latitude": 40.6705209,
+                            "longitude": -73.9659182
+                        },
+                        "geometries": [
+                            {
+                                "r": 2000
+                            }
+                        ]
+                    },
+                    {
+                        "location": {
+                            "longitude": -73.976472,
+                            "latitude": 40.693268
+                        },
+                        "geometries": [
+                            {
+                                "r": 2000000
+                            }
+                        ]
+                    },
                     {
                         "geometries": [
                           {

web/frontend/package.json

@@ -1,6 +1,6 @@
 {
   "name": "@viamrobotics/remote-control",
-  "version": "2.0.11",
+  "version": "2.0.12",
   "license": "Apache-2.0",
   "type": "module",
   "files": [
@@ -52,6 +52,7 @@
     "postcss": "8.4.24",
     "svelte": "^4.0.0",
     "svelte-check": "^3.4.4",
+    "svelte-inview": "^4.0.1",
     "tailwindcss": "3.3.2",
     "three": "0.152.2",
     "three-inspect": "0.3.4",

web/frontend/src/api/navigation.ts

@@ -198,6 +198,10 @@ export const getLocation = async (robotClient: Client, name: string) => {
   const lat = location?.getLatitude();
   const lng = location?.getLongitude();
 
+  rcLogConditionally(response?.getLocation()?.toObject());
+  rcLogConditionally(location?.getLatitude());
+  rcLogConditionally(location?.getLongitude());
+
   if (typeof lat !== 'number' || typeof lng !== 'number') {
     // eslint-disable-next-line unicorn/prefer-type-error
     throw new Error('Unable to locate robot');

web/frontend/src/components/base/index.svelte

@@ -80,6 +80,7 @@
 
   const stop = async () => {
     stopped = true;
+
     try {
       await baseClient.stop();
     } catch (error) {
@@ -200,17 +201,17 @@
     disableViews = liveCameras > 1;
   };
 
-  const handleVisibilityChange = () => {
-    // only issue the stop if there are keys pressed as to not interfere with commands issued by scripts/commands
-    if (document.visibilityState === 'hidden' && pressed.size > 0) {
+  const handleOnBlur = () => {
+    if (pressed.size <= 0) {
       pressed.clear();
       stop();
     }
   };
 
-  const handleOnBlur = () => {
-    if (pressed.size <= 0) {
-      stop();
+  const handleVisibilityChange = () => {
+    // only issue the stop if there are keys pressed as to not interfere with commands issued by scripts/commands
+    if (document.visibilityState === 'hidden') {
+      handleOnBlur();
     }
   };
 

web/frontend/src/components/motor/index.svelte

@@ -54,15 +54,63 @@ const setMovementType = (event: CustomEvent) => {
 };
 
 const setPosition = (event: CustomEvent) => {
-  position = event.detail.value;
+  const target = event.currentTarget as HTMLInputElement;
+
+  if (event.type === 'blur' && target.value === undefined) {
+    position = 0;
+  }
+
+  if (target.value === '') {
+    return;
+  }
+
+  const num = Number.parseFloat(target.value);
+
+  if (Number.isNaN(num)) {
+    return;
+  }
+
+  position = num;
 };
 
 const setRpm = (event: CustomEvent) => {
-  rpm = event.detail.value;
+  const target = event.currentTarget as HTMLInputElement;
+
+  if (event.type === 'blur' && target.value === undefined) {
+    rpm = 0;
+  }
+
+  if (target.value === '') {
+    return;
+  }
+
+  const num = Number.parseFloat(target.value);
+
+  if (Number.isNaN(num)) {
+    return;
+  }
+
+  rpm = num;
 };
 
 const setRevolutions = (event: CustomEvent) => {
-  revolutions = event.detail.value;
+  const target = event.currentTarget as HTMLInputElement;
+
+  if (event.type === 'blur' && target.value === undefined) {
+    revolutions = 0;
+  }
+
+  if (target.value === '') {
+    return;
+  }
+
+  const num = Number.parseInt(target.value, 10);
+
+  if (Number.isNaN(num)) {
+    return;
+  }
+
+  revolutions = num;
 };
 
 const setPowerSlider = (event: CustomEvent) => {
@@ -167,12 +215,13 @@ const handleToggle = async (event: CustomEvent<{ open: boolean }>) => {
   <div class="border border-t-0 border-medium p-4">
     <div class='mb-6'>
       <v-radio
+        class='inline-block'
         label="Set power"
         options={properties?.positionReporting ? 'Go, Go for, Go to' : 'Go'}
         selected={movementType}
         on:input={setMovementType}
       />
-      <small class='text-xs text-subtle-2'>
+      <small class='block pt-2 text-xs text-subtle-2'>
         {#if movementType === 'Go'}
           Continuously moves
         {:else if movementType === 'Go for'}
@@ -184,31 +233,34 @@ const handleToggle = async (event: CustomEvent<{ open: boolean }>) => {
     </div>
 
     <div class="flex flex-col gap-2">
-      {#if movementType === 'Go to'}
-        <div class='flex gap-2'>
+      <div class='flex gap-4 items-end'>
+        {#if movementType === 'Go to'}
           <v-input
             type="number"
             label="Position in revolutions"
+            placeholder='0'
             value={position}
             class="w-36 pr-2"
             on:input={setPosition}
+            on:blur={setPosition}
           />
           <v-input
             type="number"
             class="w-36 pr-2"
             label="RPM"
+            placeholder='0'
             value={rpm}
             on:input={setRpm}
           />
-        </div>
-      {:else if movementType === 'Go for'}
-        <div class='flex gap-4 items-end'>
+        {:else if movementType === 'Go for'}
           <v-input
             type="number"
             class="w-36"
             label="# in revolutions"
+            placeholder='0'
             value={revolutions}
             on:input={setRevolutions}
+            on:blur={setRevolutions}
           />
           <v-radio
             label="Direction of rotation"
@@ -219,13 +271,13 @@ const handleToggle = async (event: CustomEvent<{ open: boolean }>) => {
           <v-input
             type="number"
             label="RPM"
+            placeholder='0'
             class="w-36"
             value={rpm}
             on:input={setRpm}
+            on:blur={setRpm}
           />
-        </div>
-      {:else if movementType === 'Go'}
-        <div class='flex gap-4'>
+        {:else if movementType === 'Go'}
           <v-radio
             label="Direction of rotation"
             options="Forwards, Backwards"
@@ -245,8 +297,8 @@ const handleToggle = async (event: CustomEvent<{ open: boolean }>) => {
               on:input={setPowerSlider}
             />
           </div>
-        </div>
-      {/if}
+        {/if}
+      </div>
     </div>
 
     <div class="flex flex-row-reverse flex-wrap">

web/frontend/src/components/navigation/components/input/lnglat.svelte

@@ -9,11 +9,6 @@ export let readonly: true | undefined = undefined;
 export let lng: number | undefined;
 export let lat: number | undefined;
 
-const decimalFormat = new Intl.NumberFormat(undefined, { maximumFractionDigits: 6 });
-
-$: lngRounded = decimalFormat.format(lng ?? 0);
-$: latRounded = decimalFormat.format(lat ?? 0);
-
 const dispatch = createEventDispatcher<{ input: LngLat }>();
 
 const handleLng = (event: CustomEvent<{ value: string }>) => {
@@ -40,7 +35,7 @@ const handleLat = (event: CustomEvent<{ value: string }>) => {
     label={label ?? 'Latitude'}
     placeholder='0'
     incrementor={readonly ? undefined : 'slider'}
-    value={latRounded}
+    value={lat}
     step={$mapZoom ** 5}
     class='w-full'
     on:input={handleLat}
@@ -51,7 +46,7 @@ const handleLat = (event: CustomEvent<{ value: string }>) => {
     label={label ? '' : 'Longitude'}
     placeholder='0'
     incrementor={readonly ? undefined : 'slider'}
-    value={lngRounded}
+    value={lng}
     step={$mapZoom ** 5}
     class='w-full'
     on:input={handleLng}

web/frontend/src/components/navigation/components/map.svelte

@@ -2,7 +2,7 @@
 
 import { onMount } from 'svelte';
 import { Map, NavigationControl } from 'maplibre-gl';
-import { map, mapZoom, mapCenter, view } from '../stores';
+import { map, mapZoom, mapCenter, view, mapSize, cameraMatrix } from '../stores';
 import { style } from '../style';
 import ObstacleLayer from './obstacle-layer.svelte';
 import Waypoints from './waypoints.svelte';
@@ -17,15 +17,6 @@ const handleViewSelect = (event: CustomEvent) => {
   $view = event.detail.value;
 };
 
-const handleMove = () => {
-  if (!map.current) {
-    return;
-  }
-
-  mapCenter.set(map.current.getCenter());
-  mapZoom.set(map.current.getZoom() / map.current.getMaxZoom());
-};
-
 onMount(() => {
   const mapInstance = new Map({
     container: 'navigation-map',
@@ -36,14 +27,47 @@ onMount(() => {
     minPitch,
     maxPitch: minPitch,
   });
+  $map = mapInstance;
+
+  const handleMove = () => {
+    mapCenter.set(mapInstance.getCenter());
+    mapZoom.set(mapInstance.getZoom() / mapInstance.getMaxZoom());
+  };
+
+  const handleResize = () => {
+    mapSize.update((value) => {
+      const { clientWidth, clientHeight } = mapInstance.getCanvas();
+      value.width = clientWidth;
+      value.height = clientHeight;
+      return value;
+    });
+  };
 
   const nav = new NavigationControl({ showZoom: false });
   mapInstance.addControl(nav, 'top-right');
-
   mapInstance.on('move', handleMove);
+  mapInstance.on('resize', handleResize);
+
+  mapInstance.on('style.load', () => {
+    mapInstance.addLayer({
+      id: 'obstacle-layer',
+      type: 'custom',
+      renderingMode: '3d',
+      render (_ctx, viewProjectionMatrix) {
+        cameraMatrix.fromArray(viewProjectionMatrix);
+        mapInstance.triggerRepaint();
+      },
+    });
+  });
+
   handleMove();
+  handleResize();
 
-  $map = mapInstance;
+  return () => {
+    if (mapInstance.getLayer('obstacle-layer')) {
+      mapInstance.removeLayer('obstacle-layer');
+    }
+  };
 });
 
 $: {
@@ -70,11 +94,5 @@ $: {
 {#if $map}
   <RobotMarker {name} />
   <Waypoints {name} map={$map} />
-  <ObstacleLayer {name} map={$map} />
+  <ObstacleLayer />
 {/if}
-
-<style>
-  :global(#navigation-map ~ canvas) {
-    display: none;
-  }
-</style>

web/frontend/src/components/navigation/components/marker.svelte

@@ -10,6 +10,7 @@ export let visible = true;
 export let color: string | null = null;
 
 const marker = new Marker({ scale, color: color ?? undefined });
+marker.getElement().style.zIndex = '1';
 
 $: {
   marker.setLngLat(lngLat);