Add fixing TF

public/configs/simulator/config.json

@@ -4,7 +4,7 @@
   ],
   "ship": {
     "rosbridge": {
-      "uri": "ws://localhost:9090"
+      "uri": "ws://10.10.177.65:9090"
     },
     "topics": {
       "mesh": "/mesh_publisher/shape"
@@ -12,11 +12,7 @@
   },
   "transformTree": {
     "rosbridge": {
-      "uri": "ws://localhost:9090"
-    },
-    "topics": [
-      "/tf",
-      "/tf_static"
-    ]
+      "uri": "ws://10.10.177.65:9090"
+    }
   }
 }
\ No newline at end of file

public/configs/simulator/robots/crawler.json

 {
   "rosbridge": {
-    "uri": "ws://localhost:9090"
+    "uri": "ws://10.10.177.65:9090"
   },
   "type": "SMV",
   "name": "Crawler",
@@ -10,7 +10,7 @@
     ]
   },
   "mesh" : {
-    "uri" : "meshes/crawler/Altiscan2t.obj",
+    "uri" : "meshes/crawler/Altiscan.gltf",
     "transform" : {
       "translation" : [0,0.1,0],
       "rotation" : [0,-90,180],
@@ -21,5 +21,5 @@
     "prepareMission": "foo",
     "executeMission": "foo"
   },
-  "frame": ""
+  "frame": "world"
 }

src/common/store.ts

