diff --git a/resources/web/wwi/FloatingProtoParameterWindow.js b/resources/web/wwi/FloatingProtoParameterWindow.js index d08449482a7..85ef6db4da2 100644 --- a/resources/web/wwi/FloatingProtoParameterWindow.js +++ b/resources/web/wwi/FloatingProtoParameterWindow.js @@ -2,7 +2,9 @@ import FloatingWindow from './FloatingWindow.js'; import {VRML} from './protoVisualizer/vrml_type.js'; import WbCamera from './nodes/WbCamera.js'; import WbHingeJoint from './nodes/WbHingeJoint.js'; +import WbLidar from './nodes/WbLidar.js'; import WbWorld from './nodes/WbWorld.js'; +import WbRangeFinder from './nodes/WbRangeFinder.js'; export default class FloatingProtoParameterWindow extends FloatingWindow { #protoManager; @@ -502,7 +504,7 @@ export default class FloatingProtoParameterWindow extends FloatingWindow { for (const key of keys) { const device = nodes.get(key); - if (device instanceof WbCamera) { + if (device instanceof WbCamera || device instanceof WbRangeFinder || device instanceof WbLidar) { numberOfDevices++; let div = document.createElement('div'); diff --git a/resources/web/wwi/Parser.js b/resources/web/wwi/Parser.js index 2b315819e8c..db160c98fed 100644 --- a/resources/web/wwi/Parser.js +++ b/resources/web/wwi/Parser.js @@ -627,16 +627,32 @@ export default class Parser { newNode = new WbInertialUnit(id, translation, scale, rotation, name === '' ? 'inertial unit' : name); else if (node.tagName === 'LED') newNode = new WbLed(id, translation, scale, rotation, name === '' ? 'led' : name); - else if (node.tagName === 'Lidar') - newNode = new WbLidar(id, translation, scale, rotation, name === '' ? 'lidar' : name); - else if (node.tagName === 'LightSensor') + else if (node.tagName === 'Lidar') { + const fieldOfView = parseFloat(getNodeAttribute(node, 'fieldOfView', Math.PI / 2)); + const horizontalResolution = parseInt(getNodeAttribute(node, 'horizontalResolution', '512')); + const maxRange = parseFloat(getNodeAttribute(node, 'maxRange', '1')); + const minRange = parseFloat(getNodeAttribute(node, 'minRange', '0.01')); + const numberOfLayers = parseInt(getNodeAttribute(node, 'numberOfLayers', '4')); + const tiltAngle = parseFloat(getNodeAttribute(node, 'tiltAngle', '0')); + const verticalFieldOfView = parseFloat(getNodeAttribute(node, 'verticalFieldOfView', '0.2')); + + newNode = new WbLidar(id, translation, scale, rotation, name === '' ? 'lidar' : name, fieldOfView, maxRange, minRange, + numberOfLayers, tiltAngle, verticalFieldOfView, horizontalResolution); + } else if (node.tagName === 'LightSensor') newNode = new WbLightSensor(id, translation, scale, rotation, name === '' ? 'light sensor' : name); else if (node.tagName === 'Pen') newNode = new WbPen(id, translation, scale, rotation, name === '' ? 'pen' : name); else if (node.tagName === 'Radar') newNode = new WbRadar(id, translation, scale, rotation, name === '' ? 'radar' : name); - else if (node.tagName === 'RangeFinder') - newNode = new WbRangeFinder(id, translation, scale, rotation, name === '' ? 'range finder' : name); + else if (node.tagName === 'RangeFinder') { + const height = parseInt(getNodeAttribute(node, 'height', '64')); + const width = parseInt(getNodeAttribute(node, 'width', '64')); + const fieldOfView = parseFloat(getNodeAttribute(node, 'fieldOfView', M_PI_4)); + const maxRange = parseFloat(getNodeAttribute(node, 'maxRange', '1')); + const minRange = parseFloat(getNodeAttribute(node, 'minRange', '0.01')); + newNode = new WbRangeFinder(id, translation, scale, rotation, name === '' ? 'range finder' : name, height, width, + fieldOfView, maxRange, minRange); + } else if (node.tagName === 'Receiver') newNode = new WbReceiver(id, translation, scale, rotation, name === '' ? 'receiver' : name); else if (node.tagName === 'Speaker') diff --git a/resources/web/wwi/X3dScene.js b/resources/web/wwi/X3dScene.js index d7a164f2d47..fc291515a62 100644 --- a/resources/web/wwi/X3dScene.js +++ b/resources/web/wwi/X3dScene.js @@ -17,6 +17,7 @@ import WbGroup from './nodes/WbGroup.js'; import WbImageTexture from './nodes/WbImageTexture.js'; import WbIndexedFaceSet from './nodes/WbIndexedFaceSet.js'; import WbIndexedLineSet from './nodes/WbIndexedLineSet.js'; +import WbLidar from './nodes/WbLidar.js'; import WbLight from './nodes/WbLight.js'; import WbMaterial from './nodes/WbMaterial.js'; import WbMesh from './nodes/WbMesh.js'; @@ -37,6 +38,7 @@ import WbVector3 from './nodes/utils/WbVector3.js'; import WbNormal from './nodes/WbNormal.js'; import WbSpotLight from './nodes/WbSpotLight.js'; import WbDirectionalLight from './nodes/WbDirectionalLight.js'; +import WbRangeFinder from './nodes/WbRangeFinder.js'; export default class X3dScene { #loader; @@ -394,6 +396,24 @@ export default class X3dScene { object.far = parseFloat(pose[key]); else if (key === 'near') object.near = parseFloat(pose[key]); + } else if (object instanceof WbRangeFinder) { + if (key === 'maxRange') + object.maxRange = parseFloat(pose[key]); + else if (key === 'minRange') + object.minRange = parseFloat(pose[key]); + } else if (object instanceof WbLidar) { + if (key === 'maxRange') + object.maxRange = parseFloat(pose[key]); + else if (key === 'minRange') + object.minRange = parseFloat(pose[key]); + else if (key === 'horizontalResolution') + object.horizontalResolution = parseInt(pose[key]); + else if (key === 'numberOfLayers') + object.numberOfLayers = parseInt(pose[key]); + else if (key === 'tiltAngle') + object.tiltAngle = parseFloat(pose[key]); + else if (key === 'verticalFieldOfView') + object.verticalFieldOfView = parseFloat(pose[key]); } if (object instanceof WbLight) { diff --git a/resources/web/wwi/nodes/WbAbstractCamera.js b/resources/web/wwi/nodes/WbAbstractCamera.js index b723c143438..a72c541b209 100644 --- a/resources/web/wwi/nodes/WbAbstractCamera.js +++ b/resources/web/wwi/nodes/WbAbstractCamera.js @@ -7,7 +7,6 @@ import {arrayXPointerFloat} from './utils/utils.js'; export default class WbAbstractCamera extends WbSolid { #fieldOfView; #height; - #isFrustumEnabled; #width; constructor(id, translation, scale, rotation, name, height, width, fieldOfView) { super(id, translation, scale, rotation, name); @@ -17,7 +16,7 @@ export default class WbAbstractCamera extends WbSolid { this.#width = width; this._isRangeFinder = false; - this.#isFrustumEnabled = false; + this._isFrustumEnabled = false; this._charType = ''; } @@ -73,34 +72,38 @@ export default class WbAbstractCamera extends WbSolid { this._transform = _wr_transform_new(); _wr_transform_attach_child(this._transform, this._renderable); _wr_transform_attach_child(this.wrenNode, this._transform); - - this.#applyFrustumToWren(); + this._applyFrustumToWren(); } - #applyFrustumToWren() { + _applyFrustumToWren() { _wr_node_set_visible(this._transform, false); _wr_static_mesh_delete(this._mesh); this._mesh = undefined; - if (!this.#isFrustumEnabled) + if (!this._isFrustumEnabled) return; - const frustumColor = [1, 0, 1]; + let frustumColor; + if (this._charType === 'c') + frustumColor = [1, 0, 1]; + else if (this._charType === 'r') + frustumColor = [1, 1, 0]; + const frustumColorRgb = _wrjs_array3(frustumColor[0], frustumColor[1], frustumColor[2]); _wr_phong_material_set_color(this._material, frustumColorRgb); let drawFarPlane; let f; - const n = this.minRange(); + const n = this._minRange(); // if the far is set to 0 it means the far clipping plane is set to infinity // so, the far distance of the colored frustum should be set arbitrarily - if (this._charType === 'c' && this.maxRange() === 0) { + if (this._charType === 'c' && this._maxRange() === 0) { f = n + 2 * _wr_config_get_line_scale(); drawFarPlane = false; } else { - f = this.maxRange(); + f = this._maxRange(); drawFarPlane = true; } @@ -153,21 +156,21 @@ export default class WbAbstractCamera extends WbSolid { _wr_node_set_visible(this._transform, true); } - minRange() { + _minRange() { } - maxRange() { + _maxRange() { return 1.0; } _update() { if (this.wrenObjectsCreatedCalled) - this.#applyFrustumToWren(); + this._applyFrustumToWren(); } applyOptionalRendering(enable) { - this.#isFrustumEnabled = enable; - this.#applyFrustumToWren(); + this._isFrustumEnabled = enable; + this._applyFrustumToWren(); } #addVertex(vertices, colors, vertex, color) { diff --git a/resources/web/wwi/nodes/WbCamera.js b/resources/web/wwi/nodes/WbCamera.js index 9496c4e9ff5..dc1803da174 100644 --- a/resources/web/wwi/nodes/WbCamera.js +++ b/resources/web/wwi/nodes/WbCamera.js @@ -39,11 +39,11 @@ export default class WbCamera extends WbAbstractCamera { this._update(); } - minRange() { + _minRange() { return this.#near; } - maxRange() { + _maxRange() { return this.#far; } } diff --git a/resources/web/wwi/nodes/WbLidar.js b/resources/web/wwi/nodes/WbLidar.js index 28519991fe1..c8974224fb6 100644 --- a/resources/web/wwi/nodes/WbLidar.js +++ b/resources/web/wwi/nodes/WbLidar.js @@ -1,5 +1,212 @@ -import WbSolid from './WbSolid.js'; +import WbAbstractCamera from './WbAbstractCamera.js'; +import WbWrenShaders from '../wren/WbWrenShaders.js'; +import WbWrenRenderingContext from '../wren/WbWrenRenderingContext.js'; +import {arrayXPointerFloat} from './utils/utils.js'; // This class is used to retrieve the type of device -export default class WbLidar extends WbSolid { +export default class WbLidar extends WbAbstractCamera { + #frustumMaterial; + #frustumMesh; + #frustumRenderable; + #horizontalResolution; + #maxRange; + #minRange; + #numberOfLayers; + #tiltAngle; + #verticalFieldOfView; + constructor(id, translation, scale, rotation, name, fieldOfView, maxRange, minRange, numberOfLayers, tiltAngle, + verticalFieldOfView, horizontalResolution) { + super(id, translation, scale, rotation, name, undefined, undefined, fieldOfView); + this.#horizontalResolution = horizontalResolution; + this.#maxRange = maxRange; + this.#minRange = minRange; + this.#numberOfLayers = numberOfLayers; + this.#tiltAngle = tiltAngle; + this.#verticalFieldOfView = verticalFieldOfView; + } + + get horizontalResolution() { + return this.#horizontalResolution; + } + + set horizontalResolution(newHorizontalResolution) { + if (newHorizontalResolution <= 0) + newHorizontalResolution = 1; + + this.#horizontalResolution = newHorizontalResolution; + + if (this.#numberOfLayers > 1 && this.height < this.#numberOfLayers) + this.#horizontalResolution = Math.ceil((this.#numberOfLayers * this.fieldOfView) / this._verticalFieldOfView()); + + this._update(); + } + + get maxRange() { + return this.#maxRange; + } + + set maxRange(newMaxRange) { + if (this.#minRange > newMaxRange || newMaxRange <= 0) + newMaxRange = this.#minRange + 1; + + this.#maxRange = newMaxRange; + this._update(); + } + + get minRange() { + return this.#minRange; + } + + set minRange(newMinRange) { + if (newMinRange <= 0) + newMinRange = 0.01; + if (newMinRange >= this.#maxRange) + this.#maxRange += 1; + + this.#minRange = newMinRange; + this._update(); + } + + get numberOfLayers() { + return this.#numberOfLayers; + } + + set numberOfLayers(newNumberOfLayers) { + if (newNumberOfLayers <= 0) + newNumberOfLayers = 1; + + this.#numberOfLayers = newNumberOfLayers; + if (newNumberOfLayers !== 1) { + const maxNumberOfLayers = this.height; + + if (newNumberOfLayers > maxNumberOfLayers) + this.#numberOfLayers = maxNumberOfLayers; + } + + this._update(); + } + + get tiltAngle() { + return this.#tiltAngle; + } + + set tiltAngle(newTiltAngle) { + this.#tiltAngle = newTiltAngle; + this._update(); + } + + get verticalFieldOfView() { + return this.#verticalFieldOfView; + } + + set verticalFieldOfView(newVerticalFieldOfView) { + if (newVerticalFieldOfView > Math.PI) + newVerticalFieldOfView = Math.PI; + else if (newVerticalFieldOfView <= 0) + newVerticalFieldOfView = 0.1; + + this.#verticalFieldOfView = newVerticalFieldOfView; + if (this.#numberOfLayers > 1 && this.height < this.#numberOfLayers) + this.#verticalFieldOfView = (this.#numberOfLayers * this.fieldOfView) / this.#horizontalResolution; + + this._update(); + } + + get height() { + if (this.#numberOfLayers === 1) + return 1; + + return Math.ceil((this.#verticalFieldOfView + this.fieldOfView / this.#horizontalResolution) * + (this.#horizontalResolution / this.fieldOfView)); + } + + createWrenObjects() { + // Lidar frustum + this.#frustumMaterial = _wr_phong_material_new(); + _wr_material_set_default_program(this.#frustumMaterial, WbWrenShaders.lineSetShader()); + + this.#frustumRenderable = _wr_renderable_new(); + _wr_renderable_set_cast_shadows(this.#frustumRenderable, false); + _wr_renderable_set_receive_shadows(this.#frustumRenderable, false); + _wr_renderable_set_visibility_flags(this.#frustumRenderable, WbWrenRenderingContext.VM_REGULAR); + _wr_renderable_set_material(this.#frustumRenderable, this.#frustumMaterial, undefined); + _wr_renderable_set_drawing_mode(this.#frustumRenderable, Enum.WR_RENDERABLE_DRAWING_MODE_LINES); + _wr_node_set_visible(this.#frustumRenderable, false); + + super.createWrenObjects(); + + _wr_transform_attach_child(this.wrenNode, this.#frustumRenderable); + } + + _applyFrustumToWren() { + _wr_node_set_visible(this.#frustumRenderable, false); + _wr_static_mesh_delete(this.#frustumMesh); + this.#frustumMesh = undefined; + + if (!this._isFrustumEnabled) + return; + + const frustumColor = [0, 1, 1]; + const frustumColorRgb = _wrjs_array3(frustumColor[0], frustumColor[1], frustumColor[2]); + _wr_phong_material_set_color(this.#frustumMaterial, frustumColorRgb); + + let i = 0; + const n = this.#minRange; + const f = this.#maxRange; + const fovV = this._verticalFieldOfView(); + const fovH = this.fieldOfView; + + const intermediatePointsNumber = Math.floor(fovH / 0.2); + const vertexCount = 4 * this.#numberOfLayers * (intermediatePointsNumber + 3); + const vertices = []; + + for (let layer = 0; layer < this.#numberOfLayers; layer++) { + let vAngle = 0; + if (this.#numberOfLayers > 1) + vAngle = fovV / 2.0 - (layer / (this.#numberOfLayers - 1.0)) * fovV + this.#tiltAngle; + const cosV = Math.cos(vAngle); + const sinV = Math.sin(vAngle); + this.#pushVertex(vertices, i++, 0, 0, 0); + // min range + for (let j = 0; j < intermediatePointsNumber + 2; ++j) { + const tmpHAngle = fovH / 2.0 - fovH * j / (intermediatePointsNumber + 1); + const x = n * Math.cos(tmpHAngle) * cosV; + const y = n * Math.sin(tmpHAngle) * cosV; + const z = n * sinV; + this.#pushVertex(vertices, i++, x, y, z); + this.#pushVertex(vertices, i++, x, y, z); + } + + this.#pushVertex(vertices, i++, 0, 0, 0); + this.#pushVertex(vertices, i++, 0, 0, 0); + + // max range + for (let j = 0; j < intermediatePointsNumber + 2; ++j) { + const tmpHAngle = fovH / 2.0 - fovH * j / (intermediatePointsNumber + 1); + const x = f * Math.cos(tmpHAngle) * cosV; + const y = f * Math.sin(tmpHAngle) * cosV; + const z = f * sinV; + + this.#pushVertex(vertices, i++, x, y, z); + this.#pushVertex(vertices, i++, x, y, z); + } + this.#pushVertex(vertices, i++, 0, 0, 0); + } + + const verticesPointer = arrayXPointerFloat(vertices); + this.#frustumMesh = _wr_static_mesh_line_set_new(vertexCount, verticesPointer, undefined); + _wr_renderable_set_mesh(this.#frustumRenderable, this.#frustumMesh); + _wr_node_set_visible(this.#frustumRenderable, true); + _free(verticesPointer); + } + + #pushVertex(vertices, index, x, y, z) { + vertices[3 * index] = x; + vertices[3 * index + 1] = y; + vertices[3 * index + 2] = z; + } + + _verticalFieldOfView() { + return this.fieldOfView * this.height / this.#horizontalResolution; + } } diff --git a/resources/web/wwi/nodes/WbRangeFinder.js b/resources/web/wwi/nodes/WbRangeFinder.js index eab3abe8a70..0e8f86e1044 100644 --- a/resources/web/wwi/nodes/WbRangeFinder.js +++ b/resources/web/wwi/nodes/WbRangeFinder.js @@ -2,4 +2,44 @@ import WbAbstractCamera from './WbAbstractCamera.js'; // This class is used to retrieve the type of device export default class WbRangeFinder extends WbAbstractCamera { + #maxRange; + #minRange; + constructor(id, translation, scale, rotation, name, height, width, fieldOfView, maxRange, minRange) { + super(id, translation, scale, rotation, name, height, width, fieldOfView); + this.#maxRange = maxRange; + this.#minRange = minRange; + this._charType = 'r'; + } + + get maxRange() { + return this.#maxRange; + } + + set maxRange(newMaxRange) { + if (this.#minRange > newMaxRange || newMaxRange <= 0) + newMaxRange = this.#minRange + 1; + + this.#maxRange = newMaxRange; + this._update(); + } + + get minRange() { + return this.#minRange; + } + + set minRange(newMinRange) { + if (newMinRange <= 0 || newMinRange > this.#maxRange) + newMinRange = 0.01; + + this.#minRange = newMinRange; + this._update(); + } + + _minRange() { + return this.#minRange; + } + + _maxRange() { + return this.#maxRange; + } } diff --git a/resources/web/wwi/protos/ProtoCamera.proto b/resources/web/wwi/protos/ProtoCamera.proto index 334ff5e4c03..69fa32c0713 100644 --- a/resources/web/wwi/protos/ProtoCamera.proto +++ b/resources/web/wwi/protos/ProtoCamera.proto @@ -4,16 +4,26 @@ EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/project PROTO ProtoCamera [ field SFFloat fieldOfView 0.785398 -field SFInt32 width 64 +field SFInt32 width 64 field SFInt32 height 64 field SFFloat near 0.01 field SFFloat far 0.0 +field SFFloat minRange 0.01 +field SFFloat maxRange 1 +field SFFloat lidarFieldOfView 1.5708 +field SFFloat lidarVerticalFieldOfView 0.2 +field SFInt32 lidarNumberOfLayers 4 +field SFInt32 lidarHorizontalResolution 512 +field SFFloat lidarMinRange 0.01 +field SFFloat lidarMaxRange 1 +field SFFloat lidarTiltAngle 0.0 ] { Robot { translation 0 0 0.2 children [ Camera { + rotation 0 1 0 1.5708 children [ Shape { appearance Asphalt { @@ -29,6 +39,23 @@ field SFFloat far near IS near fieldOfView IS fieldOfView } + RangeFinder { + rotation 0 0 1 1.5708 + width IS width + height IS height + maxRange IS maxRange + minRange IS minRange + fieldOfView IS fieldOfView + } + Lidar { + minRange IS lidarMinRange + maxRange IS lidarMaxRange + fieldOfView IS lidarFieldOfView + verticalFieldOfView IS lidarVerticalFieldOfView + numberOfLayers IS lidarNumberOfLayers + tiltAngle IS lidarTiltAngle + horizontalResolution IS lidarHorizontalResolution + } ] } } diff --git a/resources/web/wwi/test.html b/resources/web/wwi/test.html index de2a26bdefc..23da8546d39 100644 --- a/resources/web/wwi/test.html +++ b/resources/web/wwi/test.html @@ -53,7 +53,7 @@ //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/dji/mavic/protos/Mavic2Pro.proto"); //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/picaxe/microbot/protos/Microbot.proto"); //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/lego/mindstorms/protos/MindstormsRover.proto"); - // webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/mir/mir100/protos/Mir100.proto"); // missing lidars? + // webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/mir/mir100/protos/Mir100.proto"); // missing lidars? //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/clearpath/moose/protos/Moose.proto"); // webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/softbank/nao/protos/Nao.proto"); // missing stuff //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/niryo/ned/protos/Ned.proto"); @@ -73,7 +73,7 @@ //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/epson/scara_t6/protos/ScaraT6.proto"); // webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/bluebotics/shrimp/protos/Shrimp.proto"); // support solid reference //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/nasa/protos/Sojourner.proto"); - webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/boston_dynamics/spot/protos/Spot.proto"); + // webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/boston_dynamics/spot/protos/Spot.proto"); //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/robotnik/summit_xl_steel/protos/SummitXlSteel.proto"); //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/surveyor/protos/SurveyorSrv1.proto"); //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/feature-web-proto/projects/robots/pal_robotics/tiago_base/protos/TiagoBase.proto"); @@ -118,6 +118,6 @@ //webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/balls/protos/SoccerBall.proto"); // webotsView.loadProto("https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/chairs/protos/OfficeChair.proto"); // DEMO PROTO - // webotsView.loadProto("protos/ProtoCamera.proto"); + webotsView.loadProto("protos/ProtoCamera.proto"); // webotsView.loadProto("protos/DemoFieldChange.proto");