RSDK-4825 Add plan rendering to navigation map (viamrobotics#429)

packages/blocks/package.json

@@ -1,6 +1,6 @@
 {
   "name": "@viamrobotics/prime-blocks",
-  "version": "0.0.16",
+  "version": "0.0.17",
   "publishConfig": {
     "access": "public"
   },

packages/blocks/src/lib/navigation-map/components/map.svelte

@@ -3,7 +3,7 @@ import { Button, Icon, ToggleButtons, Tooltip } from '@viamrobotics/prime-core';
 import type { Map } from 'maplibre-gl';
 import { MapLibre, type GeoPose } from '$lib';
 import { environment, view } from '../stores';
-import ObstacleLayer from './obstacle-layer.svelte';
+import SceneLayer from './scene-layer.svelte';
 import RobotMarker from './robot-marker.svelte';
 import CenterInputs from './center-inputs.svelte';
 import Nav from './nav/index.svelte';
@@ -84,7 +84,7 @@ onDestroy(() => {
     <RobotMarker pose={baseGeoPose} />
     <Waypoints />
 
-    <ObstacleLayer
+    <SceneLayer
       slot="layer"
       on:update-obstacles
     />

packages/blocks/src/lib/navigation-map/components/plan.svelte

@@ -0,0 +1,24 @@
+<script lang="ts">
+import type { LngLat } from '$lib/maplibre/types';
+import { T } from '@threlte/core';
+import type { ColorRepresentation } from 'three';
+
+export let plan: LngLat[];
+export let color: ColorRepresentation;
+</script>
+
+{#each plan as loc}
+  <T.Mesh userData.lngLat={loc}>
+    <T.SphereGeometry />
+    <T.MeshBasicMaterial {color} />
+  </T.Mesh>
+{/each}
+
+<T.Line userData.lngLat={plan}>
+  <T.BufferGeometry />
+  <T.LineDashedMaterial
+    {color}
+    dashSize={0.5}
+    gapSize={0.5}
+  />
+</T.Line>

packages/blocks/src/lib/navigation-map/components/scene.svelte

@@ -4,14 +4,16 @@ import { createEventDispatcher } from 'svelte';
 import { T, useThrelte } from '@threlte/core';
 import type { LngLat } from 'maplibre-gl';
 import type { Obstacle } from '../types';
-import { view, obstacles, selected, environment } from '../stores';
+import { view, obstacles, plans, selected, environment } from '../stores';
 import { computeBoundingPlugin } from '../plugins/compute-bounding';
 import { renderPlugin } from '../plugins/render';
 import { interactivityPlugin } from '../plugins/interactivity';
 import { createObstacle } from '../lib/create-obstacle';
 import { createName } from '../lib/create-name';
 import ObstacleGeometries from './obstacle.svelte';
 import Drawtool from './draw-tool.svelte';
+import Plan from './plan.svelte';
+import { theme } from '@viamrobotics/prime-core/theme';
 
 const dispatch = createEventDispatcher<{
   'update-obstacles': Obstacle[];
@@ -71,6 +73,23 @@ $: renderer.clippingPlanes = flat ? [] : [clippingPlane];
   />
 {/each}
 
+{#if $plans.previous}
+  {#each $plans.previous as plan}
+    <Plan
+      {plan}
+      color={theme.extend.colors.cyberpunk}
+    />
+  {/each}
+{/if}
+
+<!-- Render the current plan last -->
+{#if $plans.current}
+  <Plan
+    plan={$plans.current}
+    color="blue"
+  />
+{/if}
+
 {#if $environment === 'configure'}
   <Drawtool on:update={handleDraw} />
 {/if}

packages/blocks/src/lib/navigation-map/index.svelte

@@ -1,7 +1,7 @@
 <!--
   @component
 
-  A map for configuring or viewing navigation service information, such as waypoints, obstacles, or motion paths.
+  A map for configuring or viewing navigation service information, such as waypoints, obstacles, or motion plans.
 
   ```svelte
     <NavigationMap
@@ -18,10 +18,16 @@
 import { environment as envStore } from './stores';
 import type { Map as MapType } from 'maplibre-gl';
 import Map from './components/map.svelte';
-import { type Obstacle, NavigationTab, type NavigationTabType } from './types';
+import {
+  type Obstacle,
+  NavigationTab,
+  type NavigationTabType,
+  type Plans,
+} from './types';
 import {
   waypoints as waypointsStore,
   obstacles as obstaclesStore,
+  plans as plansStore,
   tab as tabStore,
   tabs as tabsStore,
 } from './stores';
@@ -36,6 +42,9 @@ export let waypoints: Waypoint[] = [];
 /** The obstacles to render on the map. */
 export let obstacles: Obstacle[] = [];
 
+/** The plans to render on the map. */
+export let plans: Plans = {};
+
 /** The initial tab to show. */
 export let tab: NavigationTabType = NavigationTab.Waypoints;
 
@@ -55,6 +64,7 @@ $: $tabStore = tab;
 $: $tabsStore = tabs;
 $: $waypointsStore = waypoints;
 $: $obstaclesStore = obstacles;
+$: $plansStore = plans;
 $: $envStore = environment;
 </script>
 

packages/blocks/src/lib/navigation-map/lib/math.ts

@@ -1,4 +1,5 @@
-import { MercatorCoordinate, LngLat } from 'maplibre-gl';
+import { MercatorCoordinate } from 'maplibre-gl';
+import type { LngLat } from '$lib';
 
 export const toPrecisionLevel = (number: number, decimals: number): number => {
   const multiplier = 10 ** decimals;
@@ -9,14 +10,19 @@ export const lngLatToMercator = (lngLat: LngLat): MercatorCoordinate => {
   return MercatorCoordinate.fromLngLat(lngLat, 0);
 };
 
-export const mercatorToCartesian = (mercator: MercatorCoordinate) => {
-  const scale = mercator.meterInMercatorCoordinateUnits();
+export const mercatorToCartesian = (
+  mercator: MercatorCoordinate,
+  scale = mercator.meterInMercatorCoordinateUnits()
+) => {
   return { x: mercator.x / scale, y: mercator.y / scale };
 };
 
-export const lngLatToCartesian = (lngLat: LngLat): { x: number; y: number } => {
+export const lngLatToCartesian = (
+  lngLat: LngLat,
+  scale?: number
+): { x: number; y: number } => {
   const mercator = lngLatToMercator(lngLat);
-  return mercatorToCartesian(mercator);
+  return mercatorToCartesian(mercator, scale);
 };
 
 export const cartesianToMercator = (x: number, y: number, scale: number) => {

packages/blocks/src/lib/navigation-map/plugins/render.ts

@@ -12,8 +12,12 @@
 import * as THREE from 'three';
 import { onDestroy } from 'svelte';
 import { useRender, useThrelte } from '@threlte/core';
-import { MercatorCoordinate } from 'maplibre-gl';
 import { useMapLibre, type LngLat } from '$lib';
+import {
+  lngLatToCartesian,
+  lngLatToMercator,
+  mercatorToCartesian,
+} from '../lib/math';
 
 export interface Props {
   lnglat?: LngLat;
@@ -39,13 +43,14 @@ export const renderPlugin = () => {
   );
 
   map.addLayer({
-    id: 'obstacle-layer',
+    id: 'scene-layer',
     type: 'custom',
     renderingMode: '3d',
     render(_, viewProjectionMatrix) {
       const center = map.getCenter();
-      const mercator = MercatorCoordinate.fromLngLat(center, 0);
+      const mercator = lngLatToMercator(center);
       const mercatorScale = mercator.meterInMercatorCoordinateUnits();
+      const { x: cx, y: cy } = mercatorToCartesian(mercator, mercatorScale);
 
       scale.makeScale(mercatorScale, mercatorScale, -mercatorScale);
       cameraTransform
@@ -57,18 +62,28 @@ export const renderPlugin = () => {
         .multiply(cameraTransform);
 
       scene.traverse((object) => {
-        const { lngLat } = object.userData as { lngLat?: LngLat | undefined };
+        const { lngLat } = object.userData as {
+          lngLat?: LngLat | LngLat[] | undefined;
+        };
 
         if (!lngLat) {
           return;
         }
 
-        const mercatorOffset = MercatorCoordinate.fromLngLat(lngLat, 0);
-        const cx = mercator.x / mercatorScale;
-        const cy = mercator.y / mercatorScale;
-        const ox = mercatorOffset.x / mercatorScale;
-        const oy = mercatorOffset.y / mercatorScale;
-        object.position.set(cx - ox, 0, cy - oy);
+        if (Array.isArray(lngLat)) {
+          if (object instanceof THREE.Line) {
+            (object.geometry as THREE.BufferGeometry).setFromPoints(
+              lngLat.map((ll) => {
+                const { x: ox, y: oy } = lngLatToCartesian(ll, mercatorScale);
+                return new THREE.Vector3(cx - ox, 0, cy - oy);
+              })
+            );
+            object.computeLineDistances();
+          }
+        } else {
+          const { x: ox, y: oy } = lngLatToCartesian(lngLat, mercatorScale);
+          object.position.set(cx - ox, 0, cy - oy);
+        }
       });
 
       renderer.render(scene, camera.current);
@@ -84,6 +99,6 @@ export const renderPlugin = () => {
   });
 
   onDestroy(() => {
-    map.removeLayer('obstacle-layer');
+    map.removeLayer('scene-layer');
   });
 };

packages/blocks/src/lib/navigation-map/stores.ts

@@ -1,13 +1,19 @@
 import { writable } from 'svelte/store';
 import { currentWritable } from '@threlte/core';
 import type { Waypoint } from '$lib';
-import { type Obstacle, NavigationTab, type NavigationTabType } from './types';
+import {
+  type Obstacle,
+  NavigationTab,
+  type NavigationTabType,
+  type Plans,
+} from './types';
 
 export const hovered = currentWritable<string | null>(null);
 export const selected = currentWritable<string | null>(null);
 
 export const waypoints = currentWritable<Waypoint[]>([]);
 export const obstacles = currentWritable<Obstacle[]>([]);
+export const plans = currentWritable<Plans>({});
 
 export type Environments = 'debug' | 'configure';
 

packages/blocks/src/lib/navigation-map/types.ts

@@ -41,3 +41,8 @@ export interface Obstacle {
   location: LngLat;
   geometries: Geometry[];
 }
+
+export interface Plans {
+  current?: LngLat[];
+  previous?: LngLat[][];
+}

packages/blocks/src/routes/navigation-map.svelte

@@ -83,6 +83,47 @@ const obstacles = [
   },
 ];
 
+const plans = {
+  current: [
+    { lng: -73.968, lat: 40.663 },
+    { lng: -73.9681, lat: 40.6631 },
+    { lng: -73.968, lat: 40.6632 },
+    { lng: -73.9681, lat: 40.6633 },
+    { lng: -73.968, lat: 40.6634 },
+    { lng: -73.9681, lat: 40.6635 },
+    { lng: -73.968, lat: 40.6636 },
+  ],
+  previous: [
+    [
+      { lng: -73.968, lat: 40.663 },
+      { lng: -73.9679, lat: 40.6631 },
+      { lng: -73.967_95, lat: 40.6632 },
+      { lng: -73.9679, lat: 40.6633 },
+      { lng: -73.967_95, lat: 40.6634 },
+      { lng: -73.9679, lat: 40.6635 },
+      { lng: -73.968, lat: 40.6636 },
+    ],
+    [
+      { lng: -73.968, lat: 40.663 },
+      { lng: -73.968_05, lat: 40.6631 },
+      { lng: -73.968_025, lat: 40.6632 },
+      { lng: -73.968_05, lat: 40.6633 },
+      { lng: -73.968_025, lat: 40.6634 },
+      { lng: -73.968_05, lat: 40.6635 },
+      { lng: -73.968, lat: 40.6636 },
+    ],
+    [
+      { lng: -73.968, lat: 40.663 },
+      { lng: -73.968_15, lat: 40.6631 },
+      { lng: -73.968_125, lat: 40.6632 },
+      { lng: -73.968_15, lat: 40.6633 },
+      { lng: -73.968_125, lat: 40.6634 },
+      { lng: -73.968_15, lat: 40.6635 },
+      { lng: -73.968, lat: 40.6636 },
+    ],
+  ],
+};
+
 let map: Map | undefined;
 
 let elapsed = 0;
@@ -111,6 +152,7 @@ $: map?.setCenter({ lat: 40.7, lng: -74.17 });
       baseGeoPose={pose}
       {obstacles}
       {waypoints}
+      {plans}
       on:create={(event) => console.log('create', event)}
       on:add-waypoint={(event) => console.log('add-waypoint', event)}
       on:delete-waypoint={(event) => console.log('delete-waypoint', event)}