@@ -43,7 +43,6 @@ export interface ConfigState {
   transformTree: Config['transformTree'] | undefined,
   robots: {[name: string]: Robot};
   aois: {[id: string]: AreaOfInterest},
-  tf2_frames: Map<string, Object3D>,
 
   connect: (uri: string, bsonMode: boolean) => void;
   updateTopics: (uri: string) => void;
@@ -97,7 +96,6 @@ export const useStore = create<ConfigState>((set, get) => {
     connections: {},
     ship: undefined,
     transformTree: undefined,
-    tf2_frames: new Map<string, Object3D>([]),
     aois: {
       "0": {
         id: 0,

config-schemas @ 38cc4f60

-Subproject commit 4ee6f567d9ab79d0a817c48f7a395bcbb20277f8
+Subproject commit 38cc4f60049587115e2512389d246f32f4af79a9

src/features/viewport/Robot.tsx

-import { PropsWithoutRef, useState } from 'react';
-import { Euler, Quaternion, Vector3 } from 'three';
+import { PropsWithoutRef, useEffect, useMemo, useRef, useState } from 'react';
+import { Euler, Group, Object3D, Quaternion, Vector3 } from 'three';
 import { useTopic } from '../../common/store';
 import { Robot as RobotConfig } from '../../schemas/Robot.schema';
 import { Frame } from './TF2';
-import { OBJLoader } from 'three/examples/jsm/loaders/OBJLoader'
 import { useLoader } from '@react-three/fiber'
+import { GLTFLoader } from 'three/examples/jsm/loaders/GLTFLoader';
 
 
 export interface RobotProps {
@@ -14,7 +14,8 @@ export interface RobotProps {
 export default function Robot(props: PropsWithoutRef<RobotProps>) {
   const [hovering, setHovering] = useState(false);
   const poseStamped = useTopic(props.config.rosbridge.uri, props.config.topics.pose, 'geometry_msgs/PoseStamped');
-  const obj = useLoader(OBJLoader, props.config.mesh?.uri || "");
+  const gltf = useLoader(GLTFLoader, props.config.mesh?.uri || "");
+  const modelClone = useMemo<Object3D>(()=>{return gltf.scene.clone(true);}, [gltf]);
 
   // If we haven't received a pose yet: do not render the robot.
   if (!poseStamped) {
@@ -30,7 +31,6 @@ export default function Robot(props: PropsWithoutRef<RobotProps>) {
     q.setFromEuler(new Euler(props.config.mesh.transform.rotation[0], props.config.mesh.transform.rotation[1], props.config.mesh.transform.rotation[2]), true);
   }
 
-
   return (
     <Frame frameId={props.config.frame}>
         <group
@@ -43,16 +43,11 @@ export default function Robot(props: PropsWithoutRef<RobotProps>) {
             quaternion={q}
             scale={props.config.mesh?.transform?.scale}
           >
-            <primitive object={obj} />
+            <primitive object={modelClone} />
             {/* <mesh
             >
               <sphereGeometry />
-              {
-                hovering ?
-                  <meshPhongMaterial color={[0, 0, 1]} />
-                  :
-                  <meshPhongMaterial color={[1, 0, 0]} />
-              }
+              <meshPhongMaterial color={[0, 1, 0]} />
             </mesh> */}
           </group>
         </group>

src/features/viewport/TF2.tsx

 import { useThree } from '@react-three/fiber'
-import { PropsWithChildren, useCallback, useEffect, useRef } from 'react';
+import { PropsWithChildren, useEffect, useRef } from 'react';
 import { Group, Matrix4, Mesh, MeshBasicMaterial, Object3D, Quaternion, SphereGeometry, Vector3 } from 'three'
-import { useConnection, useStore, useTopic } from '../../common/store';
+import { useConnection } from '../../common/store';
 import { Config as GeneralConfig } from '../../schemas/Config.schema';
-import { Topic, Ros, Message } from 'roslib';
 import tf2_msgs from '../../common/ROS/tf2_msgs';
 import geometry_msgs from '../../common/ROS/geometry_msgs';
+import { Topic } from 'roslib';
+
+const prefix = "tf2_tree_frame_";
 
 export interface FrameProps {
   frameId?: string;
 }
 
 export function Frame(props: PropsWithChildren<FrameProps>) {
-    const frameObject = useStore(state => props.frameId? state.tf2_frames.get(props.frameId) : undefined);
+    const {scene} = useThree();
+    const group = useRef<Group | null>(null);
+
+    useEffect(()=>{
+        const timer = setInterval(()=>{
+            const frameObject = scene.getObjectByProperty("name", prefix + props.frameId);
+            if(frameObject && group.current){
+                frameObject.add(group.current);
+                clearInterval(timer);
+            }
+        }, 1000);
+        return () => {clearInterval(timer);};
+    },[group,scene]);
 
     return (
-        <group attach={(_parent, self) => {
-            frameObject?.add(self as unknown as Object3D);
-            return () => frameObject?.remove(self as unknown as Object3D);
-        }}>
+        <group ref={group}>
             { props.children }
         </group>
     );
 }
 
-
 export interface TF2Props {
     config: GeneralConfig["transformTree"] | undefined;
     debugVisualization?: boolean;
@@ -39,10 +49,10 @@ function convertFromROS(poseStamped: geometry_msgs["Transform"]): Matrix4 {
 function TF2(props: TF2Props) {
     const {scene} = useThree();
     let root = useRef<Object3D>(new Object3D());
-    let knownObjects = useStore((state) => state.tf2_frames);
-    const connection = useConnection(props.config?.rosbridge.uri);
-    const debug_geometry = useRef<SphereGeometry>(new SphereGeometry(0.15, 16, 16));
+    let knownObjects = useRef(new Map<string, Object3D>());
+    const debug_geometry = useRef<SphereGeometry>(new SphereGeometry(0.1, 16, 16));
     const debug_material = useRef<MeshBasicMaterial>(new MeshBasicMaterial({ color: 0xff0000 }));
+    const connection = useConnection(props.config?.rosbridge.uri);
 
     useEffect(
         () => {
@@ -54,7 +64,7 @@ function TF2(props: TF2Props) {
 
     function createFrame(name: string, parent?: string): Object3D {
         const Result = new Object3D();
-        Result.name = name;
+        Result.name = prefix + name;
         Result.matrixAutoUpdate = false;
 
         // Debug
@@ -63,9 +73,9 @@ function TF2(props: TF2Props) {
             Result.add(sphere);
         }
 
-        knownObjects.set(name, Result);
+        knownObjects.current.set(name, Result);
         if (parent) {
-            knownObjects.get(parent)?.add(Result); // Attach to parent
+            knownObjects.current.get(parent)?.add(Result); // Attach to parent
         } else {
             root.current.add(Result);
         }
@@ -81,11 +91,11 @@ function TF2(props: TF2Props) {
             const to_id = parsedMessage.transforms[i].child_frame_id; //child
 
             //Create frames if not known
-            if (!knownObjects.has(from_id)) createFrame(from_id);
-            if (!knownObjects.has(to_id)) createFrame(to_id, from_id);
+            if (!knownObjects.current.has(from_id)) createFrame(from_id);
+            if (!knownObjects.current.has(to_id)) createFrame(to_id, from_id);
 
-            const from_frame = knownObjects.get(from_id);
-            const to_frame = knownObjects.get(to_id);
+            const from_frame = knownObjects.current.get(from_id);
+            const to_frame = knownObjects.current.get(to_id);
 
             if (!from_frame || !to_frame) continue; //something went wrong
 
@@ -100,12 +110,14 @@ function TF2(props: TF2Props) {
 
     useEffect(
         () => {
-            if (props.debugVisualization || false) {
+            if ((props.debugVisualization || false) && !(window as any).printGraph) {
                 (window as any).printGraph = () => {
                     let recurse = function (node: Object3D): string {
 
                         if (node.children.length === 0) return "\"" + node.name + "\":{}";
-                        let res = "\"" + node.name + "\" : {";
+                        let node_name = node.name;
+                        if(node_name.startsWith(prefix)) node_name = node_name.slice(prefix.length);
+                        let res = "\"" + node_name + "\" : {";
                         let first = true;
                         for (let i = 0; i < node.children.length; i++) {
                             if (node.children[i].name === "") continue;
@@ -122,23 +134,19 @@ function TF2(props: TF2Props) {
         }, [props.config]
     );
         
-
-
     // Subscribe/Unsubscribe to TF Messages
     useEffect(() => {
         if (!connection?.rosbridge || !props.config) return;
 
-        let topics = [] as Topic[];
+        let tf_topic = new Topic({ "ros": connection?.rosbridge, "name": "/tf", "messageType": 'tf2_msgs/TFMessage' });
+        let tf_static_topic = new Topic({ "ros": connection?.rosbridge, "name": "/tf_static", "messageType": 'tf2_msgs/TFMessage' });
 
-        for (let i = 0; i < props.config.topics.length; ++i) {
-            topics[i] = new Topic({ "ros": connection?.rosbridge, "name": props.config.topics[i], "messageType": 'tf2_msgs/TFMessage' });
-            topics[i].subscribe(newTFMessage);
-        }
+        tf_topic.subscribe(newTFMessage);
+        tf_static_topic.subscribe(newTFMessage);
 
         return () => {  
-            for (let i = 0; i < topics.length; ++i) {
-                topics[i].unsubscribe(newTFMessage);
-            }
+            tf_topic.unsubscribe(newTFMessage);
+            tf_static_topic.unsubscribe(newTFMessage);
         };
     }, [connection?.rosbridge, props.config]);
 

src/features/viewport/Viewport.tsx

@@ -42,7 +42,7 @@ const Viewport = () => {
             />
         }
       </For>
-      <TF2 config={transformtree} debugVisualization={false}/>
+      <TF2 config={transformtree} debugVisualization={true}/>
     </Canvas>
   );
 }

src/features/viewport/Viewport2D.tsx

@@ -8,6 +8,7 @@ import { Box } from '@mui/system';
 import LoopIcon from '@mui/icons-material/Loop';
 import { IconButton } from '@mui/material';
 import Mesh from './Mesh';
+import TF2 from './TF2';
 
 const lineHeight = 20; // Magic value for now to get more reasonable delta values if the delta mode is pixels.
 const scrollSpeed = 1.2; // Crolling by one line increases the scale by this factor.
@@ -24,6 +25,7 @@ const Viewport2D = () => {
   const [cameraPosition, setCameraPosition] = useState<[number, number]>([0, 0]);
   const sideFactor = side === 'portside' ? -1 : 1;
   const canvas = useRef<HTMLCanvasElement>(null);
+  const transformtree = useStore(state => state.transformTree);
 
   // React uses passive events by default. So, we need to register the event
   // listener this way.
@@ -124,6 +126,7 @@ const Viewport2D = () => {
           />
          }
         </For>
+        <TF2 config={transformtree} debugVisualization={false}/>
       </Canvas>
       <Box
         component="div"