From f4f962262b6d37160b21735b602ca16c2c1e22e9 Mon Sep 17 00:00:00 2001
From: Ethan Look <ethan.look@gmail.com>
Date: Tue, 28 Nov 2023 11:51:08 -0500
Subject: [PATCH] Make obstacle coloring declarative (#435)

---
 packages/blocks/package.json                  |  2 +-
 packages/blocks/src/lib/index.ts              |  1 +
 .../components/draw-tool.svelte               |  3 +-
 .../components/nav/obstacles.svelte           | 24 ++++++++++--
 .../navigation-map/components/obstacle.svelte |  4 +-
 .../lib/navigation-map/lib/create-obstacle.ts |  3 ++
 .../blocks/src/lib/navigation-map/types.ts    |  2 +
 .../blocks/src/routes/navigation-map.svelte   | 38 +++++++++++++++++--
 8 files changed, 65 insertions(+), 12 deletions(-)

diff --git a/packages/blocks/package.json b/packages/blocks/package.json
index 464dec3a..33af557c 100644
--- a/packages/blocks/package.json
+++ b/packages/blocks/package.json
@@ -1,6 +1,6 @@
 {
   "name": "@viamrobotics/prime-blocks",
-  "version": "0.0.17",
+  "version": "0.0.18",
   "publishConfig": {
     "access": "public"
   },
diff --git a/packages/blocks/src/lib/index.ts b/packages/blocks/src/lib/index.ts
index d9e4a788..157e2b04 100644
--- a/packages/blocks/src/lib/index.ts
+++ b/packages/blocks/src/lib/index.ts
@@ -18,4 +18,5 @@ export type {
   BoxGeometry,
   Geometry,
   Obstacle,
+  Plans,
 } from './navigation-map/types';
diff --git a/packages/blocks/src/lib/navigation-map/components/draw-tool.svelte b/packages/blocks/src/lib/navigation-map/components/draw-tool.svelte
index 4d38c4d5..241e5069 100644
--- a/packages/blocks/src/lib/navigation-map/components/draw-tool.svelte
+++ b/packages/blocks/src/lib/navigation-map/components/draw-tool.svelte
@@ -15,6 +15,7 @@ import {
 } from 'maplibre-gl';
 import { view } from '../stores';
 import * as math from '../lib/math';
+import { theme } from '@viamrobotics/prime-core/theme';
 
 interface $$Events extends Record<string, unknown> {
   /** Fires when a rectangle is drawn. */
@@ -128,6 +129,6 @@ $: if (drawing) {
         on:create={handleGeometryCreate}
       />
     {/if}
-    <T.MeshPhongMaterial color="red" />
+    <T.MeshPhongMaterial color={theme.extend.colors.cyberpunk} />
   </T.Mesh>
 </T.Group>
diff --git a/packages/blocks/src/lib/navigation-map/components/nav/obstacles.svelte b/packages/blocks/src/lib/navigation-map/components/nav/obstacles.svelte
index b267ef0c..0acdaa57 100644
--- a/packages/blocks/src/lib/navigation-map/components/nav/obstacles.svelte
+++ b/packages/blocks/src/lib/navigation-map/components/nav/obstacles.svelte
@@ -1,5 +1,10 @@
 <script lang="ts">
-import { IconButton, Label, TextInput } from '@viamrobotics/prime-core';
+import {
+  IconButton,
+  Label,
+  TextInput,
+  Tooltip,
+} from '@viamrobotics/prime-core';
 import { LngLat, LngLatBounds } from 'maplibre-gl';
 import {
   type LngLat as LngLatInternal,
@@ -103,7 +108,7 @@ $: debugMode = $environment === 'debug';
   </li>
 {/if}
 
-{#each $obstacles as { name, location, geometries }, index (index)}
+{#each $obstacles as { name, location, geometries, color, label }, index (index)}
   <li
     class="group flex min-h-[30px] items-center border-b border-b-medium pl-2 leading-[1] last:border-b-0"
     class:pb-3={debugMode}
@@ -113,7 +118,9 @@ $: debugMode = $environment === 'debug';
   >
     <button
       class="w-full text-left"
-      on:click={() => ($selected = name)}
+      on:click={() => {
+        $selected = $selected === name ? null : name;
+      }}
     >
       <div class="flex items-center justify-between gap-1.5">
         <small>{name}</small>
@@ -137,6 +144,17 @@ $: debugMode = $environment === 'debug';
               handleSelect({ name, location });
             }}
           />
+          <Tooltip
+            let:tooltipID
+            location="right"
+          >
+            <div
+              aria-describedby={tooltipID}
+              class="m-2 h-3.5 w-3.5"
+              style:background-color={color}
+            />
+            <p slot="description">{label}</p>
+          </Tooltip>
         </div>
       </div>
       {#if debugMode}
diff --git a/packages/blocks/src/lib/navigation-map/components/obstacle.svelte b/packages/blocks/src/lib/navigation-map/components/obstacle.svelte
index e5505c4b..4ec84443 100644
--- a/packages/blocks/src/lib/navigation-map/components/obstacle.svelte
+++ b/packages/blocks/src/lib/navigation-map/components/obstacle.svelte
@@ -230,9 +230,7 @@ useMapLibreEvent('mousedown', handleMapPointerDown);
     {/if}
 
     <T.MeshPhongMaterial
-      color={active
-        ? theme.extend.colors['solar-power']
-        : theme.extend.colors['power-wire']}
+      color={active ? theme.extend.colors['solar-power'] : obstacle.color}
     />
   </T.Mesh>
 {/each}
diff --git a/packages/blocks/src/lib/navigation-map/lib/create-obstacle.ts b/packages/blocks/src/lib/navigation-map/lib/create-obstacle.ts
index 4b2a5fe8..054454f2 100644
--- a/packages/blocks/src/lib/navigation-map/lib/create-obstacle.ts
+++ b/packages/blocks/src/lib/navigation-map/lib/create-obstacle.ts
@@ -1,4 +1,5 @@
 import type { Obstacle, Shapes, LngLat } from '$lib';
+import { theme } from '@viamrobotics/prime-core/theme';
 import { createGeometry } from './create-geometry';
 
 export const createObstacle = (
@@ -10,5 +11,7 @@ export const createObstacle = (
     name,
     location: { lng: lngLat.lng, lat: lngLat.lat },
     geometries: [createGeometry(type)],
+    label: 'static',
+    color: theme.extend.colors.cyberpunk,
   };
 };
diff --git a/packages/blocks/src/lib/navigation-map/types.ts b/packages/blocks/src/lib/navigation-map/types.ts
index 247b9b78..55c865f2 100644
--- a/packages/blocks/src/lib/navigation-map/types.ts
+++ b/packages/blocks/src/lib/navigation-map/types.ts
@@ -40,6 +40,8 @@ export interface Obstacle {
   name: string;
   location: LngLat;
   geometries: Geometry[];
+  color: string;
+  label: string;
 }
 
 export interface Plans {
diff --git a/packages/blocks/src/routes/navigation-map.svelte b/packages/blocks/src/routes/navigation-map.svelte
index 3ca53077..df38b37b 100644
--- a/packages/blocks/src/routes/navigation-map.svelte
+++ b/packages/blocks/src/routes/navigation-map.svelte
@@ -6,18 +6,22 @@ import {
   type CapsuleGeometry,
   type SphereGeometry,
   type BoxGeometry,
+  type Obstacle,
+  type Waypoint,
+  type Plans,
 } from '$lib';
 import type { Map } from 'maplibre-gl';
 import { Label, SliderInput } from '@viamrobotics/prime-core';
 import { ViamObject3D } from '@viamrobotics/three';
+import { theme } from '@viamrobotics/prime-core/theme';
 
-const waypoints = [
+const waypoints: Waypoint[] = [
   { lng: -73.968_899_054_033_95, lat: 40.663_071_086_044, id: '0' },
   { lng: -73.972_162_444_595_26, lat: 40.661_759_669_002_69, id: '1' },
   { lng: -73.969_889_726_168_73, lat: 40.659_372_529_105_895, id: '2' },
 ];
 
-const obstacles = [
+const obstacles: Obstacle[] = [
   {
     name: 'obstacle 1',
     location: {
@@ -31,6 +35,8 @@ const obstacles = [
         radius: 20,
       } as SphereGeometry,
     ],
+    color: theme.extend.colors.cyberpunk,
+    label: 'static',
   },
   {
     name: 'obstacle 2',
@@ -45,6 +51,8 @@ const obstacles = [
         radius: 200,
       } as SphereGeometry,
     ],
+    color: theme.extend.colors.cyberpunk,
+    label: 'static',
   },
   {
     name: 'obstacle 3',
@@ -64,9 +72,11 @@ const obstacles = [
         })(),
       } as CapsuleGeometry,
     ],
+    color: theme.extend.colors.cyberpunk,
+    label: 'static',
   },
   {
-    name: 'obstacle 4',
+    name: 'transient obstacle 4',
     location: {
       lng: -74.7,
       lat: 40,
@@ -80,10 +90,30 @@ const obstacles = [
         pose: new ViamObject3D(),
       } as BoxGeometry,
     ],
+    color: theme.extend.colors.hologram,
+    label: 'transient',
+  },
+  {
+    name: 'obstacle 5',
+    location: {
+      lng: -74.701,
+      lat: 40.001,
+    },
+    geometries: [
+      {
+        type: 'box',
+        length: 50,
+        width: 50,
+        height: 50,
+        pose: new ViamObject3D(),
+      } as BoxGeometry,
+    ],
+    color: theme.extend.colors.cyberpunk,
+    label: 'static',
   },
 ];
 
-const plans = {
+const plans: Plans = {
   current: [
     { lng: -73.968, lat: 40.663 },
     { lng: -73.9681, lat: 40.6631 },