From a0d0e27c6d88bbe2d0c1936522f3be60f538d222 Mon Sep 17 00:00:00 2001 From: Simon Oehrl <simon.oehrl@rwth-aachen.de> Date: Thu, 1 Jun 2023 16:50:07 +0200 Subject: [PATCH] start working on services --- public/configs/simulator/config.json | 13 +++ public/configs/simulator/robots/crawler.json | 16 ++++ src/App.tsx | 94 +++++++++++--------- src/common/ROS/index.ts | 2 + src/common/ROS/shape_msgs.ts | 14 +++ src/common/store.ts | 30 ++++++- src/features/viewport/Mesh.tsx | 53 +++++++---- src/features/viewport/MissionPlanning.tsx | 32 +++++++ 8 files changed, 193 insertions(+), 61 deletions(-) create mode 100644 public/configs/simulator/config.json create mode 100644 public/configs/simulator/robots/crawler.json create mode 100644 src/common/ROS/shape_msgs.ts create mode 100644 src/features/viewport/MissionPlanning.tsx diff --git a/public/configs/simulator/config.json b/public/configs/simulator/config.json new file mode 100644 index 0000000..968c6d9 --- /dev/null +++ b/public/configs/simulator/config.json @@ -0,0 +1,13 @@ +{ + "robots": [ + "./robots/crawler.json" + ], + "ship": { + "rosbridge": { + "uri": "ws://localhost:9090" + }, + "topics": { + "mesh": "/mesh_publisher/shape" + } + } +} diff --git a/public/configs/simulator/robots/crawler.json b/public/configs/simulator/robots/crawler.json new file mode 100644 index 0000000..3eb8094 --- /dev/null +++ b/public/configs/simulator/robots/crawler.json @@ -0,0 +1,16 @@ +{ + "rosbridge": { + "uri": "ws://localhost:9090" + }, + "type": "SMV", + "name": "Crawler", + "topics": { + "pose": "/mesh_pf1/pose", + "images": [ + ] + }, + "services": { + "prepareMission": "foo", + "executeMission": "foo" + } +} diff --git a/src/App.tsx b/src/App.tsx index 7e84c5b..cd4be62 100644 --- a/src/App.tsx +++ b/src/App.tsx @@ -10,54 +10,60 @@ import Immersive from './Immersive'; import { useLocation } from 'react-router'; function useQuery() { - const { search } = useLocation(); + const { search } = useLocation(); - return useMemo(() => new URLSearchParams(search), [search]); + return useMemo(() => new URLSearchParams(search), [search]); } function App() { - const addRobot = useStore(state => state.addRobot); - const setShip = useStore(state => state.setShip); - - let isImmersive = useQuery().get('vr') !== null; - - useEffect( - () => { - let cancelled = false; - - const fetchConfig = async () => { - const configResponse = await fetch('https://ship-it-8682bacb.nip.io/config/config.json'); - const json = await configResponse.json(); - validate(json, ConfigSchema, { throwAll: true }); - const config = json as Config; - - if (cancelled) { - return; - } - - setShip(config.ship); - - for (const robot of config.robots) { - const robotResponse = await fetch(`https://ship-it-8682bacb.nip.io/config/${robot}`); - const json = await robotResponse.json(); - validate(json, RobotSchema, { throwAll: true }); - const robotConfig = json as Robot; - if (!cancelled) { - addRobot(robotConfig); - } - } - } - - fetchConfig().catch(console.error); - - return () => { cancelled = true }; - }, - [addRobot, setShip] - ); - - return ( - isImmersive ? <Immersive /> : <Desktop /> - ); + const addRobot = useStore(state => state.addRobot); + const setShip = useStore(state => state.setShip); + const query = useQuery(); + console.log(query); + + let isImmersive = query.get('vr') !== null; + let config = query.get('config'); + + useEffect( + () => { + if (config) { + const configPath = config; + let cancelled = false; + + const fetchConfig = async () => { + const configResponse = await fetch(configPath); + const json = await configResponse.json(); + validate(json, ConfigSchema, { throwAll: true }); + const config = json as Config; + + if (cancelled) { + return; + } + + setShip(config.ship); + + for (const robot of config.robots) { + const robotResponse = await fetch(`${configPath.substring(0, configPath.lastIndexOf('/'))}/${robot}`); + const json = await robotResponse.json(); + validate(json, RobotSchema, { throwAll: true }); + const robotConfig = json as Robot; + if (!cancelled) { + addRobot(robotConfig); + } + } + } + + fetchConfig().catch(console.error); + + return () => { cancelled = true }; + } + }, + [addRobot, setShip, config] + ); + + return ( + isImmersive ? <Immersive /> : <Desktop /> + ); } export default App; diff --git a/src/common/ROS/index.ts b/src/common/ROS/index.ts index 7d3d7b3..c4fc7ac 100644 --- a/src/common/ROS/index.ts +++ b/src/common/ROS/index.ts @@ -2,10 +2,12 @@ import std_msgs from './std_msgs'; import geometry_msgs from './geometry_msgs'; import sensor_msgs from './sensor_msgs'; import mesh_msgs from './mesh_msgs'; +import shape_msgs from './shape_msgs'; export type Messages = { std_msgs: std_msgs; geometry_msgs: geometry_msgs; sensor_msgs: sensor_msgs; + shape_msgs: shape_msgs; mesh_msgs: mesh_msgs; } diff --git a/src/common/ROS/shape_msgs.ts b/src/common/ROS/shape_msgs.ts new file mode 100644 index 0000000..01d61a1 --- /dev/null +++ b/src/common/ROS/shape_msgs.ts @@ -0,0 +1,14 @@ +import geometry_msgs from "./geometry_msgs"; + +type shape_msgs = { + MeshTriangle: { + vertex_indices: [number, number, number] + } + + Mesh: { + triangles: shape_msgs['MeshTriangle'][] + vertices: geometry_msgs['Point'][] + } +} + +export default shape_msgs; diff --git a/src/common/store.ts b/src/common/store.ts index 7b6d7f8..11ec76c 100644 --- a/src/common/store.ts +++ b/src/common/store.ts @@ -1,5 +1,5 @@ -import { useCallback, useEffect } from 'react'; -import { Ros, Topic } from 'roslib'; +import { useCallback, useEffect, useState } from 'react'; +import { Ros, Service, Topic } from 'roslib'; import create from 'zustand'; import { Config } from '../schemas/Config.schema'; import { Robot } from '../schemas/Robot.schema'; @@ -351,3 +351,29 @@ export function useTopic< return connectionTopic.value as Messages[Package][Message] | undefined; } } + +export function useService<Request, Response>(uri?: string, serviceName?: string) { + const connection = useConnection(uri); + + const [service, setService] = useState<Service>(); + + useEffect( + () => { + if (connection?.rosbridge && serviceName) { + setService(new Service({ + ros: connection?.rosbridge, + name: serviceName, + serviceType: '', + })); + return () => setService(undefined); + } + }, + [connection?.rosbridge] + ); + + return (request: Request) => { + return new Promise<Response>((resolve, reject) => { + service?.callService(request, (response) => resolve(response), reject); + }); + }; +} diff --git a/src/features/viewport/Mesh.tsx b/src/features/viewport/Mesh.tsx index efea140..64fce3c 100644 --- a/src/features/viewport/Mesh.tsx +++ b/src/features/viewport/Mesh.tsx @@ -1,6 +1,7 @@ import { EventHandlers } from "@react-three/fiber/dist/declarations/src/core/events"; import { useEffect, useState } from "react"; import { BufferGeometry, DoubleSide, Float32BufferAttribute, Uint32BufferAttribute } from "three"; +import { Messages } from "../../common/ROS"; import { useTopic } from "../../common/store"; export interface MeshProps extends EventHandlers { @@ -16,22 +17,44 @@ const Mesh = (props: MeshProps) => { useEffect(() => { if (mesh) { const geometry = new BufferGeometry(); - const vertices = new Float32Array(mesh.mesh_geometry.vertices.length * 3); - const indices = new Uint32Array(mesh.mesh_geometry.faces.length * 3); - mesh.mesh_geometry.vertices.forEach((vertex, index) => { - vertices[index * 3 + 0] = -vertex.x; - vertices[index * 3 + 2] = vertex.y; - vertices[index * 3 + 1] = vertex.z; - }); - - mesh.mesh_geometry.faces.forEach((face, index) => { - indices[index * 3 + 0] = face.vertex_indices[0]; - indices[index * 3 + 1] = face.vertex_indices[1]; - indices[index * 3 + 2] = face.vertex_indices[2]; - }); - geometry.setIndex(new Uint32BufferAttribute(indices, 1)); - geometry.setAttribute('position', new Float32BufferAttribute(vertices, 3)); + if ('triangles' in mesh && 'vertices' in mesh) { + // Hack to support shape_msgs/Mesh messages + const meshShape = mesh as Messages['shape_msgs']['Mesh']; + const vertices = new Float32Array(meshShape.vertices.length * 3); + const indices = new Uint32Array(meshShape.triangles.length * 3); + + meshShape.vertices.forEach((vertex, index) => { + vertices[index * 3 + 0] = -vertex.x; + vertices[index * 3 + 2] = vertex.y; + vertices[index * 3 + 1] = vertex.z; + }); + + meshShape.triangles.forEach((triangle, index) => { + indices[index * 3 + 0] = triangle.vertex_indices[0]; + indices[index * 3 + 1] = triangle.vertex_indices[1]; + indices[index * 3 + 2] = triangle.vertex_indices[2]; + }); + geometry.setIndex(new Uint32BufferAttribute(indices, 1)); + geometry.setAttribute('position', new Float32BufferAttribute(vertices, 3)); + } else { + const vertices = new Float32Array(mesh.mesh_geometry.vertices.length * 3); + const indices = new Uint32Array(mesh.mesh_geometry.faces.length * 3); + + mesh.mesh_geometry.vertices.forEach((vertex, index) => { + vertices[index * 3 + 0] = -vertex.x; + vertices[index * 3 + 2] = vertex.y; + vertices[index * 3 + 1] = vertex.z; + }); + + mesh.mesh_geometry.faces.forEach((face, index) => { + indices[index * 3 + 0] = face.vertex_indices[0]; + indices[index * 3 + 1] = face.vertex_indices[1]; + indices[index * 3 + 2] = face.vertex_indices[2]; + }); + geometry.setIndex(new Uint32BufferAttribute(indices, 1)); + geometry.setAttribute('position', new Float32BufferAttribute(vertices, 3)); + } geometry.computeVertexNormals(); setGeometry(geometry); if (props.onGeometryUpdate) { diff --git a/src/features/viewport/MissionPlanning.tsx b/src/features/viewport/MissionPlanning.tsx new file mode 100644 index 0000000..772ee4e --- /dev/null +++ b/src/features/viewport/MissionPlanning.tsx @@ -0,0 +1,32 @@ +import { useFrame } from "@react-three/fiber"; +import { useController } from "@react-three/xr"; +import { useEffect, useState } from "react"; +import { Service } from "roslib"; +import { Vector3 } from "three"; +import { useService } from "../../common/store"; + +export interface MissionPlanningProps { +} + +const MissionPlanning = (props: MissionPlanningProps) => { + const rightController = useController('right'); + const [triggerPressed, setTriggerPressed] = useState(false); + + const planMission = useService('', ''); + + useFrame((state, delta, frame) => { + // console.log(rightController?.controller.position); + // + + const triggerCurrentlyPressed = rightController?.inputSource.gamepad?.buttons[0].pressed; + if (!triggerCurrentlyPressed && triggerPressed) { + planMission({ test: 'test' }).then(response => console.log(response)); + // console.log(rightController?.controller.position); + } + setTriggerPressed(triggerCurrentlyPressed || false); + }); + + return null; +} + +export default MissionPlanning; -- GitLab