From 9f741ffa97affab139debdd0ae8d7c92810a39d6 Mon Sep 17 00:00:00 2001 From: Lou Amadio Date: Fri, 2 Jun 2023 19:47:53 -0700 Subject: [PATCH] Final changes before PR --- package-lock.json | 50 +++---- package.json | 32 ++++- src/urdfPreview/URDFPreviewPanel.ts | 206 +++++++++++++++++++++++----- src/urdfPreview/preview.ts | 22 +++ 4 files changed, 248 insertions(+), 62 deletions(-) diff --git a/package-lock.json b/package-lock.json index 20dbafca..3e093c19 100644 --- a/package-lock.json +++ b/package-lock.json @@ -1,15 +1,15 @@ { "name": "vscode-ros", - "version": "0.9.3", + "version": "0.9.4", "lockfileVersion": 2, "requires": true, "packages": { "": { "name": "vscode-ros", - "version": "0.9.3", + "version": "0.9.4", "license": "SEE LICENSE IN LICENSE", "dependencies": { - "@polyhobbyist/babylon_ros": "^0.0.2", + "@polyhobbyist/babylon_ros": "^0.0.4", "@polyhobbyist/babylon-collada-loader": "^0.0.3", "@vscode/debugadapter": "^1.59.0", "@vscode/extension-telemetry": "^0.6.2", @@ -371,13 +371,13 @@ } }, "node_modules/@polyhobbyist/babylon_ros": { - "version": "0.0.2", - "resolved": "https://registry.npmjs.org/@polyhobbyist/babylon_ros/-/babylon_ros-0.0.2.tgz", - "integrity": "sha512-OS3MleDYG49UqcJPJMEB99SfNhSwj8vPq/PI1LDtscBKlNh9IgE1wa0WGcDby+qoKHsMI+11V4MmKlU9/GQ+hw==", + "version": "0.0.4", + "resolved": "https://registry.npmjs.org/@polyhobbyist/babylon_ros/-/babylon_ros-0.0.4.tgz", + "integrity": "sha512-t3pbnutuv6h5Kh+KI1baVu5DC93h5x0PO9ias2m5ELMFXLAjbMu3U4CjeyqjTAPoGFfrW9UvNKDBv06mEh6cjA==", "dependencies": { "@types/xml2js": "^0.4.11", "babylonjs": "^5.53.0", - "babylonjs-gui": "^5.53.0", + "babylonjs-gui": "^5.57.1", "babylonjs-loaders": "^5.53.0", "babylonjs-materials": "^5.53.0", "babylonjs-viewer": "^5.53.0", @@ -1021,9 +1021,9 @@ } }, "node_modules/babylonjs": { - "version": "5.54.0", - "resolved": "https://registry.npmjs.org/babylonjs/-/babylonjs-5.54.0.tgz", - "integrity": "sha512-IuW1VSssFyPC0qFRhBun/qBZSwNMGMGQJNkrppsTKALAHGQdcF9Ggj8w9Dk08Jix9IJdwNiVtFuBNLBctz/fAQ==", + "version": "5.57.1", + "resolved": "https://registry.npmjs.org/babylonjs/-/babylonjs-5.57.1.tgz", + "integrity": "sha512-X1t3mi8GuJjFVziN1yBJtekphilGN9VfOHm2tn/H6gra+WS7UZkrOOHLlKwYEXKdU73opxOR95jHXmv692KR6g==", "hasInstallScript": true }, "node_modules/babylonjs-gltf2interface": { @@ -1032,11 +1032,11 @@ "integrity": "sha512-GOG4PFIk8olsN0UVqj9izhxILHNQL0dSl8rrKJtQ/3OAAjTtgn6CIWF6iqmdTmQ9tY/FopCPDNGTsB2XZJ2vwQ==" }, "node_modules/babylonjs-gui": { - "version": "5.54.0", - "resolved": "https://registry.npmjs.org/babylonjs-gui/-/babylonjs-gui-5.54.0.tgz", - "integrity": "sha512-doDbNsGKKwjAf1dtoIun+ORJ2RuZKjs/EKd+1IJiGGKI1jv85MncmkG8C50zzU5RiEqpHa7TNAUUdrBvdMgFxQ==", + "version": "5.57.1", + "resolved": "https://registry.npmjs.org/babylonjs-gui/-/babylonjs-gui-5.57.1.tgz", + "integrity": "sha512-gOfRlC+XAyPqEF16Agd7W7g+j+9aLbJJ34/jiUgFLTFcAHZGHnh+kYX9+fRfjb8v0nRm633p8al0s8nUFoXl3A==", "dependencies": { - "babylonjs": "^5.54.0" + "babylonjs": "^5.57.1" } }, "node_modules/babylonjs-loaders": { @@ -5627,13 +5627,13 @@ } }, "@polyhobbyist/babylon_ros": { - "version": "0.0.2", - "resolved": "https://registry.npmjs.org/@polyhobbyist/babylon_ros/-/babylon_ros-0.0.2.tgz", - "integrity": "sha512-OS3MleDYG49UqcJPJMEB99SfNhSwj8vPq/PI1LDtscBKlNh9IgE1wa0WGcDby+qoKHsMI+11V4MmKlU9/GQ+hw==", + "version": "0.0.4", + "resolved": "https://registry.npmjs.org/@polyhobbyist/babylon_ros/-/babylon_ros-0.0.4.tgz", + "integrity": "sha512-t3pbnutuv6h5Kh+KI1baVu5DC93h5x0PO9ias2m5ELMFXLAjbMu3U4CjeyqjTAPoGFfrW9UvNKDBv06mEh6cjA==", "requires": { "@types/xml2js": "^0.4.11", "babylonjs": "^5.53.0", - "babylonjs-gui": "^5.53.0", + "babylonjs-gui": "^5.57.1", "babylonjs-loaders": "^5.53.0", "babylonjs-materials": "^5.53.0", "babylonjs-viewer": "^5.53.0", @@ -6203,9 +6203,9 @@ "integrity": "sha512-DMD0KiN46eipeziST1LPP/STfDU0sufISXmjSgvVsoU2tqxctQeASejWcfNtxYKqETM1UxQ8sp2OrSBWpHY6sw==" }, "babylonjs": { - "version": "5.54.0", - "resolved": "https://registry.npmjs.org/babylonjs/-/babylonjs-5.54.0.tgz", - "integrity": "sha512-IuW1VSssFyPC0qFRhBun/qBZSwNMGMGQJNkrppsTKALAHGQdcF9Ggj8w9Dk08Jix9IJdwNiVtFuBNLBctz/fAQ==" + "version": "5.57.1", + "resolved": "https://registry.npmjs.org/babylonjs/-/babylonjs-5.57.1.tgz", + "integrity": "sha512-X1t3mi8GuJjFVziN1yBJtekphilGN9VfOHm2tn/H6gra+WS7UZkrOOHLlKwYEXKdU73opxOR95jHXmv692KR6g==" }, "babylonjs-gltf2interface": { "version": "5.54.0", @@ -6213,11 +6213,11 @@ "integrity": "sha512-GOG4PFIk8olsN0UVqj9izhxILHNQL0dSl8rrKJtQ/3OAAjTtgn6CIWF6iqmdTmQ9tY/FopCPDNGTsB2XZJ2vwQ==" }, "babylonjs-gui": { - "version": "5.54.0", - "resolved": "https://registry.npmjs.org/babylonjs-gui/-/babylonjs-gui-5.54.0.tgz", - "integrity": "sha512-doDbNsGKKwjAf1dtoIun+ORJ2RuZKjs/EKd+1IJiGGKI1jv85MncmkG8C50zzU5RiEqpHa7TNAUUdrBvdMgFxQ==", + "version": "5.57.1", + "resolved": "https://registry.npmjs.org/babylonjs-gui/-/babylonjs-gui-5.57.1.tgz", + "integrity": "sha512-gOfRlC+XAyPqEF16Agd7W7g+j+9aLbJJ34/jiUgFLTFcAHZGHnh+kYX9+fRfjb8v0nRm633p8al0s8nUFoXl3A==", "requires": { - "babylonjs": "^5.54.0" + "babylonjs": "^5.57.1" } }, "babylonjs-loaders": { diff --git a/package.json b/package.json index 65848af4..f2b33953 100644 --- a/package.json +++ b/package.json @@ -129,6 +129,36 @@ "type": "bool", "default": false, "description": "Specify if the extension should not capture the environment VSCode is running in to pass to child processes." + }, + "ros.BackgroundColor": { + "type": "string", + "title": "Background Color", + "default": "#000000", + "description": "Background color of the URDF preview panel." + }, + "ros.GridMainColor": { + "type": "string", + "title": "Grid Main Color", + "default": "#00FF00", + "description": "Color of the main grid in the URDF preview panel." + }, + "ros.GridMinorColor": { + "type": "string", + "title": "Grid Minor Line Color", + "default": "#001100", + "description": "Color of the line grid in the URDF preview panel." + }, + "ros.GridMinorOpacity": { + "type": "number", + "default": 0.5, + "title": "Minor Grid Line Opacity", + "description": "Minor grid line opacity between 0 and 1 in the URDF preview panel." + }, + "ros.CameraDistanceToRobot": { + "type": "number", + "title": "Camera Distance to Robot", + "default": 1, + "description": "Distance in meters from the camera to the robot in the URDF preview panel." } } }, @@ -426,7 +456,7 @@ "test-compile": "npm run webpack" }, "dependencies": { - "@polyhobbyist/babylon_ros": "^0.0.3", + "@polyhobbyist/babylon_ros": "^0.0.4", "@polyhobbyist/babylon-collada-loader": "^0.0.3", "@vscode/debugadapter": "^1.59.0", "@vscode/extension-telemetry": "^0.6.2", diff --git a/src/urdfPreview/URDFPreviewPanel.ts b/src/urdfPreview/URDFPreviewPanel.ts index c668978b..9a664e26 100644 --- a/src/urdfPreview/URDFPreviewPanel.ts +++ b/src/urdfPreview/URDFPreviewPanel.ts @@ -2,6 +2,7 @@ import * as BABYLON from 'babylonjs'; import * as Materials from 'babylonjs-materials'; import * as urdf from '@polyhobbyist/babylon_ros'; import * as ColladaFileLoader from '@polyhobbyist/babylon-collada-loader'; +import * as GUI from 'babylonjs-gui'; // Get access to the VS Code API from within the webview context const vscode = acquireVsCodeApi(); @@ -12,29 +13,105 @@ let scene : BABYLON.Scene | undefined = undefined; let currentRobot : urdf.Robot | undefined = undefined; -function applyAxisToTransform(scene : BABYLON.Scene, t : BABYLON.TransformNode | undefined) { - if (t) { - let a = new BABYLON.AxesViewer(scene, .5); - a.xAxis.parent = t; - a.yAxis.parent = t; - a.zAxis.parent = t; +let ground : BABYLON.GroundMesh | undefined = undefined; +let camera : BABYLON.ArcRotateCamera | undefined = undefined; +let statusLabel = new GUI.TextBlock(); + +let readyToRender : Boolean = false; + +let axisList : BABYLON.PositionGizmo[] = []; + +function addAxisToTransform(scene : BABYLON.Scene, layer: BABYLON.UtilityLayerRenderer, transform : BABYLON.TransformNode | undefined) { + if (transform) { + let axis = new BABYLON.PositionGizmo(layer); + axis.scaleRatio = 0.5 + axis.attachedNode = transform; + axisList.push(axis); + + let drag = () => { + if (transform) { + statusLabel.text = transform.name + + "\nX: " + transform.position.x.toFixed(6) + + "\nY: " + transform.position.y.toFixed(6) + + "\nZ: " + transform.position.z.toFixed(6); + statusLabel.linkOffsetY = -100; + statusLabel.linkWithMesh(transform); + } + } + + axis.xGizmo.dragBehavior.onDragObservable.add(drag); + axis.yGizmo.dragBehavior.onDragObservable.add(drag); + axis.zGizmo.dragBehavior.onDragObservable.add(drag); + + } +} + +function toggleAxisOnRobot(scene : BABYLON.Scene, layer: BABYLON.UtilityLayerRenderer, robot : urdf.Robot) { + + if (axisList.length == 0) { + robot.joints.forEach((j) => { + addAxisToTransform(scene, layer, j.transform); + }); + robot.links.forEach((l) => { + l.visuals.forEach((v) => { + addAxisToTransform(scene, layer, l.transform); + }); + }); + } else { + axisList.forEach((a) => { + a.dispose(); + }); + axisList = []; } } -function applyAxisToLink(scene : BABYLON.Scene, robot : urdf.Robot, l : string) { - let r = robot.links.get(l); - if (r && r.visuals[0].transform) { - applyAxisToTransform(scene, r.visuals[0].transform); +let rotationGizmos : BABYLON.RotationGizmo[] = []; + +function addRotationToTransform(scene : BABYLON.Scene, layer: BABYLON.UtilityLayerRenderer, transform : BABYLON.TransformNode | undefined) { + if (transform) { + let rotationGizmo = new BABYLON.RotationGizmo(layer); + rotationGizmo.scaleRatio = 0.5 + rotationGizmo.attachedNode = transform; + rotationGizmos.push(rotationGizmo); + + let drag = () => { + if (transform) { + statusLabel.text = transform.name + + "\nR:" + transform.rotation.x.toFixed(6) + + "\nP:" + transform.rotation.y.toFixed(6) + + "\nY:" + transform.rotation.z.toFixed(6); + statusLabel.linkOffsetY = -100; + statusLabel.linkWithMesh(transform); + } + } + + rotationGizmo.xGizmo.dragBehavior.onDragObservable.add(drag); + rotationGizmo.yGizmo.dragBehavior.onDragObservable.add(drag); + rotationGizmo.zGizmo.dragBehavior.onDragObservable.add(drag); } + } -function applyAxisToJoint(scene : BABYLON.Scene, robot : urdf.Robot, j : string) { - let r = robot.joints.get(j); - if (r && r.transform) { - applyAxisToTransform(scene, r.transform); +function toggleAxisRotationOnRobot(ui: GUI.AdvancedDynamicTexture, scene : BABYLON.Scene, layer: BABYLON.UtilityLayerRenderer, robot : urdf.Robot) { + if (rotationGizmos.length == 0) { + robot.joints.forEach((j) => { + addRotationToTransform(scene, layer, j.transform); + }); + + robot.links.forEach((l) => { + l.visuals.forEach((v) => { + addRotationToTransform(scene, layer, v.transform); + }); + }); + } else { + rotationGizmos.forEach((a) => { + a.dispose(); + }); + rotationGizmos = []; } } + var createScene = async function () { scene = new BABYLON.Scene(engine); if (BABYLON.SceneLoader) { @@ -52,12 +129,11 @@ var createScene = async function () { }); scene.useRightHandedSystem = true; - scene.clearColor = BABYLON.Color4.FromColor3(BABYLON.Color3.Black());// TODO (polyhobbyist) Make this configurable - - var radius = 5; // TODO (polyhobbyist): make this configurable + scene.clearColor = BABYLON.Color4.FromColor3(BABYLON.Color3.Black()); + // This creates and positions a free camera (non-mesh) - var camera = new BABYLON.ArcRotateCamera("camera1", - Math.PI / 3, 5 * Math.PI / 12, radius, new BABYLON.Vector3(0, 0, 0), scene); + camera = new BABYLON.ArcRotateCamera("camera1", - Math.PI / 3, 5 * Math.PI / 12, 1, new BABYLON.Vector3(0, 0, 0), scene); camera.wheelDeltaPercentage = 0.01; camera.minZ = 0.1; @@ -67,20 +143,74 @@ var createScene = async function () { camera.attachControl(canvas, true); var groundMaterial = new Materials.GridMaterial("groundMaterial", scene); - groundMaterial.majorUnitFrequency = 1; + groundMaterial.majorUnitFrequency = 5; groundMaterial.minorUnitVisibility = 0.5; - groundMaterial.gridRatio = 2; + groundMaterial.gridRatio = 1; groundMaterial.opacity = 0.8; groundMaterial.useMaxLine = true; - groundMaterial.lineColor = BABYLON.Color3.Green(); // TODO (polyhobbyist) Make this configurable + groundMaterial.lineColor = BABYLON.Color3.Green(); + groundMaterial.mainColor = BABYLON.Color3.Green(); - var ground = BABYLON.MeshBuilder.CreateGround("ground", {width: 50, height: 50}, scene); + ground = BABYLON.MeshBuilder.CreateGround("ground", {width: 50, height: 50}, scene); ground.material = groundMaterial; + ground.isPickable = false; return scene; }; +function createUI(scene : BABYLON.Scene) { + var advancedTexture = GUI.AdvancedDynamicTexture.CreateFullscreenUI("UI"); + + statusLabel.color = "white"; + statusLabel.textHorizontalAlignment = GUI.Control.HORIZONTAL_ALIGNMENT_LEFT; + statusLabel.horizontalAlignment = GUI.Control.HORIZONTAL_ALIGNMENT_CENTER; + statusLabel.resizeToFit = true; + statusLabel.outlineColor = "green"; + statusLabel.outlineWidth = 2.0; + advancedTexture.addControl(statusLabel); + + var toolbar = new GUI.StackPanel(); + toolbar.paddingTop = "10px"; + toolbar.paddingLeft = "10px"; + toolbar.width = "300px"; + toolbar.height = "50px"; + toolbar.fontSize = "14px"; + toolbar.horizontalAlignment = GUI.Control.HORIZONTAL_ALIGNMENT_LEFT; + toolbar.verticalAlignment = GUI.Control.VERTICAL_ALIGNMENT_TOP; + toolbar.isVertical = false; + advancedTexture.addControl(toolbar); + + + var utilLayer = new BABYLON.UtilityLayerRenderer(scene); + + const gizmoManager = new BABYLON.GizmoManager(scene); + gizmoManager.usePointerToAttachGizmos = false; + + var button = GUI.Button.CreateSimpleButton("axisButton", "Axis"); + button.width = 0.2; + button.height = "40px"; + button.color = "white"; + button.background = "green"; + button.onPointerClickObservable.add(function() { + + toggleAxisOnRobot(scene, utilLayer, currentRobot); + }); + toolbar.addControl(button); + + var buttonRotate = GUI.Button.CreateSimpleButton("rotatioButton", "Rotation"); + buttonRotate.width = 0.2; + buttonRotate.height = "40px"; + buttonRotate.color = "white"; + buttonRotate.background = "green"; + buttonRotate.onPointerClickObservable.add(function() { + toggleAxisRotationOnRobot(advancedTexture, scene, utilLayer, currentRobot); + }); + + toolbar.addControl(buttonRotate); + +} + async function applyURDF(urdfText) { try { if (currentRobot) { @@ -108,26 +238,14 @@ async function applyURDF(urdfText) { text: `Could not render URDF due to: ${err}\n${err.stack}`, }); } - - //applyAxisToTransform(scene, robot.transform); - - //applyAxisToLink(scene, robot, "base_link"); - //applyAxisToLink(scene, robot, "right_leg"); - - //applyAxisToJoint(scene, robot, "base_to_right_leg"); - } + // Main function that gets executed once the webview DOM loads async function main() { scene = await createScene(); + createUI(scene); - engine.runRenderLoop(function () { - if (scene != undefined) { - scene.render(); - } - }); - window.addEventListener('message', event => { const message = event.data; // The JSON data our extension sent switch (message.command) { @@ -137,6 +255,22 @@ async function main() { case 'previewFile': vscode.setState({previewFile: message.previewFile}); break; + case 'colors': + camera.radius = message.cameraRadius; + scene.clearColor = BABYLON.Color4.FromHexString(message.backgroundColor); + let gm = ground.material as Materials.GridMaterial; + gm.lineColor = BABYLON.Color3.FromHexString(message.gridLineColor); + gm.mainColor = BABYLON.Color3.FromHexString(message.gridMainColor); + gm.minorUnitVisibility = parseFloat(message.gridMinorOpacity); + + readyToRender = true; + break; + } + }); + + engine.runRenderLoop(function () { + if (scene != undefined && readyToRender) { + scene.render(); } }); diff --git a/src/urdfPreview/preview.ts b/src/urdfPreview/preview.ts index da219b68..710f1916 100644 --- a/src/urdfPreview/preview.ts +++ b/src/urdfPreview/preview.ts @@ -7,6 +7,7 @@ import { rosApi } from '../ros/ros'; import { xacro } from '../ros/utils'; import { Disposable, window } from 'vscode'; import * as extension from "../extension"; +import * as vscode_utils from "../vscode-utils"; export default class URDFPreview { @@ -78,6 +79,11 @@ export default class URDFPreview this.dispose(); }, null, subscriptions); + vscode.workspace.onDidChangeConfiguration(event => { + this.updateColors(); + }, this, subscriptions); + + this._disposables = subscriptions; } @@ -91,6 +97,20 @@ export default class URDFPreview } } + private updateColors() { + if (this._webview) { + const config = vscode_utils.getExtensionConfiguration(); + this._webview.webview.postMessage({ + command: 'colors', + cameraRadius: config.get("CameraDistanceToRobot", "1.0"), + backgroundColor: config.get("BackgroundColor", "#000000"), + gridLineColor: config.get("GridMinorColor", "#00FF00"), + gridMainColor: config.get("GridMainColor", "#001100"), + gridMinorOpacity: config.get("GridMinorOpacity", "0.4") + }); + } + } + private async loadResource() { this._processing = true; @@ -138,6 +158,8 @@ export default class URDFPreview extension.outputChannel.appendLine("URDF previewing: " + previewFile); extension.outputChannel.append(urdfText); + this.updateColors(); + this._webview.webview.postMessage({ command: 'previewFile', previewFile: this._resource.path}); this._webview.webview.postMessage({ command: 'urdf', urdf: urdfText });