Skip to content
Snippets Groups Projects
Commit 1eb2f429 authored by Sebastian Pape's avatar Sebastian Pape
Browse files

Add fixing TF

parent 6e150124
No related branches found
No related tags found
No related merge requests found
Pipeline #275465 failed
...@@ -4,7 +4,7 @@ ...@@ -4,7 +4,7 @@
], ],
"ship": { "ship": {
"rosbridge": { "rosbridge": {
"uri": "ws://localhost:9090" "uri": "ws://10.10.177.65:9090"
}, },
"topics": { "topics": {
"mesh": "/mesh_publisher/shape" "mesh": "/mesh_publisher/shape"
...@@ -12,11 +12,7 @@ ...@@ -12,11 +12,7 @@
}, },
"transformTree": { "transformTree": {
"rosbridge": { "rosbridge": {
"uri": "ws://localhost:9090" "uri": "ws://10.10.177.65:9090"
}, }
"topics": [
"/tf",
"/tf_static"
]
} }
} }
\ No newline at end of file
{ {
"rosbridge": { "rosbridge": {
"uri": "ws://localhost:9090" "uri": "ws://10.10.177.65:9090"
}, },
"type": "SMV", "type": "SMV",
"name": "Crawler", "name": "Crawler",
...@@ -10,7 +10,7 @@ ...@@ -10,7 +10,7 @@
] ]
}, },
"mesh" : { "mesh" : {
"uri" : "meshes/crawler/Altiscan2t.obj", "uri" : "meshes/crawler/Altiscan.gltf",
"transform" : { "transform" : {
"translation" : [0,0.1,0], "translation" : [0,0.1,0],
"rotation" : [0,-90,180], "rotation" : [0,-90,180],
...@@ -21,5 +21,5 @@ ...@@ -21,5 +21,5 @@
"prepareMission": "foo", "prepareMission": "foo",
"executeMission": "foo" "executeMission": "foo"
}, },
"frame": "" "frame": "world"
} }
This diff is collapsed.
...@@ -43,7 +43,6 @@ export interface ConfigState { ...@@ -43,7 +43,6 @@ export interface ConfigState {
transformTree: Config['transformTree'] | undefined, transformTree: Config['transformTree'] | undefined,
robots: {[name: string]: Robot}; robots: {[name: string]: Robot};
aois: {[id: string]: AreaOfInterest}, aois: {[id: string]: AreaOfInterest},
tf2_frames: Map<string, Object3D>,
connect: (uri: string, bsonMode: boolean) => void; connect: (uri: string, bsonMode: boolean) => void;
updateTopics: (uri: string) => void; updateTopics: (uri: string) => void;
...@@ -97,7 +96,6 @@ export const useStore = create<ConfigState>((set, get) => { ...@@ -97,7 +96,6 @@ export const useStore = create<ConfigState>((set, get) => {
connections: {}, connections: {},
ship: undefined, ship: undefined,
transformTree: undefined, transformTree: undefined,
tf2_frames: new Map<string, Object3D>([]),
aois: { aois: {
"0": { "0": {
id: 0, id: 0,
......
config-schemas @ 38cc4f60
Subproject commit 4ee6f567d9ab79d0a817c48f7a395bcbb20277f8 Subproject commit 38cc4f60049587115e2512389d246f32f4af79a9
import { PropsWithoutRef, useState } from 'react'; import { PropsWithoutRef, useEffect, useMemo, useRef, useState } from 'react';
import { Euler, Quaternion, Vector3 } from 'three'; import { Euler, Group, Object3D, Quaternion, Vector3 } from 'three';
import { useTopic } from '../../common/store'; import { useTopic } from '../../common/store';
import { Robot as RobotConfig } from '../../schemas/Robot.schema'; import { Robot as RobotConfig } from '../../schemas/Robot.schema';
import { Frame } from './TF2'; import { Frame } from './TF2';
import { OBJLoader } from 'three/examples/jsm/loaders/OBJLoader'
import { useLoader } from '@react-three/fiber' import { useLoader } from '@react-three/fiber'
import { GLTFLoader } from 'three/examples/jsm/loaders/GLTFLoader';
export interface RobotProps { export interface RobotProps {
...@@ -14,7 +14,8 @@ export interface RobotProps { ...@@ -14,7 +14,8 @@ export interface RobotProps {
export default function Robot(props: PropsWithoutRef<RobotProps>) { export default function Robot(props: PropsWithoutRef<RobotProps>) {
const [hovering, setHovering] = useState(false); const [hovering, setHovering] = useState(false);
const poseStamped = useTopic(props.config.rosbridge.uri, props.config.topics.pose, 'geometry_msgs/PoseStamped'); 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 we haven't received a pose yet: do not render the robot.
if (!poseStamped) { if (!poseStamped) {
...@@ -30,7 +31,6 @@ export default function Robot(props: PropsWithoutRef<RobotProps>) { ...@@ -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); q.setFromEuler(new Euler(props.config.mesh.transform.rotation[0], props.config.mesh.transform.rotation[1], props.config.mesh.transform.rotation[2]), true);
} }
return ( return (
<Frame frameId={props.config.frame}> <Frame frameId={props.config.frame}>
<group <group
...@@ -43,16 +43,11 @@ export default function Robot(props: PropsWithoutRef<RobotProps>) { ...@@ -43,16 +43,11 @@ export default function Robot(props: PropsWithoutRef<RobotProps>) {
quaternion={q} quaternion={q}
scale={props.config.mesh?.transform?.scale} scale={props.config.mesh?.transform?.scale}
> >
<primitive object={obj} /> <primitive object={modelClone} />
{/* <mesh {/* <mesh
> >
<sphereGeometry /> <sphereGeometry />
{ <meshPhongMaterial color={[0, 1, 0]} />
hovering ?
<meshPhongMaterial color={[0, 0, 1]} />
:
<meshPhongMaterial color={[1, 0, 0]} />
}
</mesh> */} </mesh> */}
</group> </group>
</group> </group>
......
import { useThree } from '@react-three/fiber' 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 { 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 { Config as GeneralConfig } from '../../schemas/Config.schema';
import { Topic, Ros, Message } from 'roslib';
import tf2_msgs from '../../common/ROS/tf2_msgs'; import tf2_msgs from '../../common/ROS/tf2_msgs';
import geometry_msgs from '../../common/ROS/geometry_msgs'; import geometry_msgs from '../../common/ROS/geometry_msgs';
import { Topic } from 'roslib';
const prefix = "tf2_tree_frame_";
export interface FrameProps { export interface FrameProps {
frameId?: string; frameId?: string;
} }
export function Frame(props: PropsWithChildren<FrameProps>) { 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 ( return (
<group attach={(_parent, self) => { <group ref={group}>
frameObject?.add(self as unknown as Object3D);
return () => frameObject?.remove(self as unknown as Object3D);
}}>
{ props.children } { props.children }
</group> </group>
); );
} }
export interface TF2Props { export interface TF2Props {
config: GeneralConfig["transformTree"] | undefined; config: GeneralConfig["transformTree"] | undefined;
debugVisualization?: boolean; debugVisualization?: boolean;
...@@ -39,10 +49,10 @@ function convertFromROS(poseStamped: geometry_msgs["Transform"]): Matrix4 { ...@@ -39,10 +49,10 @@ function convertFromROS(poseStamped: geometry_msgs["Transform"]): Matrix4 {
function TF2(props: TF2Props) { function TF2(props: TF2Props) {
const {scene} = useThree(); const {scene} = useThree();
let root = useRef<Object3D>(new Object3D()); let root = useRef<Object3D>(new Object3D());
let knownObjects = useStore((state) => state.tf2_frames); let knownObjects = useRef(new Map<string, Object3D>());
const connection = useConnection(props.config?.rosbridge.uri); const debug_geometry = useRef<SphereGeometry>(new SphereGeometry(0.1, 16, 16));
const debug_geometry = useRef<SphereGeometry>(new SphereGeometry(0.15, 16, 16));
const debug_material = useRef<MeshBasicMaterial>(new MeshBasicMaterial({ color: 0xff0000 })); const debug_material = useRef<MeshBasicMaterial>(new MeshBasicMaterial({ color: 0xff0000 }));
const connection = useConnection(props.config?.rosbridge.uri);
useEffect( useEffect(
() => { () => {
...@@ -54,7 +64,7 @@ function TF2(props: TF2Props) { ...@@ -54,7 +64,7 @@ function TF2(props: TF2Props) {
function createFrame(name: string, parent?: string): Object3D { function createFrame(name: string, parent?: string): Object3D {
const Result = new Object3D(); const Result = new Object3D();
Result.name = name; Result.name = prefix + name;
Result.matrixAutoUpdate = false; Result.matrixAutoUpdate = false;
// Debug // Debug
...@@ -63,9 +73,9 @@ function TF2(props: TF2Props) { ...@@ -63,9 +73,9 @@ function TF2(props: TF2Props) {
Result.add(sphere); Result.add(sphere);
} }
knownObjects.set(name, Result); knownObjects.current.set(name, Result);
if (parent) { if (parent) {
knownObjects.get(parent)?.add(Result); // Attach to parent knownObjects.current.get(parent)?.add(Result); // Attach to parent
} else { } else {
root.current.add(Result); root.current.add(Result);
} }
...@@ -81,11 +91,11 @@ function TF2(props: TF2Props) { ...@@ -81,11 +91,11 @@ function TF2(props: TF2Props) {
const to_id = parsedMessage.transforms[i].child_frame_id; //child const to_id = parsedMessage.transforms[i].child_frame_id; //child
//Create frames if not known //Create frames if not known
if (!knownObjects.has(from_id)) createFrame(from_id); if (!knownObjects.current.has(from_id)) createFrame(from_id);
if (!knownObjects.has(to_id)) createFrame(to_id, from_id); if (!knownObjects.current.has(to_id)) createFrame(to_id, from_id);
const from_frame = knownObjects.get(from_id); const from_frame = knownObjects.current.get(from_id);
const to_frame = knownObjects.get(to_id); const to_frame = knownObjects.current.get(to_id);
if (!from_frame || !to_frame) continue; //something went wrong if (!from_frame || !to_frame) continue; //something went wrong
...@@ -100,12 +110,14 @@ function TF2(props: TF2Props) { ...@@ -100,12 +110,14 @@ function TF2(props: TF2Props) {
useEffect( useEffect(
() => { () => {
if (props.debugVisualization || false) { if ((props.debugVisualization || false) && !(window as any).printGraph) {
(window as any).printGraph = () => { (window as any).printGraph = () => {
let recurse = function (node: Object3D): string { let recurse = function (node: Object3D): string {
if (node.children.length === 0) return "\"" + node.name + "\":{}"; 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; let first = true;
for (let i = 0; i < node.children.length; i++) { for (let i = 0; i < node.children.length; i++) {
if (node.children[i].name === "") continue; if (node.children[i].name === "") continue;
...@@ -122,23 +134,19 @@ function TF2(props: TF2Props) { ...@@ -122,23 +134,19 @@ function TF2(props: TF2Props) {
}, [props.config] }, [props.config]
); );
// Subscribe/Unsubscribe to TF Messages // Subscribe/Unsubscribe to TF Messages
useEffect(() => { useEffect(() => {
if (!connection?.rosbridge || !props.config) return; 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) { tf_topic.subscribe(newTFMessage);
topics[i] = new Topic({ "ros": connection?.rosbridge, "name": props.config.topics[i], "messageType": 'tf2_msgs/TFMessage' }); tf_static_topic.subscribe(newTFMessage);
topics[i].subscribe(newTFMessage);
}
return () => { return () => {
for (let i = 0; i < topics.length; ++i) { tf_topic.unsubscribe(newTFMessage);
topics[i].unsubscribe(newTFMessage); tf_static_topic.unsubscribe(newTFMessage);
}
}; };
}, [connection?.rosbridge, props.config]); }, [connection?.rosbridge, props.config]);
......
...@@ -42,7 +42,7 @@ const Viewport = () => { ...@@ -42,7 +42,7 @@ const Viewport = () => {
/> />
} }
</For> </For>
<TF2 config={transformtree} debugVisualization={false}/> <TF2 config={transformtree} debugVisualization={true}/>
</Canvas> </Canvas>
); );
} }
......
...@@ -8,6 +8,7 @@ import { Box } from '@mui/system'; ...@@ -8,6 +8,7 @@ import { Box } from '@mui/system';
import LoopIcon from '@mui/icons-material/Loop'; import LoopIcon from '@mui/icons-material/Loop';
import { IconButton } from '@mui/material'; import { IconButton } from '@mui/material';
import Mesh from './Mesh'; 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 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. const scrollSpeed = 1.2; // Crolling by one line increases the scale by this factor.
...@@ -24,6 +25,7 @@ const Viewport2D = () => { ...@@ -24,6 +25,7 @@ const Viewport2D = () => {
const [cameraPosition, setCameraPosition] = useState<[number, number]>([0, 0]); const [cameraPosition, setCameraPosition] = useState<[number, number]>([0, 0]);
const sideFactor = side === 'portside' ? -1 : 1; const sideFactor = side === 'portside' ? -1 : 1;
const canvas = useRef<HTMLCanvasElement>(null); const canvas = useRef<HTMLCanvasElement>(null);
const transformtree = useStore(state => state.transformTree);
// React uses passive events by default. So, we need to register the event // React uses passive events by default. So, we need to register the event
// listener this way. // listener this way.
...@@ -124,6 +126,7 @@ const Viewport2D = () => { ...@@ -124,6 +126,7 @@ const Viewport2D = () => {
/> />
} }
</For> </For>
<TF2 config={transformtree} debugVisualization={false}/>
</Canvas> </Canvas>
<Box <Box
component="div" component="div"
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment