RSDK-6434 - Refactor the Prime SlamMap2D component to better path type (

viamrobotics#470)

Co-authored-by: Micheal Parks <[email protected]>

packages/blocks/package.json

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

packages/blocks/src/lib/slam-map-2d/index.svelte

@@ -11,7 +11,7 @@
     basePose={{ x: number, y: number, z: number }} // An optional pose of a robot.
     destination={{ x: number, y: number }} // An optional user-specificed robot destination.
     helpers={true | false} // Whether or not scene helpers should be rendered. Default true.
-    motionPath={'0.1,0.2\n0.3,0.4\n'} // An optional motion path.
+    motionPath={new Float32Array(x1, y1, x2, y2, x3, y3])} // An optional motion path. Units are assumed to be in Meters. Must not contain NaN.
   />
   ```
 -->
@@ -35,7 +35,7 @@ export let destination: THREE.Vector2 | undefined = undefined;
 export let helpers = true;
 
 /** An optional motion path */
-export let motionPath: string | undefined = undefined;
+export let motionPath: Float32Array | undefined = undefined;
 </script>
 
 <div class="relative h-full w-full">

packages/blocks/src/lib/slam-map-2d/motion-path.svelte

@@ -1,10 +1,12 @@
 <!--
   @component
   Renders a motion plan using thick lines.
-  Assumes the motion plan is coming in as a string in the format:
-  x,y\n
-  x,y\n
-  ...
+  
+  Assumes the motion plan is coming in as a Float32Array in the following format:
+  Float32Array([x1, y1, x2, y2, x3, y3, ...])
+
+  Units are assumed to be in Meters.
+  Must not contain NaN.
 -->
 <script lang="ts">
 import { T, extend } from '@threlte/core';
@@ -15,30 +17,24 @@ import { renderOrder } from './render-order';
 
 extend({ Line2, LineMaterial, LineGeometry });
 
-export let path: string | undefined;
+export let path: Float32Array | undefined;
 
-const updatePath = (pathstr?: string) => {
-  if (pathstr === undefined) {
+const updatePath = (xy?: Float32Array) => {
+  if (xy === undefined) {
     return;
   }
 
   const lineGeometry = new LineGeometry();
-  const points: number[] = [];
-
-  for (const xy of pathstr.split('\n')) {
-    const [xString, yString] = xy.split(',');
-    if (xString !== undefined && yString !== undefined) {
-      const x = Number.parseFloat(xString) / 1000;
-      const y = Number.parseFloat(yString) / 1000;
-      if (Number.isNaN(x) || Number.isNaN(y)) {
-        continue;
-      }
-      points.push(x, y, 0);
+  const xyz: number[] = [];
+  for (let i = 0; i < xy.length - 1; i += 2) {
+    const x = xy[i];
+    const y = xy[i + 1];
+    if (x !== undefined && y !== undefined) {
+      xyz.push(x, y, 0);
     }
   }
 
-  const vertices = new Float32Array(points);
-  lineGeometry.setPositions(vertices);
+  lineGeometry.setPositions(xyz);
 
   return lineGeometry;
 };

packages/blocks/src/lib/slam-map-2d/scene.svelte

@@ -15,7 +15,7 @@ export let pointcloud: Uint8Array | undefined;
 export let basePose: { x: number; y: number; theta: number } | undefined =
   undefined;
 export let destination: THREE.Vector2 | undefined;
-export let motionPath: string | undefined;
+export let motionPath: Float32Array | undefined = undefined;
 
 extend({ MapControls });
 

packages/blocks/src/routes/+page.svelte

@@ -9,6 +9,22 @@ const fetchPointcloud = async () => {
   const buffer = await response.arrayBuffer();
   return new Uint8Array(buffer);
 };
+
+const path = () =>
+  new Float32Array(
+    motionPath.split('\n').flatMap((str) => {
+      const [xStr, yStr] = str.split(',');
+      if (xStr && yStr) {
+        const x = Number.parseFloat(xStr) / 1000;
+        const y = Number.parseFloat(yStr) / 1000;
+        if (!Number.isNaN(x) && !Number.isNaN(y)) {
+          return [x, y];
+        }
+        return [];
+      }
+      return [];
+    })
+  );
 </script>
 
 <div class="m-auto flex max-w-6xl flex-col gap-6 py-6">
@@ -18,7 +34,7 @@ const fetchPointcloud = async () => {
         <SlamMap2D
           basePose={{ x: 1, y: 2, theta: 45 }}
           {pointcloud}
-          {motionPath}
+          motionPath={path()}
           on:click={(event) => console.log(event.detail)}
         />
       {/await}