diff --git a/build/ros3d.js b/build/ros3d.js index 9bc55842..28dc1d19 100644 --- a/build/ros3d.js +++ b/build/ros3d.js @@ -63,7 +63,7 @@ ROS3D.makeColorMaterial = function(r, g, b, a) { if (a <= 0.99) { return new THREE.MeshBasicMaterial({ color : color.getHex(), - opacity : a + 0.1, + opacity : a, transparent : true, depthWrite : true, blendSrc : THREE.SrcAlphaFactor, @@ -149,9 +149,9 @@ ROS3D.closestAxisPoint = function(axisRay, camera, mousePos) { // project axis onto screen var o = axisRay.origin.clone(); - projector.projectVector(o, camera); + o.project(camera); var o2 = axisRay.direction.clone().add(axisRay.origin); - projector.projectVector(o2, camera); + o2.project(camera); // d is the axis vector in screen space (d = o2-o) var d = o2.clone().sub(o); @@ -167,12 +167,349 @@ ROS3D.closestAxisPoint = function(axisRay, camera, mousePos) { // go back to 3d by shooting a ray var vector = new THREE.Vector3(mp.x, mp.y, 0.5); - projector.unprojectVector(vector, camera); + vector.unproject(camera); var mpRay = new THREE.Ray(camera.position, vector.sub(camera.position).normalize()); return ROS3D.findClosestPoint(axisRay, mpRay); }; +/** + * @author Julius Kammerl - jkammerl@willowgarage.com + */ + +/** + * The CloseCloud object. An alternative DepthCloud implementation which + * has higher resolution when the depth range is less than 2m. + * + * @constructor + * @param options - object with following keys: + * + * * url - the URL of the stream + * * streamType (optional) - the stream type: mjpeg or vp8 video (defaults to vp8) + * * f (optional) - the camera's focal length (defaults to standard Kinect calibration) + * * pointSize (optional) - point size (pixels) for rendered point cloud + * * width (optional) - width of the depthcloud encoded video stream + * * height (optional) - height of the depthcloud encoded video stream + * * whiteness (optional) - blends rgb values to white (0..100) + * * varianceThreshold (optional) - threshold for variance filter, used for compression artifact removal + */ +ROS3D.CloseCloud = function(options) { + THREE.Object3D.call(this); + this.type = 'CloseCloud'; + + this.options = options || {}; + this.url = options.url; + this.streamType = options.streamType || 'vp8'; + this.f = options.f || 526; + this.pointSize = options.pointSize || 3; + this.width = options.width || 1024; + this.height = options.height || 1024; + this.whiteness = options.whiteness || 0; + this.varianceThreshold = options.varianceThreshold || 0.000016667; + + var metaLoaded = false; + + this.isMjpeg = this.streamType.toLowerCase() === 'mjpeg'; + + this.video = document.createElement(this.isMjpeg ? 'img' : 'video'); + this.video.width = this.width; + this.video.height = this.height; + this.video.addEventListener(this.isMjpeg ? 'load' : 'loadedmetadata', this.metaLoaded.bind(this), false); + + if (!this.isMjpeg) { + this.video.loop = true; + } + + this.video.src = this.url; + this.video.crossOrigin = 'Anonymous'; + this.video.setAttribute('crossorigin', 'Anonymous'); + + // define custom shaders + this.vertex_shader = [ + 'uniform sampler2D map;', + '', + 'uniform float width;', + 'uniform float height;', + '', + 'uniform float pointSize;', + 'uniform float zOffset;', + '', + 'uniform float focallength;', + '', + 'varying vec2 vUvP;', + 'varying vec2 colorP;', + '', + 'varying float depthVariance;', + 'varying float maskVal;', + '', + 'float sampleDepth(vec2 pos)', + ' {', + ' float depth;', + ' ', + ' vec2 vUv = vec2( pos.x / (width*2.0), pos.y / (height*2.0)+0.5 );', + ' vec2 vUv2 = vec2( pos.x / (width*2.0)+0.5, pos.y / (height*2.0)+0.5 );', + ' ', + ' vec4 depthColor = texture2D( map, vUv );', + ' ', + ' depth = ( depthColor.r + depthColor.g + depthColor.b ) / 3.0;', + ' ', + ' if (depth > (1.0 - 3.0/255.0) )', // If we're closer than 3 values from saturation, check the next depth image + ' {', + ' vec4 depthColor2 = texture2D( map, vUv2 );', + ' float depth2 = ( depthColor2.r + depthColor2.g + depthColor2.b ) / 3.0 ;', + ' depth += depth2;', + ' }', + ' ', + ' return depth;', + ' }', + '', + 'float median(float a, float b, float c)', + ' {', + ' float r=a;', + ' ', + ' if ( (a0.5) || (vUvP.y<0.5) || (vUvP.y>0.0))', + ' {', + ' vec2 smp = decodeDepth(vec2(position.x, position.y));', + ' float depth = smp.x;', + ' depthVariance = smp.y;', + ' ', + ' float z = -depth;', + ' ', + ' pos = vec4(', + ' ( position.x / width - 0.5 ) * z *0.5 * (1000.0/focallength) * -1.0,', + ' ( position.y / height - 0.5 ) * z *0.5 * (1000.0/focallength),', + ' (- z + zOffset / 1000.0) * 1.0,', + ' 1.0);', + ' ', + ' vec2 maskP = vec2( position.x / (width*2.0), position.y / (height*2.0) );', + ' vec4 maskColor = texture2D( map, maskP );', + ' maskVal = ( maskColor.r + maskColor.g + maskColor.b ) / 3.0 ;', + ' }', + ' ', + ' gl_PointSize = pointSize;', + ' gl_Position = projectionMatrix * modelViewMatrix * pos;', + ' ', + '}' + ].join('\n'); + + this.fragment_shader = [ + 'uniform sampler2D map;', + 'uniform float varianceThreshold;', + 'uniform float whiteness;', + '', + 'varying vec2 vUvP;', + 'varying vec2 colorP;', + '', + 'varying float depthVariance;', + 'varying float maskVal;', + '', + '', + 'void main() {', + ' ', + ' vec4 color;', + ' ', + ' if ( (depthVariance>varianceThreshold) || (maskVal>0.5) ||(vUvP.x<0.0)|| (vUvP.x>0.5) || (vUvP.y<0.5) || (vUvP.y>1.0))', + ' { ', + ' discard;', + ' }', + ' else ', + ' {', + ' color = texture2D( map, colorP );', + ' ', + ' float fader = whiteness /100.0;', + ' ', + ' color.r = color.r * (1.0-fader)+ fader;', + ' ', + ' color.g = color.g * (1.0-fader)+ fader;', + ' ', + ' color.b = color.b * (1.0-fader)+ fader;', + ' ', + ' color.a = 1.0;//smoothstep( 20000.0, -20000.0, gl_FragCoord.z / gl_FragCoord.w );', + ' }', + ' ', + ' gl_FragColor = vec4( color.r, color.g, color.b, color.a );', + ' ', + '}' + ].join('\n'); +}; +ROS3D.CloseCloud.prototype = Object.create( THREE.Object3D.prototype ); +ROS3D.CloseCloud.prototype.constructor = ROS3D.CloseCloud; + +ROS3D.CloseCloud.prototype.clone = function ( object, recursive ) { + + if ( object === undefined ) { object = new ROS3D.CloseCloud( this.options ); } + + THREE.Object3D.prototype.clone.call( this, object, recursive ); + + return object; + +}; + +/** + * Callback called when video metadata is ready + */ +ROS3D.CloseCloud.prototype.metaLoaded = function() { + this.metaLoaded = true; + this.initStreamer(); +}; + +/** + * Callback called when video metadata is ready + */ +ROS3D.CloseCloud.prototype.initStreamer = function() { + + if (this.metaLoaded) { + this.dctexture = new THREE.Texture(this.video); + this.dcgeometry = new THREE.Geometry(); + + var qwidth = this.width / 2; + var qheight = this.height / 2; + // the number of points is a forth of the total image size + for (var i = 0, l = qwidth * qheight; i < l; i++) { + + var vertex = new THREE.Vector3(); + vertex.x = (i % qwidth); + vertex.y = Math.floor(i / qwidth); + + this.dcgeometry.vertices.push(vertex); + } + + this.dcmaterial = new THREE.ShaderMaterial({ + uniforms : { + 'map' : { + type : 't', + value : this.dctexture + }, + 'width' : { + type : 'f', + value : qwidth + }, + 'height' : { + type : 'f', + value : qheight + }, + 'focallength' : { + type : 'f', + value : this.f + }, + 'pointSize' : { + type : 'f', + value : this.pointSize + }, + 'zOffset' : { + type : 'f', + value : 0 + }, + 'whiteness' : { + type : 'f', + value : this.whiteness + }, + 'varianceThreshold' : { + type : 'f', + value : this.varianceThreshold + } + }, + vertexShader : this.vertex_shader, + fragmentShader : this.fragment_shader + }); + this.dcmaterial.color = new THREE.Color( 0xffffff ); + this.mesh = new THREE.PointCloud(this.dcgeometry, this.dcmaterial); + this.mesh.frustumCulled = false; + this.mesh.position.set(0,0,0); + + this.add(this.mesh); + + var that = this; + this.interval = setInterval(function() { + if (that.isMjpeg || that.video.readyState === that.video.HAVE_ENOUGH_DATA) { + that.dctexture.needsUpdate = true; + } + }, 1000 / 30); + } +}; + +/** + * Start video playback + */ +ROS3D.CloseCloud.prototype.startStream = function() { + if (!this.isMjpeg) { + this.video.play(); + } +}; + +/** + * Stop video playback + */ +ROS3D.CloseCloud.prototype.stopStream = function() { + if (!this.isMjpeg) { + this.video.pause(); + } + this.video.src = ''; // forcefully silence the video streaming url. + clearInterval(this.interval); +}; + /** * @author Julius Kammerl - jkammerl@willowgarage.com */ @@ -458,7 +795,7 @@ ROS3D.DepthCloud.prototype.initStreamer = function() { fragmentShader : this.fragment_shader }); - this.mesh = new THREE.ParticleSystem(this.geometry, this.material); + this.mesh = new THREE.PointCloud(this.geometry, this.material); this.mesh.position.x = 0; this.mesh.position.y = 0; this.add(this.mesh); @@ -491,6 +828,200 @@ ROS3D.DepthCloud.prototype.stopStream = function() { } }; +/** + * @author Julius Kammerl - jkammerl@willowgarage.com + * @author Peter Soetens - peter@thesourceworks.com + */ + +/** + * The PointCloud object. Allows you to visualise PointCloud2 messages. + * + * @constructor + * @param options - object with following keys: + * + * * topic - the topic to subscribe to for PointCloud2 messages + * * pointSize (optional) - point size (pixels) for rendered point cloud + * * width (optional) - width of the video stream + * * height (optional) - height of the video stream + * * whiteness (optional) - blends rgb values to white (0..100) + */ +ROS3D.PointCloud = function(options) { + options = options || {}; + + this.topic = options.topic; + this.pointSize = options.pointSize || 3; + this.width = options.width || 640; + this.height = options.height || 480; + this.whiteness = options.whiteness || 0; + this.queue_length = options.queue_length || 0; + this.opacity = options.opacity || 1; + this.transparent = options.transparent || false; + + var ros = options.ros; + var topic = options.topic; + + var positions = new Float32Array(this.width * this.height * 3); + var colors = new Float32Array(this.width * this.height * 3); + + this.geometry = new THREE.BufferGeometry(); + + this.geometry.addAttribute('position', new THREE.BufferAttribute(positions, 3)); + this.geometry.addAttribute('color', new THREE.BufferAttribute(colors, 3)); + + this.mesh = new THREE.PointCloud( + this.geometry, + new THREE.PointCloudMaterial({ + size: this.pointSize / 1000.0, + opacity: this.opacity, + transparent: this.transparent, + vertexColors: THREE.VertexColors + }) + ); + + // If we don't clear this, the point cloud gets undrawn when we get too close with the camera, + // even if it doesn't seem close. + this.mesh.frustumCulled = false; + + // subscribe to the topic + this.rosTopic = new ROSLIB.Topic({ + ros : ros, + name : topic, + messageType : 'sensor_msgs/PointCloud2', + compression : 'png', + queue_length : this.queue_length, + throttle_rate : 500 + }); + +}; + +/** + * Start video playback + * @todo: move this code + coloring of the point cloud into a vertex+fragment shader. + * The message.data is a base64 encoded string, but also internally an Image object. + * Maybe the image can be passed to the vertex shader and decoded there, which would + * help in speeding up : receiving as png and no longer decompressing in javascript. + */ +ROS3D.PointCloud.prototype.startStream = function() { + var that = this, + position = that.geometry.attributes.position.array, + color = that.geometry.attributes.color.array; + + this.rosTopic.subscribe(function(message) { + var bufferView = new ROS3D.BufferView({ + data: message.data, + type: 'base64', + endian: ROS3D.LITTLE_ENDIAN + }); + var l = message.width * message.height, i = 0, i3 = 0; + + // Guard against empty pointcloud (zero points: zero message width or height). + if (l === 0) { + return; + } + + for (; i < that.width * that.height; i++, i3 += 3) { + + if ( i < l ) { + bufferView.resetSinglePointOffset(); + + position[i3] = bufferView.readFloat32(); + position[i3 + 1] = bufferView.readFloat32(); + position[i3 + 2] = bufferView.readFloat32(); + bufferView.incrementOffset(4); // skip empty channel + + switch (message.point_step) { + // message contains normals; colors are in the third channel + case 48: + bufferView.incrementOffset(16); + /* falls through */ + case 32: + color[i3 + 2] = bufferView.readUint8() / 255.0; // B + color[i3 + 1] = bufferView.readUint8() / 255.0; // G + color[i3] = bufferView.readUint8() / 255.0; // R + break; + // 16-byte point step messages don't have colors + case 16: + /* falls through */ + default: + } + + // adjust offset for the next point + bufferView.incrementOffset(message.point_step - bufferView.singlePointOffset); + } else { + // Converge all other points which should be invisible into the "last" point of the + // "visible" cloud (effectively reset) + position[i3] = position[i3 - 3].x; + position[i3 + 1] = position[i3 - 2].y; + position[i3 + 2] = position[i3 - 1].z; + } + } + that.geometry.attributes.position.needsUpdate = true; + that.geometry.attributes.color.needsUpdate = true; + }); +}; + +/** + * Stop video playback + */ +ROS3D.PointCloud.prototype.stopStream = function() { + this.rosTopic.unsubscribe(); +}; + +/** + * @author Peter Soetens - peter@pickit3d.com + * @author Viktor Kunovski - viktor@pickit3d.com + */ + +ROS3D.BIG_ENDIAN = 0; +ROS3D.LITTLE_ENDIAN = 1; + +ROS3D.BufferView = function(options) { + options = options || {}; + + this.endian = options.endian || ROS3D.BIG_ENDIAN; + this.type = options.type || 'base64'; + + var data = options.data || ''; + + this.view = new DataView(this.base64ToArrayBuffer(data)); + + this.offset = 0; + this.singlePointOffset = 0; +}; + +ROS3D.BufferView.prototype.readFloat32 = function() { + var value = this.view.getFloat32(this.offset, !!this.endian); + this.offset += 4; + this.singlePointOffset += 4; + return value; +}; + +ROS3D.BufferView.prototype.readUint8 = function() { + var value = this.view.getUint8(this.offset); + this.offset += 1; + this.singlePointOffset += 1; + return value; +}; + +ROS3D.BufferView.prototype.incrementOffset = function(offset) { + this.offset += +offset; + this.singlePointOffset += +offset; +}; + +ROS3D.BufferView.prototype.resetSinglePointOffset = function() { + this.singlePointOffset = 0; +}; + +ROS3D.BufferView.prototype.base64ToArrayBuffer = function(base64) { + var binaryString = window.atob(base64); + var len = binaryString.length; + var bytes = new Uint8Array(len); + for (var i = 0; i < len; i++) { + bytes[i] = binaryString.charCodeAt(i); + } + return bytes.buffer; +}; + /** * @author David Gossow - dgossow@willowgarage.com */ @@ -758,7 +1289,7 @@ ROS3D.InteractiveMarker.prototype.buttonClick = function(control, event3d) { * @param event3d - the event that caused this */ ROS3D.InteractiveMarker.prototype.setPosition = function(control, position) { - this.position = position; + this.position.copy( position ); this.feedbackEvent('user-pose-change', control); }; @@ -770,7 +1301,7 @@ ROS3D.InteractiveMarker.prototype.setPosition = function(control, position) { */ ROS3D.InteractiveMarker.prototype.setOrientation = function(control, orientation) { orientation.normalize(); - this.quaternion = orientation; + this.quaternion.copy( orientation ); this.feedbackEvent('user-pose-change', control); }; @@ -791,8 +1322,8 @@ ROS3D.InteractiveMarker.prototype.onServerSetPose = function(event) { this.position.y = pose.position.y; this.position.z = pose.position.z; - this.quaternion = new THREE.Quaternion(pose.orientation.x, pose.orientation.y, - pose.orientation.z, pose.orientation.w); + this.quaternion.copy( new THREE.Quaternion(pose.orientation.x, pose.orientation.y, + pose.orientation.z, pose.orientation.w) ); this.updateMatrixWorld(true); } @@ -1154,7 +1685,7 @@ ROS3D.InteractiveMarkerControl = function(options) { break; case ROS3D.INTERACTIVE_MARKER_FIXED: this.updateMatrixWorld = function(force) { - that.quaternion = that.parent.quaternion.clone().inverse(); + that.quaternion.copy( that.parent.quaternion.clone().inverse() ); that.updateMatrix(); that.matrixWorldNeedsUpdate = true; ROS3D.InteractiveMarkerControl.prototype.updateMatrixWorld.call(that, force); @@ -1721,7 +2252,7 @@ ROS3D.Marker = function(options) { break; case ROS3D.MARKER_CUBE: // set the cube dimensions - var cubeGeom = new THREE.CubeGeometry(message.scale.x, message.scale.y, message.scale.z); + var cubeGeom = new THREE.BoxGeometry(message.scale.x, message.scale.y, message.scale.z); this.add(new THREE.Mesh(cubeGeom, colorMaterial)); break; case ROS3D.MARKER_SPHERE: @@ -1738,13 +2269,13 @@ ROS3D.Marker = function(options) { var cylinderGeom = new THREE.CylinderGeometry(0.5, 0.5, 1, 16, 1, false); var cylinderMesh = new THREE.Mesh(cylinderGeom, colorMaterial); cylinderMesh.quaternion.setFromAxisAngle(new THREE.Vector3(1, 0, 0), Math.PI * 0.5); - cylinderMesh.scale = new THREE.Vector3(message.scale.x, message.scale.z, message.scale.y); + cylinderMesh.scale.set(message.scale.x, message.scale.z, message.scale.y); this.add(cylinderMesh); break; case ROS3D.MARKER_LINE_STRIP: + var lineStripGeom = new THREE.Geometry(); var lineStripMaterial = new THREE.LineBasicMaterial({ - size : message.scale.x }); // add the points @@ -1758,7 +2289,7 @@ ROS3D.Marker = function(options) { } // determine the colors for each - if (message.colors.length === message.points.length) { + if ( message.colors.length === message.points.length) { lineStripMaterial.vertexColors = true; for ( j = 0; j < message.points.length; j++) { var clr = new THREE.Color(); @@ -1775,7 +2306,6 @@ ROS3D.Marker = function(options) { case ROS3D.MARKER_LINE_LIST: var lineListGeom = new THREE.Geometry(); var lineListMaterial = new THREE.LineBasicMaterial({ - size : message.scale.x }); // add the points @@ -1816,7 +2346,7 @@ ROS3D.Marker = function(options) { // add the points var p, cube, curColor, newMesh; for (p = 0; p < numPoints; p+=stepSize) { - cube = new THREE.CubeGeometry(message.scale.x, message.scale.y, message.scale.z); + cube = new THREE.BoxGeometry(message.scale.x, message.scale.y, message.scale.z); // check the color if(createColors) { @@ -1870,7 +2400,7 @@ ROS3D.Marker = function(options) { case ROS3D.MARKER_POINTS: // for now, use a particle system for the lists var geometry = new THREE.Geometry(); - var material = new THREE.ParticleBasicMaterial({ + var material = new THREE.PointCloudMaterial({ size : message.scale.x }); @@ -1897,7 +2427,7 @@ ROS3D.Marker = function(options) { } // add the particle system - this.add(new THREE.ParticleSystem(geometry, material)); + this.add(new THREE.PointCloud(geometry, material)); break; case ROS3D.MARKER_TEXT_VIEW_FACING: // only work on non-empty text @@ -1966,7 +2496,7 @@ ROS3D.Marker = function(options) { vertices : message.points, colors : message.colors }); - tri.scale = new THREE.Vector3(message.scale.x, message.scale.y, message.scale.z); + tri.scale.set(message.scale.x, message.scale.y, message.scale.z); this.add(tri); break; default: @@ -1988,7 +2518,7 @@ ROS3D.Marker.prototype.setPose = function(pose) { this.position.z = pose.position.z; // set the rotation - this.quaternion = new THREE.Quaternion(pose.orientation.x, pose.orientation.y, + this.quaternion.set( pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); this.quaternion.normalize(); @@ -2182,8 +2712,10 @@ ROS3D.MarkerArrayClient.prototype.subscribe = function(){ ros : this.ros, name : this.topicName, messageType : 'visualization_msgs/MarkerArray', - compression : 'png' + compression : 'png', + queue_length : 2 }); + this.rosTopic.subscribe(this.processMessage.bind(this)); }; @@ -2205,7 +2737,7 @@ ROS3D.MarkerArrayClient.prototype.processMessage = function(arrayMessage){ loader : this.loader }); this.markers[message.ns + message.id] = new ROS3D.SceneNode({ - frameID : message.header.frame_id, + frameID : message.header.frame_id.replace(/^\//, ''), tfClient : this.tfClient, object : newMarker }); @@ -2216,9 +2748,11 @@ ROS3D.MarkerArrayClient.prototype.processMessage = function(arrayMessage){ console.warn('Received marker message with deprecated action identifier "1"'); } else if(message.action === 2) { // "DELETE" - this.markers[message.ns + message.id].unsubscribeTf(); - this.rootObject.remove(this.markers[message.ns + message.id]); - delete this.markers[message.ns + message.id]; + if(message.ns + message.id in this.markers) { + this.markers[message.ns + message.id].unsubscribeTf(); + this.rootObject.remove(this.markers[message.ns + message.id]); + delete this.markers[message.ns + message.id]; + } } else if(message.action === 3) { // "DELETE ALL" for (var m in this.markers){ @@ -2241,6 +2775,17 @@ ROS3D.MarkerArrayClient.prototype.unsubscribe = function(){ } }; +ROS3D.MarkerArrayClient.prototype.removeArray = function() { + this.rosTopic.unsubscribe(); + for (var key in this.markers) { + if (this.markers.hasOwnProperty(key)) { + this.markers[key].unsubscribeTf(); + this.rootObject.remove( this.markers[key] ); + } + } + this.markers = {}; +}; + /** * @author Russell Toris - rctoris@wpi.edu */ @@ -2314,7 +2859,7 @@ ROS3D.MarkerClient.prototype.processMessage = function(message){ } this.markers[message.ns + message.id] = new ROS3D.SceneNode({ - frameID : message.header.frame_id, + frameID : message.header.frame_id.replace(/^\//, ''), tfClient : this.tfClient, object : newMarker }); @@ -2366,11 +2911,11 @@ ROS3D.Arrow = function(options) { coneGeometry.applyMatrix(m); // put the arrow together - THREE.GeometryUtils.merge(geometry, coneGeometry); + geometry.merge(coneGeometry); THREE.Mesh.call(this, geometry, material); - this.position = origin; + this.position.copy( origin ); this.setDirection(direction); }; ROS3D.Arrow.prototype.__proto__ = THREE.Mesh.prototype; @@ -2504,9 +3049,12 @@ ROS3D.Axes = function(options) { var shaftRadius = options.shaftRadius || 0.008; var headRadius = options.headRadius || 0.023; var headLength = options.headLength || 0.1; + var scaleArg = options.scale || 1.0; THREE.Object3D.call(this); + this.scale.set(scaleArg,scaleArg,scaleArg); + // create the cylinders for the objects this.lineGeom = new THREE.CylinderGeometry(shaftRadius, shaftRadius, 1.0 - headLength); this.headGeom = new THREE.CylinderGeometry(0, headRadius, headLength); @@ -2532,17 +3080,17 @@ ROS3D.Axes = function(options) { // create the arrow var arrow = new THREE.Mesh(that.headGeom, material); - arrow.position = axis.clone(); + arrow.position.copy( axis ); arrow.position.multiplyScalar(0.95); - arrow.quaternion = rot; + arrow.quaternion.copy( rot ); arrow.updateMatrix(); that.add(arrow); // create the line var line = new THREE.Mesh(that.lineGeom, material); - line.position = axis.clone(); + line.position.copy( axis ); line.position.multiplyScalar(0.45); - line.quaternion = rot; + line.quaternion.copy( rot ); line.updateMatrix(); that.add(line); } @@ -2658,7 +3206,7 @@ ROS3D.MeshResource = function(options) { // check for a scale factor in ColladaLoader2 if(loaderType === ROS3D.COLLADA_LOADER_2 && collada.dae.asset.unit) { var scale = collada.dae.asset.unit; - collada.scene.scale = new THREE.Vector3(scale, scale, scale); + collada.scene.scale.set(scale, scale, scale); } // add a texture to anything that is missing one @@ -2758,7 +3306,6 @@ ROS3D.TriangleList = function(options) { geometry.computeBoundingBox(); geometry.computeBoundingSphere(); - geometry.computeCentroids(); geometry.computeFaceNormals(); this.add(new THREE.Mesh(geometry, material)); @@ -3695,7 +4242,7 @@ ROS3D.Particles = function(options) { transparent: true, }); - this.ps = new THREE.ParticleSystem( this.geom, this.shaderMaterial ); + this.ps = new THREE.PointCloud( this.geom, this.shaderMaterial ); this.sn = null; this.points = this.geom.vertices; @@ -3903,7 +4450,7 @@ ROS3D.Urdf = function(options) { // check for a scale if(link.visuals[i].geometry.scale) { - mesh.scale = new THREE.Vector3( + mesh.scale.set( visual.geometry.scale.x, visual.geometry.scale.y, visual.geometry.scale.z @@ -3930,7 +4477,7 @@ ROS3D.Urdf = function(options) { switch (visual.geometry.type) { case ROSLIB.URDF_BOX: var dimension = visual.geometry.dimension; - var cube = new THREE.CubeGeometry(dimension.x, dimension.y, dimension.z); + var cube = new THREE.BoxGeometry(dimension.x, dimension.y, dimension.z); shapeMesh = new THREE.Mesh(cube, colorMaterial); break; case ROSLIB.URDF_CYLINDER: @@ -4101,6 +4648,10 @@ ROS3D.Highlighter.prototype.renderHighlight = function(renderer, scene, camera) var renderList = []; this.getWebglObjects(scene, this.hoverObjs, renderList); + if ( renderList.length === 0 ) { + return; + } + // define highlight material scene.overrideMaterial = new THREE.MeshBasicMaterial({ fog : false, @@ -4145,7 +4696,6 @@ ROS3D.MouseHandler = function(options) { this.fallbackTarget = options.fallbackTarget; this.lastTarget = this.fallbackTarget; this.dragging = false; - this.projector = new THREE.Projector(); // listen to DOM events var eventNames = [ 'contextmenu', 'click', 'dblclick', 'mouseout', 'mousedown', 'mouseup', @@ -4193,7 +4743,7 @@ ROS3D.MouseHandler.prototype.processDomEvent = function(domEvent) { var deviceX = left / target.clientWidth * 2 - 1; var deviceY = -top / target.clientHeight * 2 + 1; var vector = new THREE.Vector3(deviceX, deviceY, 0.5); - this.projector.unprojectVector(vector, this.camera); + vector.unproject(this.camera); // THREE r69 // use the THREE raycaster var mouseRaycaster = new THREE.Raycaster(this.camera.position.clone(), vector.sub( this.camera.position).normalize()); @@ -4448,14 +4998,15 @@ ROS3D.OrbitControls = function(options) { moveStartIntersection = intersectViewPlane(event3D.mouseRay, moveStartCenter, moveStartNormal); + this.showAxes(); break; case 2: state = STATE.ZOOM; zoomStart.set(event.clientX, event.clientY); + this.showAxes(); break; } - this.showAxes(); } /** @@ -4474,7 +5025,7 @@ ROS3D.OrbitControls = function(options) { that.rotateUp(2 * Math.PI * rotateDelta.y / pixelsPerRound * that.userRotateSpeed); rotateStart.copy(rotateEnd); - this.showAxes(); + } else if (state === STATE.ZOOM) { zoomEnd.set(event.clientX, event.clientY); zoomDelta.subVectors(zoomEnd, zoomStart); @@ -4566,9 +5117,9 @@ ROS3D.OrbitControls = function(options) { delta = -event.detail; } if (delta > 0) { - that.zoomIn(); - } else { that.zoomOut(); + } else { + that.zoomIn(); } this.showAxes(); @@ -4715,6 +5266,7 @@ ROS3D.OrbitControls.prototype.showAxes = function() { this.axes.traverse(function(obj) { obj.visible = true; }); + if (this.hideTimeout) { clearTimeout(this.hideTimeout); } @@ -4723,7 +5275,7 @@ ROS3D.OrbitControls.prototype.showAxes = function() { obj.visible = false; }); that.hideTimeout = false; - }, 1000); + }, 300); }; /** @@ -4824,9 +5376,11 @@ ROS3D.OrbitControls.prototype.update = function() { phi = Math.max(eps, Math.min(Math.PI - eps, phi)); var radius = offset.length(); - offset.y = radius * Math.sin(phi) * Math.sin(theta); - offset.z = radius * Math.cos(phi); - offset.x = radius * Math.sin(phi) * Math.cos(theta); + offset.set( + radius * Math.sin(phi) * Math.cos(theta), + radius * Math.sin(phi) * Math.sin(theta), + radius * Math.cos(phi) + ); offset.multiplyScalar(this.scale); position.copy(this.center).add(offset); @@ -4834,8 +5388,8 @@ ROS3D.OrbitControls.prototype.update = function() { this.camera.lookAt(this.center); radius = offset.length(); - this.axes.position = this.center.clone(); - this.axes.scale.x = this.axes.scale.y = this.axes.scale.z = radius * 0.05; + this.axes.position.copy( this.center ); + this.axes.scale.set( radius * 0.05, radius * 0.05, radius *0.05); this.axes.updateMatrixWorld(true); this.thetaDelta = 0; @@ -4863,7 +5417,7 @@ THREE.EventDispatcher.prototype.apply( ROS3D.OrbitControls.prototype ); * @constructor * @param options - object with following keys: * - * * tfClient - a handle to the TF client + * * tfClient (optional) - a handle to the TF client * * frameID - the frame ID this object belongs to * * pose (optional) - the pose associated with this object * * object - the THREE 3D object to be rendered @@ -4871,7 +5425,7 @@ THREE.EventDispatcher.prototype.apply( ROS3D.OrbitControls.prototype ); ROS3D.SceneNode = function(options) { options = options || {}; var that = this; - this.tfClient = options.tfClient; + this.tfClient = options.tfClient || null; this.frameID = options.frameID; var object = options.object; this.pose = options.pose || new ROSLIB.Pose(); @@ -4881,26 +5435,21 @@ ROS3D.SceneNode = function(options) { this.visible = false; // add the model - this.add(object); - + if (object) { + this.add(object); + } // set the inital pose this.updatePose(this.pose); // save the TF handler so we can remove it later this.tfUpdate = function(msg) { - - // apply the transform - var tf = new ROSLIB.Transform(msg); - var poseTransformed = new ROSLIB.Pose(that.pose); - poseTransformed.applyTransform(tf); - - // update the world - that.updatePose(poseTransformed); - that.visible = true; + that.transformPose(msg); }; // listen for TF updates - this.tfClient.subscribe(this.frameID, this.tfUpdate); + if (this.tfClient) { + this.tfClient.subscribe(this.frameID, this.tfUpdate); + } }; ROS3D.SceneNode.prototype.__proto__ = THREE.Object3D.prototype; @@ -4917,7 +5466,24 @@ ROS3D.SceneNode.prototype.updatePose = function(pose) { }; ROS3D.SceneNode.prototype.unsubscribeTf = function() { - this.tfClient.unsubscribe(this.frameID, this.tfUpdate); + if (this.tfClient) { + this.tfClient.unsubscribe(this.frameID, this.tfUpdate); + } +}; + +/** + * Transform the pose of the associated model. + * @param transform - A ROS Transform like object which has a translation and orientation property. + */ +ROS3D.SceneNode.prototype.transformPose = function(transform) { + // apply the transform + var tf = new ROSLIB.Transform( transform ); + var poseTransformed = new ROSLIB.Pose(this.pose); + poseTransformed.applyTransform(tf); + + // update the world + this.updatePose(poseTransformed); + this.visible = true; }; /** @@ -4944,6 +5510,10 @@ ROS3D.SceneNode.prototype.unsubscribeTf = function() { ROS3D.Viewer = function(options) { options = options || {}; var divID = options.divID; + var canvas = (!!options.canvas && + options.canvas.nodeName.toLowerCase() === 'canvas') + ? options.canvas + : undefined; var width = options.width; var height = options.height; var background = options.background || '#111111'; @@ -4957,10 +5527,11 @@ ROS3D.Viewer = function(options) { y : 3, z : 3 }; - var cameraZoomSpeed = options.cameraZoomSpeed || 0.5; + var cameraZoomSpeed = options.cameraZoomSpeed || 2.5; // create the canvas to render to this.renderer = new THREE.WebGLRenderer({ + canvas: canvas, antialias : antialias, alpha: true }); @@ -4975,9 +5546,8 @@ ROS3D.Viewer = function(options) { // create the global camera this.camera = new THREE.PerspectiveCamera(40, width / height, near, far); - this.camera.position.x = cameraPosition.x; - this.camera.position.y = cameraPosition.y; - this.camera.position.z = cameraPosition.z; + this.camera.position.set( cameraPosition.x, cameraPosition.y, cameraPosition.z ); + // add controls to the camera this.cameraControls = new ROS3D.OrbitControls({ scene : this.scene, @@ -5009,7 +5579,11 @@ ROS3D.Viewer = function(options) { this.animationRequestId = undefined; // add the renderer to the page - document.getElementById(divID).appendChild(this.renderer.domElement); + if (divID) { + document.getElementById(divID).appendChild(this.renderer.domElement); + } else if (!canvas) { + throw new Error('No canvas nor HTML container provided for rendering.'); + } // begin the render loop this.start(); @@ -5036,7 +5610,7 @@ ROS3D.Viewer.prototype.draw = function(){ this.cameraControls.update(); // put light to the top-left of the camera - this.directionalLight.position = this.camera.localToWorld(new THREE.Vector3(-1, 1, 0)); + this.directionalLight.position.copy( this.camera.localToWorld(new THREE.Vector3(-1, 1, 0)) ); this.directionalLight.position.normalize(); // set the scene @@ -5044,7 +5618,7 @@ ROS3D.Viewer.prototype.draw = function(){ this.renderer.render(this.scene, this.camera); // render any mouseovers - this.highlighter.renderHighlight(this.renderer, this.scene, this.camera); + //this.highlighter.renderHighlight(this.renderer, this.scene, this.camera); // draw the frame this.animationRequestId = requestAnimationFrame(this.draw.bind(this)); diff --git a/build/ros3d.min.js b/build/ros3d.min.js index f36d3908..6192cbea 100644 --- a/build/ros3d.min.js +++ b/build/ros3d.min.js @@ -1,3 +1,3 @@ -function setFrame(a,b){null===a.sn&&(a.sn=new ROS3D.SceneNode({frameID:b,tfClient:a.tfClient,object:a.ps}),a.rootObject.add(a.sn))}function finishedUpdate(a,b){null===a.first_size&&(a.first_size=b,a.max_pts=Math.max(a.max_pts,b));for(var c=b;ca.max_pts&&console.error("Attempted to draw more points than max_pts allows")}function read_point(a,b,c){for(var d=[],e=a.point_step*b,f=0;f=8&&(d-=8,b.push(c>>d),c&=Math.pow(2,d)-1),c<<=6;return b}var ROS3D=ROS3D||{REVISION:"0.17.0-SNAPSHOT"};ROS3D.MARKER_ARROW=0,ROS3D.MARKER_CUBE=1,ROS3D.MARKER_SPHERE=2,ROS3D.MARKER_CYLINDER=3,ROS3D.MARKER_LINE_STRIP=4,ROS3D.MARKER_LINE_LIST=5,ROS3D.MARKER_CUBE_LIST=6,ROS3D.MARKER_SPHERE_LIST=7,ROS3D.MARKER_POINTS=8,ROS3D.MARKER_TEXT_VIEW_FACING=9,ROS3D.MARKER_MESH_RESOURCE=10,ROS3D.MARKER_TRIANGLE_LIST=11,ROS3D.INTERACTIVE_MARKER_KEEP_ALIVE=0,ROS3D.INTERACTIVE_MARKER_POSE_UPDATE=1,ROS3D.INTERACTIVE_MARKER_MENU_SELECT=2,ROS3D.INTERACTIVE_MARKER_BUTTON_CLICK=3,ROS3D.INTERACTIVE_MARKER_MOUSE_DOWN=4,ROS3D.INTERACTIVE_MARKER_MOUSE_UP=5,ROS3D.INTERACTIVE_MARKER_NONE=0,ROS3D.INTERACTIVE_MARKER_MENU=1,ROS3D.INTERACTIVE_MARKER_BUTTON=2,ROS3D.INTERACTIVE_MARKER_MOVE_AXIS=3,ROS3D.INTERACTIVE_MARKER_MOVE_PLANE=4,ROS3D.INTERACTIVE_MARKER_ROTATE_AXIS=5,ROS3D.INTERACTIVE_MARKER_MOVE_ROTATE=6,ROS3D.INTERACTIVE_MARKER_INHERIT=0,ROS3D.INTERACTIVE_MARKER_FIXED=1,ROS3D.INTERACTIVE_MARKER_VIEW_FACING=2,ROS3D.COLLADA_LOADER=1,ROS3D.COLLADA_LOADER_2=2,ROS3D.makeColorMaterial=function(a,b,c,d){var e=new THREE.Color;return e.setRGB(a,b,c),d<=.99?new THREE.MeshBasicMaterial({color:e.getHex(),opacity:d+.1,transparent:!0,depthWrite:!0,blendSrc:THREE.SrcAlphaFactor,blendDst:THREE.OneMinusSrcAlphaFactor,blendEquation:THREE.ReverseSubtractEquation,blending:THREE.NormalBlending}):new THREE.MeshPhongMaterial({color:e.getHex(),opacity:d,blending:THREE.NormalBlending})},ROS3D.intersectPlane=function(a,b,c){var d=new THREE.Vector3,e=new THREE.Vector3;d.subVectors(b,a.origin);var f=a.direction.dot(c);if(!(Math.abs(f)0.99)"," {"," vec4 depthColor2 = texture2D( map, vUv2 );"," float depth2 = ( depthColor2.r + depthColor2.g + depthColor2.b ) / 3.0 ;"," depth = 0.99+depth2;"," }"," "," return depth;"," }","","float median(float a, float b, float c)"," {"," float r=a;"," "," if ( (a0.5) || (vUvP.y<0.5) || (vUvP.y>0.0))"," {"," vec2 smp = decodeDepth(vec2(position.x, position.y));"," float depth = smp.x;"," depthVariance = smp.y;"," "," float z = -depth;"," "," pos = vec4("," ( position.x / width - 0.5 ) * z * (1000.0/focallength) * -1.0,"," ( position.y / height - 0.5 ) * z * (1000.0/focallength),"," (- z + zOffset / 1000.0) * 2.0,"," 1.0);"," "," vec2 maskP = vec2( position.x / (width*2.0), position.y / (height*2.0) );"," vec4 maskColor = texture2D( map, maskP );"," maskVal = ( maskColor.r + maskColor.g + maskColor.b ) / 3.0 ;"," }"," "," gl_PointSize = pointSize;"," gl_Position = projectionMatrix * modelViewMatrix * pos;"," ","}"].join("\n"),this.fragment_shader=["uniform sampler2D map;","uniform float varianceThreshold;","uniform float whiteness;","","varying vec2 vUvP;","varying vec2 colorP;","","varying float depthVariance;","varying float maskVal;","","","void main() {"," "," vec4 color;"," "," if ( (depthVariance>varianceThreshold) || (maskVal>0.5) ||(vUvP.x<0.0)|| (vUvP.x>0.5) || (vUvP.y<0.5) || (vUvP.y>1.0))"," { "," discard;"," }"," else "," {"," color = texture2D( map, colorP );"," "," float fader = whiteness /100.0;"," "," color.r = color.r * (1.0-fader)+ fader;"," "," color.g = color.g * (1.0-fader)+ fader;"," "," color.b = color.b * (1.0-fader)+ fader;"," "," color.a = 1.0;//smoothstep( 20000.0, -20000.0, gl_FragCoord.z / gl_FragCoord.w );"," }"," "," gl_FragColor = vec4( color.r, color.g, color.b, color.a );"," ","}"].join("\n")},ROS3D.DepthCloud.prototype.__proto__=THREE.Object3D.prototype,ROS3D.DepthCloud.prototype.metaLoaded=function(){this.metaLoaded=!0,this.initStreamer()},ROS3D.DepthCloud.prototype.initStreamer=function(){if(this.metaLoaded){this.texture=new THREE.Texture(this.video),this.geometry=new THREE.Geometry;for(var a=0,b=this.width*this.height;a0&&(this.menu=new ROS3D.InteractiveMarkerMenu({menuEntries:c.menuEntries,menuFontSize:c.menuFontSize}),this.menu.addEventListener("menu-select",function(a){b.dispatchEvent(a)}))},ROS3D.InteractiveMarker.prototype.__proto__=THREE.Object3D.prototype,ROS3D.InteractiveMarker.prototype.showMenu=function(a,b){this.menu&&this.menu.show(a,b)},ROS3D.InteractiveMarker.prototype.moveAxis=function(a,b,c){if(this.dragging){var d=a.currentControlOri,e=b.clone().applyQuaternion(d),f=this.dragStart.event3d.intersection.point,g=e.clone().applyQuaternion(this.dragStart.orientationWorld.clone()),h=new THREE.Ray(f,g),i=ROS3D.closestAxisPoint(h,c.camera,c.mousePos),j=new THREE.Vector3;j.addVectors(this.dragStart.position,e.clone().applyQuaternion(this.dragStart.orientation).multiplyScalar(i)),this.setPosition(a,j),c.stopPropagation()}},ROS3D.InteractiveMarker.prototype.movePlane=function(a,b,c){if(this.dragging){var d=a.currentControlOri,e=b.clone().applyQuaternion(d),f=this.dragStart.event3d.intersection.point,g=e.clone().applyQuaternion(this.dragStart.orientationWorld),h=ROS3D.intersectPlane(c.mouseRay,f,g),i=new THREE.Vector3;i.subVectors(h,f),i.add(this.dragStart.positionWorld),this.setPosition(a,i),c.stopPropagation()}},ROS3D.InteractiveMarker.prototype.rotateAxis=function(a,b,c){if(this.dragging){a.updateMatrixWorld();var d=a.currentControlOri,e=d.clone().multiply(b.clone()),f=new THREE.Vector3(1,0,0).applyQuaternion(e),g=this.dragStart.event3d.intersection.point,h=f.applyQuaternion(this.dragStart.orientationWorld),i=ROS3D.intersectPlane(c.mouseRay,g,h),j=new THREE.Ray(this.dragStart.positionWorld,h),k=ROS3D.intersectPlane(j,g,h),l=this.dragStart.orientationWorld.clone().multiply(e),m=l.clone().inverse();i.sub(k),i.applyQuaternion(m);var n=this.dragStart.event3d.intersection.point.clone();n.sub(k),n.applyQuaternion(m);var o=Math.atan2(i.y,i.z),p=Math.atan2(n.y,n.z),q=p-o,r=new THREE.Quaternion;r.setFromAxisAngle(f,q),this.setOrientation(a,r.multiply(this.dragStart.orientationWorld)),c.stopPropagation()}},ROS3D.InteractiveMarker.prototype.feedbackEvent=function(a,b){this.dispatchEvent({type:a,position:this.position.clone(),orientation:this.quaternion.clone(),controlName:b.name})},ROS3D.InteractiveMarker.prototype.startDrag=function(a,b){if(0===b.domEvent.button){b.stopPropagation(),this.dragging=!0,this.updateMatrixWorld(!0);var c=new THREE.Vector3;this.matrixWorld.decompose(this.dragStart.positionWorld,this.dragStart.orientationWorld,c),this.dragStart.position=this.position.clone(),this.dragStart.orientation=this.quaternion.clone(),this.dragStart.event3d=b,this.feedbackEvent("user-mousedown",a)}},ROS3D.InteractiveMarker.prototype.stopDrag=function(a,b){0===b.domEvent.button&&(b.stopPropagation(),this.dragging=!1,this.dragStart.event3d={},this.onServerSetPose(this.bufferedPoseEvent),this.bufferedPoseEvent=void 0,this.feedbackEvent("user-mouseup",a))},ROS3D.InteractiveMarker.prototype.buttonClick=function(a,b){b.stopPropagation(),this.feedbackEvent("user-button-click",a)},ROS3D.InteractiveMarker.prototype.setPosition=function(a,b){this.position=b,this.feedbackEvent("user-pose-change",a)},ROS3D.InteractiveMarker.prototype.setOrientation=function(a,b){b.normalize(),this.quaternion=b,this.feedbackEvent("user-pose-change",a)},ROS3D.InteractiveMarker.prototype.onServerSetPose=function(a){if(void 0!==a)if(this.dragging)this.bufferedPoseEvent=a;else{var b=a.pose;this.position.x=b.position.x,this.position.y=b.position.y,this.position.z=b.position.z,this.quaternion=new THREE.Quaternion(b.orientation.x,b.orientation.y,b.orientation.z,b.orientation.w),this.updateMatrixWorld(!0)}},ROS3D.InteractiveMarker.prototype.dispose=function(){var a=this;this.children.forEach(function(b){b.children.forEach(function(a){a.dispose(),b.remove(a)}),a.remove(b)})},THREE.EventDispatcher.prototype.apply(ROS3D.InteractiveMarker.prototype),ROS3D.InteractiveMarkerClient=function(a){a=a||{},this.ros=a.ros,this.tfClient=a.tfClient,this.topicName=a.topic,this.path=a.path||"/",this.camera=a.camera,this.rootObject=a.rootObject||new THREE.Object3D,this.loader=a.loader||ROS3D.COLLADA_LOADER_2,this.menuFontSize=a.menuFontSize||"0.8em",this.interactiveMarkers={},this.updateTopic=null,this.feedbackTopic=null,this.topicName&&this.subscribe(this.topicName)},ROS3D.InteractiveMarkerClient.prototype.subscribe=function(a){this.unsubscribe(),this.updateTopic=new ROSLIB.Topic({ros:this.ros,name:a+"/tunneled/update",messageType:"visualization_msgs/InteractiveMarkerUpdate",compression:"png"}),this.updateTopic.subscribe(this.processUpdate.bind(this)),this.feedbackTopic=new ROSLIB.Topic({ros:this.ros,name:a+"/feedback",messageType:"visualization_msgs/InteractiveMarkerFeedback",compression:"png"}),this.feedbackTopic.advertise(),this.initService=new ROSLIB.Service({ros:this.ros,name:a+"/tunneled/get_init",serviceType:"demo_interactive_markers/GetInit"});var b=new ROSLIB.ServiceRequest({});this.initService.callService(b,this.processInit.bind(this))},ROS3D.InteractiveMarkerClient.prototype.unsubscribe=function(){this.updateTopic&&this.updateTopic.unsubscribe(),this.feedbackTopic&&this.feedbackTopic.unadvertise();for(var a in this.interactiveMarkers)this.eraseIntMarker(a);this.interactiveMarkers={}},ROS3D.InteractiveMarkerClient.prototype.processInit=function(a){var b=a.msg;b.erases=[];for(var c in this.interactiveMarkers)b.erases.push(c);b.poses=[],this.processUpdate(b)},ROS3D.InteractiveMarkerClient.prototype.processUpdate=function(a){var b=this;a.erases.forEach(function(a){b.eraseIntMarker(a)}),a.poses.forEach(function(a){var c=b.interactiveMarkers[a.name];c&&c.setPoseFromServer(a.pose)}),a.markers.forEach(function(a){var c=b.interactiveMarkers[a.name];c&&b.eraseIntMarker(c.name);var d=new ROS3D.InteractiveMarkerHandle({message:a,feedbackTopic:b.feedbackTopic,tfClient:b.tfClient,menuFontSize:b.menuFontSize});b.interactiveMarkers[a.name]=d;var e=new ROS3D.InteractiveMarker({handle:d,camera:b.camera,path:b.path,loader:b.loader});e.name=a.name,b.rootObject.add(e),d.on("pose",function(a){e.onServerSetPose({pose:a})}),e.addEventListener("user-pose-change",d.setPoseFromClientBound),e.addEventListener("user-mousedown",d.onMouseDownBound),e.addEventListener("user-mouseup",d.onMouseUpBound),e.addEventListener("user-button-click",d.onButtonClickBound),e.addEventListener("menu-select",d.onMenuSelectBound),d.subscribeTf()})},ROS3D.InteractiveMarkerClient.prototype.eraseIntMarker=function(a){if(this.interactiveMarkers[a]){var b=this.rootObject.getObjectByName(a);this.rootObject.remove(b);var c=this.interactiveMarkers[a];c.unsubscribeTf(),b.removeEventListener("user-pose-change",c.setPoseFromClientBound),b.removeEventListener("user-mousedown",c.onMouseDownBound),b.removeEventListener("user-mouseup",c.onMouseUpBound),b.removeEventListener("user-button-click",c.onButtonClickBound),b.removeEventListener("menu-select",c.onMenuSelectBound),delete this.interactiveMarkers[a],b.dispose()}},ROS3D.InteractiveMarkerControl=function(a){function b(a){a.stopPropagation()}var c=this;THREE.Object3D.call(this),a=a||{},this.parent=a.parent;var d=a.handle,e=a.message;this.name=e.name,this.camera=a.camera,this.path=a.path||"/",this.loader=a.loader||ROS3D.COLLADA_LOADER_2,this.dragging=!1,this.startMousePos=new THREE.Vector2;var f=new THREE.Quaternion(e.orientation.x,e.orientation.y,e.orientation.z,e.orientation.w);f.normalize();var g=new THREE.Vector3(1,0,0);switch(g.applyQuaternion(f),this.currentControlOri=new THREE.Quaternion,e.interaction_mode){case ROS3D.INTERACTIVE_MARKER_MOVE_AXIS:this.addEventListener("mousemove",this.parent.moveAxis.bind(this.parent,this,g)),this.addEventListener("touchmove",this.parent.moveAxis.bind(this.parent,this,g));break;case ROS3D.INTERACTIVE_MARKER_ROTATE_AXIS:this.addEventListener("mousemove",this.parent.rotateAxis.bind(this.parent,this,f));break;case ROS3D.INTERACTIVE_MARKER_MOVE_PLANE:this.addEventListener("mousemove",this.parent.movePlane.bind(this.parent,this,g));break;case ROS3D.INTERACTIVE_MARKER_BUTTON:this.addEventListener("click",this.parent.buttonClick.bind(this.parent,this))}e.interaction_mode!==ROS3D.INTERACTIVE_MARKER_NONE&&(this.addEventListener("mousedown",this.parent.startDrag.bind(this.parent,this)),this.addEventListener("mouseup",this.parent.stopDrag.bind(this.parent,this)),this.addEventListener("contextmenu",this.parent.showMenu.bind(this.parent,this)),this.addEventListener("mouseup",function(a){0===c.startMousePos.distanceToSquared(a.mousePos)&&(a.type="contextmenu",c.dispatchEvent(a))}),this.addEventListener("mouseover",b),this.addEventListener("mouseout",b),this.addEventListener("click",b),this.addEventListener("mousedown",function(a){c.startMousePos=a.mousePos}),this.addEventListener("touchstart",function(a){1===a.domEvent.touches.length&&(a.type="mousedown",a.domEvent.button=0,c.dispatchEvent(a))}),this.addEventListener("touchmove",function(a){1===a.domEvent.touches.length&&(a.type="mousemove",a.domEvent.button=0,c.dispatchEvent(a))}),this.addEventListener("touchend",function(a){0===a.domEvent.touches.length&&(a.domEvent.button=0,a.type="mouseup",c.dispatchEvent(a),a.type="click",c.dispatchEvent(a))}));var h=new THREE.Quaternion,i=this.parent.position.clone().multiplyScalar(-1);switch(e.orientation_mode){case ROS3D.INTERACTIVE_MARKER_INHERIT:h=this.parent.quaternion.clone().inverse(),this.updateMatrixWorld=function(a){ROS3D.InteractiveMarkerControl.prototype.updateMatrixWorld.call(c,a),c.currentControlOri.copy(c.quaternion),c.currentControlOri.normalize()};break;case ROS3D.INTERACTIVE_MARKER_FIXED:this.updateMatrixWorld=function(a){c.quaternion=c.parent.quaternion.clone().inverse(),c.updateMatrix(),c.matrixWorldNeedsUpdate=!0,ROS3D.InteractiveMarkerControl.prototype.updateMatrixWorld.call(c,a),c.currentControlOri.copy(c.quaternion)};break;case ROS3D.INTERACTIVE_MARKER_VIEW_FACING:var j=e.independent_marker_orientation;this.updateMatrixWorld=function(a){c.camera.updateMatrixWorld();var b=(new THREE.Matrix4).extractRotation(c.camera.matrixWorld),d=new THREE.Matrix4,e=.5*Math.PI,f=new THREE.Euler(-e,0,e);d.makeRotationFromEuler(f);var g=new THREE.Matrix4;g.getInverse(c.parent.matrixWorld),b.multiplyMatrices(b,d),b.multiplyMatrices(g,b),c.currentControlOri.setFromRotationMatrix(b),j||(c.quaternion.copy(c.currentControlOri),c.updateMatrix(),c.matrixWorldNeedsUpdate=!0),ROS3D.InteractiveMarkerControl.prototype.updateMatrixWorld.call(c,a)};break;default:console.error("Unkown orientation mode: "+e.orientation_mode)}var k=new ROSLIB.TFClient({ros:d.tfClient.ros,fixedFrame:d.message.header.frame_id,serverName:d.tfClient.serverName});e.markers.forEach(function(a){var b=function(b){var d=new ROS3D.Marker({message:a,path:c.path,loader:c.loader});if(null!==b){var e=new ROSLIB.Pose({position:d.position,orientation:d.quaternion});e.applyTransform(new ROSLIB.Transform(b));var f=new ROS3D.Marker({message:a,path:c.path,loader:c.loader});f.position.add(i),f.position.applyQuaternion(h),f.quaternion.multiplyQuaternions(h,f.quaternion);var g=new THREE.Vector3(f.position.x,f.position.y,f.position.z),j=new ROSLIB.Transform({translation:g,orientation:f.quaternion});e.applyTransform(j),d.setPose(e),d.updateMatrixWorld(),k.unsubscribe(a.header.frame_id)}c.add(d)};""!==a.header.frame_id?k.subscribe(a.header.frame_id,b):b(null)})},ROS3D.InteractiveMarkerControl.prototype.__proto__=THREE.Object3D.prototype,ROS3D.InteractiveMarkerHandle=function(a){a=a||{},this.message=a.message,this.feedbackTopic=a.feedbackTopic,this.tfClient=a.tfClient,this.menuFontSize=a.menuFontSize||"0.8em",this.name=this.message.name,this.header=this.message.header,this.controls=this.message.controls,this.menuEntries=this.message.menu_entries,this.dragging=!1,this.timeoutHandle=null,this.tfTransform=new ROSLIB.Transform,this.pose=new ROSLIB.Pose,this.setPoseFromClientBound=this.setPoseFromClient.bind(this),this.onMouseDownBound=this.onMouseDown.bind(this),this.onMouseUpBound=this.onMouseUp.bind(this),this.onButtonClickBound=this.onButtonClick.bind(this),this.onMenuSelectBound=this.onMenuSelect.bind(this),this.setPoseFromServer(this.message.pose),this.tfUpdateBound=this.tfUpdate.bind(this)},ROS3D.InteractiveMarkerHandle.prototype.__proto__=EventEmitter2.prototype,ROS3D.InteractiveMarkerHandle.prototype.subscribeTf=function(){0===this.message.header.stamp.secs&&0===this.message.header.stamp.nsecs&&this.tfClient.subscribe(this.message.header.frame_id,this.tfUpdateBound)},ROS3D.InteractiveMarkerHandle.prototype.unsubscribeTf=function(){this.tfClient.unsubscribe(this.message.header.frame_id,this.tfUpdateBound)},ROS3D.InteractiveMarkerHandle.prototype.emitServerPoseUpdate=function(){var a=new ROSLIB.Pose(this.pose);a.applyTransform(this.tfTransform),this.emit("pose",a)},ROS3D.InteractiveMarkerHandle.prototype.setPoseFromServer=function(a){this.pose=new ROSLIB.Pose(a),this.emitServerPoseUpdate()},ROS3D.InteractiveMarkerHandle.prototype.tfUpdate=function(a){this.tfTransform=new ROSLIB.Transform(a),this.emitServerPoseUpdate()},ROS3D.InteractiveMarkerHandle.prototype.setPoseFromClient=function(a){this.pose=new ROSLIB.Pose(a);var b=this.tfTransform.clone();b.rotation.invert(),b.translation.multiplyQuaternion(b.rotation),b.translation.x*=-1,b.translation.y*=-1,b.translation.z*=-1,this.pose.applyTransform(b),this.sendFeedback(ROS3D.INTERACTIVE_MARKER_POSE_UPDATE,void 0,0,a.controlName),this.dragging&&(this.timeoutHandle&&clearTimeout(this.timeoutHandle),this.timeoutHandle=setTimeout(this.setPoseFromClient.bind(this,a),250))},ROS3D.InteractiveMarkerHandle.prototype.onButtonClick=function(a){this.sendFeedback(ROS3D.INTERACTIVE_MARKER_BUTTON_CLICK,a.clickPosition,0,a.controlName)},ROS3D.InteractiveMarkerHandle.prototype.onMouseDown=function(a){this.sendFeedback(ROS3D.INTERACTIVE_MARKER_MOUSE_DOWN,a.clickPosition,0,a.controlName),this.dragging=!0},ROS3D.InteractiveMarkerHandle.prototype.onMouseUp=function(a){this.sendFeedback(ROS3D.INTERACTIVE_MARKER_MOUSE_UP,a.clickPosition,0,a.controlName),this.dragging=!1,this.timeoutHandle&&clearTimeout(this.timeoutHandle)},ROS3D.InteractiveMarkerHandle.prototype.onMenuSelect=function(a){this.sendFeedback(ROS3D.INTERACTIVE_MARKER_MENU_SELECT,void 0,a.id,a.controlName)},ROS3D.InteractiveMarkerHandle.prototype.sendFeedback=function(a,b,c,d){var e=void 0!==b;b=b||{x:0,y:0,z:0};var f={header:this.header,client_id:this.clientID,marker_name:this.name,control_name:d,event_type:a,pose:this.pose,mouse_point:b,mouse_point_valid:e,menu_entry_id:c};this.feedbackTopic.publish(f)},ROS3D.InteractiveMarkerMenu=function(a){function b(a,b){this.dispatchEvent({type:"menu-select",domEvent:b,id:a.id,controlName:this.controlName}),this.hide(b)}function c(a,e){var f=document.createElement("ul");a.appendChild(f);for(var g=e.children,h=0;h0?(c(i,g[h]),j.addEventListener("click",d.hide.bind(d)),j.addEventListener("touchstart",d.hide.bind(d))):(j.addEventListener("click",b.bind(d,g[h])),j.addEventListener("touchstart",b.bind(d,g[h])),j.className="default-interactive-marker-menu-entry")}}var d=this;a=a||{};var e=a.menuEntries,f=a.className||"default-interactive-marker-menu",g=(a.entryClassName,a.overlayClassName||"default-interactive-marker-overlay"),h=a.menuFontSize||"0.8em",i=[];if(i[0]={children:[]},THREE.EventDispatcher.call(this),null===document.getElementById("default-interactive-marker-menu-css")){var j=document.createElement("style");j.id="default-interactive-marker-menu-css",j.type="text/css",j.innerHTML=".default-interactive-marker-menu {background-color: #444444;border: 1px solid #888888;border: 1px solid #888888;padding: 0px 0px 0px 0px;color: #FFFFFF;font-family: sans-serif;font-size: "+h+";z-index: 1002;}.default-interactive-marker-menu ul {padding: 0px 0px 5px 0px;margin: 0px;list-style-type: none;}.default-interactive-marker-menu ul li div {-webkit-touch-callout: none;-webkit-user-select: none;-khtml-user-select: none;-moz-user-select: none;-ms-user-select: none;user-select: none;cursor: default;padding: 3px 10px 3px 10px;}.default-interactive-marker-menu-entry:hover { background-color: #666666; cursor: pointer;}.default-interactive-marker-menu ul ul { font-style: italic; padding-left: 10px;}.default-interactive-marker-overlay { position: absolute; top: 0%; left: 0%; width: 100%; height: 100%; background-color: black; z-index: 1001; -moz-opacity: 0.0; opacity: .0; filter: alpha(opacity = 0);}",document.getElementsByTagName("head")[0].appendChild(j)}this.menuDomElem=document.createElement("div"),this.menuDomElem.style.position="absolute",this.menuDomElem.className=f,this.menuDomElem.addEventListener("contextmenu",function(a){a.preventDefault()}),this.overlayDomElem=document.createElement("div"),this.overlayDomElem.className=g,this.hideListener=this.hide.bind(this),this.overlayDomElem.addEventListener("contextmenu",this.hideListener),this.overlayDomElem.addEventListener("click",this.hideListener),this.overlayDomElem.addEventListener("touchstart",this.hideListener);var k,l,m;for(k=0;k0){var W=this.msgColor,X=document.createElement("canvas"),Y=X.getContext("2d");Y.font="normal 100px sans-serif";var Z=Y.measureText(c.text),$=Z.width;X.width=$,X.height=150,Y.font="normal 100px sans-serif",Y.fillStyle="rgba("+Math.round(255*W.r)+", "+Math.round(255*W.g)+", "+Math.round(255*W.b)+", "+W.a+")",Y.textAlign="left",Y.textBaseline="middle",Y.fillText(c.text,0,X.height/2);var _=new THREE.Texture(X);_.needsUpdate=!0;var aa=new THREE.SpriteMaterial({map:_,useScreenCoordinates:!1}),ba=new THREE.Sprite(aa),ca=c.scale.x;ba.scale.set($/X.height*ca,ca,1),this.add(ba)}break;case ROS3D.MARKER_MESH_RESOURCE:var da=null;0===c.color.r&&0===c.color.g&&0===c.color.b&&0===c.color.a||(da=e),this.msgMesh=c.mesh_resource.substr(10);var ea=new ROS3D.MeshResource({path:b,resource:this.msgMesh,material:da,loader:d});this.add(ea);break;case ROS3D.MARKER_TRIANGLE_LIST:var fa=new ROS3D.TriangleList({material:e,vertices:c.points,colors:c.colors});fa.scale=new THREE.Vector3(c.scale.x,c.scale.y,c.scale.z),this.add(fa);break;default:console.error("Currently unsupported marker type: "+c.type)}},ROS3D.Marker.prototype.__proto__=THREE.Object3D.prototype,ROS3D.Marker.prototype.setPose=function(a){this.position.x=a.position.x,this.position.y=a.position.y,this.position.z=a.position.z,this.quaternion=new THREE.Quaternion(a.orientation.x,a.orientation.y,a.orientation.z,a.orientation.w),this.quaternion.normalize(),this.updateMatrixWorld()},ROS3D.Marker.prototype.update=function(a){if(this.setPose(a.pose),a.color.r!==this.msgColor.r||a.color.g!==this.msgColor.g||a.color.b!==this.msgColor.b||a.color.a!==this.msgColor.a){var b=ROS3D.makeColorMaterial(a.color.r,a.color.g,a.color.b,a.color.a);switch(a.type){case ROS3D.MARKER_LINE_STRIP:case ROS3D.MARKER_LINE_LIST:case ROS3D.MARKER_POINTS:break;case ROS3D.MARKER_ARROW:case ROS3D.MARKER_CUBE:case ROS3D.MARKER_SPHERE:case ROS3D.MARKER_CYLINDER:case ROS3D.MARKER_TRIANGLE_LIST:case ROS3D.MARKER_TEXT_VIEW_FACING:this.traverse(function(a){a instanceof THREE.Mesh&&(a.material=b)});break;case ROS3D.MARKER_MESH_RESOURCE:var c=null;0===a.color.r&&0===a.color.g&&0===a.color.b&&0===a.color.a||(c=this.colorMaterial),this.traverse(function(a){a instanceof THREE.Mesh&&(a.material=c)});break;case ROS3D.MARKER_CUBE_LIST:case ROS3D.MARKER_SPHERE_LIST:default:return!1}this.msgColor=a.color}var d=Math.abs(this.msgScale[0]-a.scale.x)>1e-6||Math.abs(this.msgScale[1]-a.scale.y)>1e-6||Math.abs(this.msgScale[2]-a.scale.z)>1e-6;switch(this.msgScale=[a.scale.x,a.scale.y,a.scale.z],a.type){case ROS3D.MARKER_CUBE:case ROS3D.MARKER_SPHERE:case ROS3D.MARKER_CYLINDER:if(d)return!1;break;case ROS3D.MARKER_TEXT_VIEW_FACING:if(d||this.text!==a.text)return!1;break;case ROS3D.MARKER_MESH_RESOURCE:if(a.mesh_resource.substr(10)!==this.msgMesh)return!1;if(d)return!1;break;case ROS3D.MARKER_ARROW:case ROS3D.MARKER_LINE_STRIP:case ROS3D.MARKER_LINE_LIST:case ROS3D.MARKER_CUBE_LIST:case ROS3D.MARKER_SPHERE_LIST:case ROS3D.MARKER_POINTS:case ROS3D.MARKER_TRIANGLE_LIST:return!1}return!0},ROS3D.Marker.prototype.dispose=function(){this.children.forEach(function(a){a instanceof ROS3D.MeshResource?a.children.forEach(function(b){void 0!==b.material&&b.material.dispose(),b.children.forEach(function(a){void 0!==a.geometry&&a.geometry.dispose(),void 0!==a.material&&a.material.dispose(),b.remove(a)}),a.remove(b)}):(void 0!==a.geometry&&a.geometry.dispose(),void 0!==a.material&&a.material.dispose()),a.parent.remove(a)})},ROS3D.MarkerArrayClient=function(a){a=a||{},this.ros=a.ros,this.topicName=a.topic,this.tfClient=a.tfClient,this.rootObject=a.rootObject||new THREE.Object3D,this.path=a.path||"/",this.loader=a.loader||ROS3D.COLLADA_LOADER_2,this.markers={},this.rosTopic=void 0,this.subscribe()},ROS3D.MarkerArrayClient.prototype.__proto__=EventEmitter2.prototype,ROS3D.MarkerArrayClient.prototype.subscribe=function(){this.unsubscribe(),this.rosTopic=new ROSLIB.Topic({ros:this.ros,name:this.topicName,messageType:"visualization_msgs/MarkerArray",compression:"png"}),this.rosTopic.subscribe(this.processMessage.bind(this))},ROS3D.MarkerArrayClient.prototype.processMessage=function(a){a.markers.forEach(function(a){if(0===a.action){var b=!1;if(a.ns+a.id in this.markers&&((b=this.markers[a.ns+a.id].children[0].update(a))||(this.markers[a.ns+a.id].unsubscribeTf(),this.rootObject.remove(this.markers[a.ns+a.id]))),!b){var c=new ROS3D.Marker({message:a,path:this.path,loader:this.loader});this.markers[a.ns+a.id]=new ROS3D.SceneNode({frameID:a.header.frame_id,tfClient:this.tfClient,object:c}),this.rootObject.add(this.markers[a.ns+a.id])}}else if(1===a.action)console.warn('Received marker message with deprecated action identifier "1"');else if(2===a.action)this.markers[a.ns+a.id].unsubscribeTf(),this.rootObject.remove(this.markers[a.ns+a.id]),delete this.markers[a.ns+a.id];else if(3===a.action){for(var d in this.markers)this.markers[d].unsubscribeTf(),this.rootObject.remove(this.markers[d]);this.markers={}}else console.warn('Received marker message with unknown action identifier "'+a.action+'"')}.bind(this)),this.emit("change")},ROS3D.MarkerArrayClient.prototype.unsubscribe=function(){this.rosTopic&&this.rosTopic.unsubscribe()},ROS3D.MarkerClient=function(a){a=a||{},this.ros=a.ros,this.topicName=a.topic,this.tfClient=a.tfClient,this.rootObject=a.rootObject||new THREE.Object3D,this.path=a.path||"/",this.loader=a.loader||ROS3D.COLLADA_LOADER_2,this.markers={},this.rosTopic=void 0,this.subscribe()},ROS3D.MarkerClient.prototype.__proto__=EventEmitter2.prototype,ROS3D.MarkerClient.prototype.unsubscribe=function(){this.rosTopic&&this.rosTopic.unsubscribe()},ROS3D.MarkerClient.prototype.subscribe=function(){this.unsubscribe(),this.rosTopic=new ROSLIB.Topic({ros:this.ros,name:this.topicName,messageType:"visualization_msgs/Marker",compression:"png"}),this.rosTopic.subscribe(this.processMessage.bind(this))},ROS3D.MarkerClient.prototype.processMessage=function(a){var b=new ROS3D.Marker({message:a,path:this.path,loader:this.loader}),c=this.markers[a.ns+a.id];c&&(c.unsubscribeTf(),this.rootObject.remove(c)),this.markers[a.ns+a.id]=new ROS3D.SceneNode({frameID:a.header.frame_id,tfClient:this.tfClient,object:b}),this.rootObject.add(this.markers[a.ns+a.id]),this.emit("change")},ROS3D.Arrow=function(a){a=a||{};var b=a.origin||new THREE.Vector3(0,0,0),c=a.direction||new THREE.Vector3(1,0,0),d=a.length||1,e=a.headLength||.2,f=a.shaftDiameter||.05,g=a.headDiameter||.1,h=a.material||new THREE.MeshBasicMaterial,i=d-e,j=new THREE.CylinderGeometry(.5*f,.5*f,i,12,1),k=new THREE.Matrix4;k.setPosition(new THREE.Vector3(0,.5*i,0)),j.applyMatrix(k);var l=new THREE.CylinderGeometry(0,.5*g,e,12,1);k.setPosition(new THREE.Vector3(0,i+.5*e,0)),l.applyMatrix(k),THREE.GeometryUtils.merge(j,l),THREE.Mesh.call(this,j,h),this.position=b,this.setDirection(c)},ROS3D.Arrow.prototype.__proto__=THREE.Mesh.prototype,ROS3D.Arrow.prototype.setDirection=function(a){var b=new THREE.Vector3(0,1,0).cross(a),c=Math.acos(new THREE.Vector3(0,1,0).dot(a.clone().normalize()));this.matrix=(new THREE.Matrix4).makeRotationAxis(b.normalize(),c),this.rotation.setFromRotationMatrix(this.matrix,this.rotation.order)},ROS3D.Arrow.prototype.setLength=function(a){this.scale.set(a,a,a)},ROS3D.Arrow.prototype.setColor=function(a){this.material.color.setHex(a)},ROS3D.Arrow.prototype.dispose=function(){void 0!==this.geometry&&this.geometry.dispose(),void 0!==this.material&&this.material.dispose()},ROS3D.Arrow2=function(a){a=a||{};var b=a.origin||new THREE.Vector3(0,0,0),c=a.direction||new THREE.Vector3(1,0,0),d=a.length||1;a.headLength,a.shaftDiameter,a.headDiameter,a.material||new THREE.MeshBasicMaterial;THREE.ArrowHelper.call(this,c,b,d,16711680)},ROS3D.Arrow2.prototype.__proto__=THREE.ArrowHelper.prototype,ROS3D.Arrow2.prototype.dispose=function(){void 0!==this.line&&(this.line.material.dispose(),this.line.geometry.dispose()),void 0!==this.cone&&(this.cone.material.dispose(),this.cone.geometry.dispose())},ROS3D.Axes=function(a){function b(a){var b=new THREE.Color;b.setRGB(a.x,a.y,a.z);var d=new THREE.MeshBasicMaterial({color:b.getHex()}),e=new THREE.Vector3;e.crossVectors(a,new THREE.Vector3(0,-1,0));var f=new THREE.Quaternion;f.setFromAxisAngle(e,.5*Math.PI);var g=new THREE.Mesh(c.headGeom,d);g.position=a.clone(),g.position.multiplyScalar(.95),g.quaternion=f,g.updateMatrix(),c.add(g);var h=new THREE.Mesh(c.lineGeom,d);h.position=a.clone(),h.position.multiplyScalar(.45),h.quaternion=f,h.updateMatrix(),c.add(h)}var c=this;a=a||{};var d=a.shaftRadius||.008,e=a.headRadius||.023,f=a.headLength||.1;THREE.Object3D.call(this),this.lineGeom=new THREE.CylinderGeometry(d,d,1-f),this.headGeom=new THREE.CylinderGeometry(0,e,f),b(new THREE.Vector3(1,0,0)),b(new THREE.Vector3(0,1,0)),b(new THREE.Vector3(0,0,1))},ROS3D.Axes.prototype.__proto__=THREE.Object3D.prototype,ROS3D.Grid=function(a){a=a||{};var b=a.num_cells||10,c=a.color||"#cccccc",d=a.lineWidth||1,e=a.cellSize||1;THREE.Object3D.call(this);for(var f=new THREE.LineBasicMaterial({color:c,linewidth:d}),g=0;g<=b;++g){var h=e*b/2,i=h-g*e,j=new THREE.Geometry;j.vertices.push(new THREE.Vector3(-h,i,0),new THREE.Vector3(h,i,0));var k=new THREE.Geometry;k.vertices.push(new THREE.Vector3(i,-h,0),new THREE.Vector3(i,h,0)),this.add(new THREE.Line(j,f)),this.add(new THREE.Line(k,f))}},ROS3D.Grid.prototype.__proto__=THREE.Object3D.prototype,ROS3D.MeshResource=function(a){var b=this;a=a||{};var c=a.path||"/",d=a.resource,e=a.material||null;this.warnings=a.warnings;var f=a.loader||ROS3D.COLLADA_LOADER_2;THREE.Object3D.call(this),"/"!==c.substr(c.length-1)&&(this.path+="/");var g,h=c+d,i=h.substr(-4).toLowerCase();".dae"===i?(g=f===ROS3D.COLLADA_LOADER?new THREE.ColladaLoader:new ColladaLoader2,g.log=function(a){b.warnings&&console.warn(a)},g.load(h,function(a){if(f===ROS3D.COLLADA_LOADER_2&&a.dae.asset.unit){var c=a.dae.asset.unit;a.scene.scale=new THREE.Vector3(c,c,c)}if(null!==e){var d=function(a,b){if(a.material=b,a.children)for(var c=0;c=this.keep&&(this.sns[0].unsubscribeTf(),this.rootObject.remove(this.sns[0]),this.sns.shift()),this.options.origin=new THREE.Vector3(a.pose.pose.position.x,a.pose.pose.position.y,a.pose.pose.position.z);var b=new THREE.Quaternion(a.pose.pose.orientation.x,a.pose.pose.orientation.y,a.pose.pose.orientation.z,a.pose.pose.orientation.w);this.options.direction=new THREE.Vector3(1,0,0),this.options.direction.applyQuaternion(b),this.options.material=new THREE.MeshBasicMaterial({color:this.color});var c=new ROS3D.Arrow(this.options);this.sns.push(new ROS3D.SceneNode({frameID:a.header.frame_id,tfClient:this.tfClient,object:c})),this.rootObject.add(this.sns[this.sns.length-1])},ROS3D.Path=function(a){a=a||{},this.ros=a.ros,this.topicName=a.topic||"/path",this.tfClient=a.tfClient,this.color=a.color||13369599,this.rootObject=a.rootObject||new THREE.Object3D,THREE.Object3D.call(this),this.sn=null,this.line=null,this.rosTopic=void 0,this.subscribe()},ROS3D.Path.prototype.__proto__=THREE.Object3D.prototype,ROS3D.Path.prototype.unsubscribe=function(){this.rosTopic&&this.rosTopic.unsubscribe()},ROS3D.Path.prototype.subscribe=function(){this.unsubscribe(),this.rosTopic=new ROSLIB.Topic({ros:this.ros,name:this.topicName,messageType:"nav_msgs/Path"}),this.rosTopic.subscribe(this.processMessage.bind(this))},ROS3D.Path.prototype.processMessage=function(a){null!==this.sn&&(this.sn.unsubscribeTf(),this.rootObject.remove(this.sn));for(var b=new THREE.Geometry,c=0;ca.range_max)this.particles.alpha[c]=0;else{var e=a.angle_min+c*a.angle_increment;this.particles.points[c]=new THREE.Vector3(d*Math.cos(e),d*Math.sin(e),0),this.particles.alpha[c]=1}this.particles.colors[c]=new THREE.Color(this.color)}finishedUpdate(this.particles,b)},ROS3D.Particles=function(a){a=a||{},this.tfClient=a.tfClient;var b=a.texture||"https://upload.wikimedia.org/wikipedia/commons/a/a2/Pixel-white.png",c=a.size||.05;this.max_pts=a.max_pts||1e4,this.first_size=null,this.prev_pts=0,this.rootObject=a.rootObject||new THREE.Object3D;THREE.Object3D.call(this),this.vertex_shader=["attribute vec3 customColor;","attribute float alpha;","varying vec3 vColor;","varying float falpha;","void main() ","{"," vColor = customColor; // set color associated to vertex; use later in fragment shader"," vec4 mvPosition = modelViewMatrix * vec4( position, 1.0 );"," falpha = alpha; ",""," // option (1): draw particles at constant size on screen"," // gl_PointSize = size;"," // option (2): scale particles as objects in 3D space"," gl_PointSize = ",c,"* ( 300.0 / length( mvPosition.xyz ) );"," gl_Position = projectionMatrix * mvPosition;","}"].join("\n"),this.fragment_shader=["uniform sampler2D texture;","varying vec3 vColor; // colors associated to vertices; assigned by vertex shader","varying float falpha;","void main() ","{"," // THREE.Material.alphaTest is not evaluated for ShaderMaterial, so we"," // have to take care of this ourselves."," if (falpha < 0.5) discard;"," // calculates a color for the particle"," gl_FragColor = vec4( vColor, falpha );"," // sets particle texture to desired color"," gl_FragColor = gl_FragColor * texture2D( texture, gl_PointCoord );","}"].join("\n"),this.geom=new THREE.Geometry;for(var d=0;d=0;f--)if(d[f].object===b[e]){c.push(d[f]);break}this.getWebglObjects(a,b[e].children,c)}},ROS3D.Highlighter.prototype.renderHighlight=function(a,b,c){var d=[];this.getWebglObjects(b,this.hoverObjs,d),b.overrideMaterial=new THREE.MeshBasicMaterial({fog:!1,opacity:.5,depthTest:!0,depthWrite:!1,polygonOffset:!0,polygonOffsetUnits:-1,side:THREE.DoubleSide});var e=b.__webglObjects;b.__webglObjects=d,a.render(b,c),b.__webglObjects=e,b.overrideMaterial=null},ROS3D.MouseHandler=function(a){THREE.EventDispatcher.call(this),this.renderer=a.renderer,this.camera=a.camera,this.rootObject=a.rootObject,this.fallbackTarget=a.fallbackTarget,this.lastTarget=this.fallbackTarget,this.dragging=!1,this.projector=new THREE.Projector;var b=["contextmenu","click","dblclick","mouseout","mousedown","mouseup","mousemove","mousewheel","DOMMouseScroll","touchstart","touchend","touchcancel","touchleave","touchmove"];this.listeners={},b.forEach(function(a){this.listeners[a]=this.processDomEvent.bind(this),this.renderer.domElement.addEventListener(a,this.listeners[a],!1)},this)},ROS3D.MouseHandler.prototype.processDomEvent=function(a){a.preventDefault();var b,c,d=a.target,e=d.getBoundingClientRect();if(-1!==a.type.indexOf("touch")){b=0,c=0;for(var f=0;f0?(d=o[0].object,n.intersection=this.lastIntersection=o[0]):d=this.fallbackTarget,d!==this.lastTarget&&a.type.match(/mouse/)){var p=this.notify(d,"mouseover",n);0===p?this.notify(this.lastTarget,"mouseout",n):1===p&&(d=this.fallbackTarget)!==this.lastTarget&&(this.notify(d,"mouseover",n),this.notify(this.lastTarget,"mouseout",n))}if(d!==this.lastTarget&&a.type.match(/touch/)){this.notify(d,a.type,n)?(this.notify(this.lastTarget,"touchleave",n),this.notify(this.lastTarget,"touchend",n)):(d=this.fallbackTarget)!==this.lastTarget&&(this.notify(this.lastTarget,"touchmove",n),this.notify(this.lastTarget,"touchend",n))}this.notify(d,a.type,n),"mousedown"!==a.type&&"touchstart"!==a.type&&"touchmove"!==a.type||(this.dragging=!0),this.lastTarget=d},ROS3D.MouseHandler.prototype.notify=function(a,b,c){for(c.type=b,c.cancelBubble=!1,c.continueBubble=!1,c.stopPropagation=function(){c.cancelBubble=!0},c.continuePropagation=function(){c.continueBubble=!0},c.currentTarget=a;c.currentTarget;){if(c.currentTarget.dispatchEvent&&c.currentTarget.dispatchEvent instanceof Function){if(c.currentTarget.dispatchEvent(c),c.cancelBubble)return this.dispatchEvent(c),0;if(c.continueBubble)return 2}c.currentTarget=c.currentTarget.parent}return 1},THREE.EventDispatcher.prototype.apply(ROS3D.MouseHandler.prototype),ROS3D.OrbitControls=function(a){function b(a){var b=a.domEvent;switch(b.preventDefault(),b.button){case 0:A=z.ROTATE,n.set(b.clientX,b.clientY);break;case 1:A=z.MOVE,u=new THREE.Vector3(0,0,1);var c=(new THREE.Matrix4).extractRotation(this.camera.matrix);u.applyMatrix4(c),t=j.center.clone(),v=j.camera.position.clone(),w=d(a.mouseRay,t,u);break;case 2:A=z.ZOOM,q.set(b.clientX,b.clientY)}this.showAxes()}function c(a){var b=a.domEvent;if(A===z.ROTATE)o.set(b.clientX,b.clientY),p.subVectors(o,n),j.rotateLeft(2*Math.PI*p.x/l*j.userRotateSpeed),j.rotateUp(2*Math.PI*p.y/l*j.userRotateSpeed),n.copy(o),this.showAxes();else if(A===z.ZOOM)r.set(b.clientX,b.clientY),s.subVectors(r,q),s.y>0?j.zoomIn():j.zoomOut(),q.copy(r),this.showAxes();else if(A===z.MOVE){var c=d(a.mouseRay,j.center,u);if(!c)return;var e=(new THREE.Vector3).subVectors(w.clone(),c.clone());j.center.addVectors(t.clone(),e.clone()),j.camera.position.addVectors(v.clone(),e.clone()),j.update(),j.camera.updateMatrixWorld(),this.showAxes()}}function d(a,b,c){var d=new THREE.Vector3;new THREE.Vector3;d.subVectors(b,a.origin);var e=a.direction.dot(c);if(Math.abs(e)0?j.zoomIn():j.zoomOut(),this.showAxes()}}function g(a){var b=a.domEvent;switch(b.touches.length){case 1:A=z.ROTATE,n.set(b.touches[0].pageX-window.scrollX,b.touches[0].pageY-window.scrollY);break;case 2:A=z.NONE,u=new THREE.Vector3(0,0,1);var c=(new THREE.Matrix4).extractRotation(this.camera.matrix);u.applyMatrix4(c),t=j.center.clone(),v=j.camera.position.clone(),w=d(a.mouseRay,t,u),x[0]=new THREE.Vector2(b.touches[0].pageX,b.touches[0].pageY),x[1]=new THREE.Vector2(b.touches[1].pageX,b.touches[1].pageY),y[0]=new THREE.Vector2(0,0),y[1]=new THREE.Vector2(0,0)}this.showAxes(),b.preventDefault()}function h(a){var b=a.domEvent;if(A===z.ROTATE)o.set(b.touches[0].pageX-window.scrollX,b.touches[0].pageY-window.scrollY),p.subVectors(o,n),j.rotateLeft(2*Math.PI*p.x/l*j.userRotateSpeed),j.rotateUp(2*Math.PI*p.y/l*j.userRotateSpeed),n.copy(o),this.showAxes();else{if(y[0].set(x[0].x-b.touches[0].pageX,x[0].y-b.touches[0].pageY),y[1].set(x[1].x-b.touches[1].pageX,x[1].y-b.touches[1].pageY),y[0].lengthSq()>m&&y[1].lengthSq()>m&&(x[0].set(b.touches[0].pageX,b.touches[0].pageY),x[1].set(b.touches[1].pageX,b.touches[1].pageY),y[0].dot(y[1])>0&&A!==z.ZOOM?A=z.MOVE:y[0].dot(y[1])<0&&A!==z.MOVE&&(A=z.ZOOM),A===z.ZOOM)){var c=new THREE.Vector2;c.subVectors(x[0],x[1]),y[0].dot(c)<0&&y[1].dot(c)>0?j.zoomOut():y[0].dot(c)>0&&y[1].dot(c)<0&&j.zoomIn()}if(A===z.MOVE){var e=d(a.mouseRay,j.center,u);if(!e)return;var f=(new THREE.Vector3).subVectors(w.clone(),e.clone());j.center.addVectors(t.clone(),f.clone()),j.camera.position.addVectors(v.clone(),f.clone()),j.update(),j.camera.updateMatrixWorld()}this.showAxes(),b.preventDefault()}}function i(a){var b=a.domEvent;1===b.touches.length&&A!==z.ROTATE?(A=z.ROTATE,n.set(b.touches[0].pageX-window.scrollX,b.touches[0].pageY-window.scrollY)):A=z.NONE}THREE.EventDispatcher.call(this);var j=this;a=a||{};var k=a.scene;this.camera=a.camera,this.center=new THREE.Vector3,this.userZoom=!0,this.userZoomSpeed=a.userZoomSpeed||1,this.userRotate=!0,this.userRotateSpeed=a.userRotateSpeed||1,this.autoRotate=a.autoRotate,this.autoRotateSpeed=a.autoRotateSpeed||2,this.camera.up=new THREE.Vector3(0,0,1);var l=1800,m=10,n=new THREE.Vector2,o=new THREE.Vector2,p=new THREE.Vector2,q=new THREE.Vector2,r=new THREE.Vector2,s=new THREE.Vector2,t=new THREE.Vector3,u=new THREE.Vector3,v=new THREE.Vector3,w=new THREE.Vector3,x=new Array(2),y=new Array(2);this.phiDelta=0,this.thetaDelta=0,this.scale=1,this.lastPosition=new THREE.Vector3;var z={NONE:-1,ROTATE:0,ZOOM:1,MOVE:2},A=z.NONE;this.axes=new ROS3D.Axes({shaftRadius:.025,headRadius:.07,headLength:.2}),k.add(this.axes),this.axes.traverse(function(a){a.visible=!1}),this.addEventListener("mousedown",b),this.addEventListener("mouseup",e),this.addEventListener("mousemove",c),this.addEventListener("touchstart",g),this.addEventListener("touchmove",h),this.addEventListener("touchend",i),this.addEventListener("mousewheel",f),this.addEventListener("DOMMouseScroll",f)},ROS3D.OrbitControls.prototype.showAxes=function(){var a=this;this.axes.traverse(function(a){a.visible=!0}),this.hideTimeout&&clearTimeout(this.hideTimeout),this.hideTimeout=setTimeout(function(){a.axes.traverse(function(a){a.visible=!1}),a.hideTimeout=!1},1e3)},ROS3D.OrbitControls.prototype.rotateLeft=function(a){void 0===a&&(a=2*Math.PI/60/60*this.autoRotateSpeed),this.thetaDelta-=a},ROS3D.OrbitControls.prototype.rotateRight=function(a){void 0===a&&(a=2*Math.PI/60/60*this.autoRotateSpeed),this.thetaDelta+=a},ROS3D.OrbitControls.prototype.rotateUp=function(a){void 0===a&&(a=2*Math.PI/60/60*this.autoRotateSpeed),this.phiDelta-=a},ROS3D.OrbitControls.prototype.rotateDown=function(a){void 0===a&&(a=2*Math.PI/60/60*this.autoRotateSpeed),this.phiDelta+=a},ROS3D.OrbitControls.prototype.zoomIn=function(a){void 0===a&&(a=Math.pow(.95,this.userZoomSpeed)),this.scale/=a},ROS3D.OrbitControls.prototype.zoomOut=function(a){void 0===a&&(a=Math.pow(.95,this.userZoomSpeed)),this.scale*=a},ROS3D.OrbitControls.prototype.update=function(){var a=this.camera.position,b=a.clone().sub(this.center),c=Math.atan2(b.y,b.x),d=Math.atan2(Math.sqrt(b.y*b.y+b.x*b.x),b.z);this.autoRotate&&this.rotateLeft(2*Math.PI/60/60*this.autoRotateSpeed),c+=this.thetaDelta,d+=this.phiDelta;d=Math.max(1e-6,Math.min(Math.PI-1e-6,d));var e=b.length();b.y=e*Math.sin(d)*Math.sin(c),b.z=e*Math.cos(d),b.x=e*Math.sin(d)*Math.cos(c),b.multiplyScalar(this.scale),a.copy(this.center).add(b),this.camera.lookAt(this.center),e=b.length(),this.axes.position=this.center.clone(),this.axes.scale.x=this.axes.scale.y=this.axes.scale.z=.05*e,this.axes.updateMatrixWorld(!0),this.thetaDelta=0,this.phiDelta=0,this.scale=1,this.lastPosition.distanceTo(this.camera.position)>0&&(this.dispatchEvent({type:"change"}),this.lastPosition.copy(this.camera.position))},THREE.EventDispatcher.prototype.apply(ROS3D.OrbitControls.prototype),ROS3D.SceneNode=function(a){a=a||{};var b=this;this.tfClient=a.tfClient,this.frameID=a.frameID;var c=a.object;this.pose=a.pose||new ROSLIB.Pose,THREE.Object3D.call(this),this.visible=!1,this.add(c),this.updatePose(this.pose),this.tfUpdate=function(a){var c=new ROSLIB.Transform(a),d=new ROSLIB.Pose(b.pose);d.applyTransform(c),b.updatePose(d),b.visible=!0},this.tfClient.subscribe(this.frameID,this.tfUpdate)},ROS3D.SceneNode.prototype.__proto__=THREE.Object3D.prototype,ROS3D.SceneNode.prototype.updatePose=function(a){this.position.set(a.position.x,a.position.y,a.position.z),this.quaternion.set(a.orientation.x,a.orientation.y,a.orientation.z,a.orientation.w),this.updateMatrixWorld(!0)},ROS3D.SceneNode.prototype.unsubscribeTf=function(){this.tfClient.unsubscribe(this.frameID,this.tfUpdate)},ROS3D.Viewer=function(a){a=a||{};var b=a.divID,c=a.width,d=a.height,e=a.background||"#111111",f=a.antialias,g=a.intensity||.66,h=a.near||.01,i=a.far||1e3,j=a.alpha||1,k=a.cameraPose||{x:3,y:3,z:3},l=a.cameraZoomSpeed||.5;this.renderer=new THREE.WebGLRenderer({antialias:f,alpha:!0}),this.renderer.setClearColor(parseInt(e.replace("#","0x"),16),j),this.renderer.sortObjects=!1,this.renderer.setSize(c,d),this.renderer.shadowMapEnabled=!1,this.renderer.autoClear=!1,this.scene=new THREE.Scene,this.camera=new THREE.PerspectiveCamera(40,c/d,h,i),this.camera.position.x=k.x,this.camera.position.y=k.y,this.camera.position.z=k.z,this.cameraControls=new ROS3D.OrbitControls({scene:this.scene,camera:this.camera}),this.cameraControls.userZoomSpeed=l,this.scene.add(new THREE.AmbientLight(5592405)),this.directionalLight=new THREE.DirectionalLight(16777215,g),this.scene.add(this.directionalLight),this.selectableObjects=new THREE.Object3D,this.scene.add(this.selectableObjects);var m=new ROS3D.MouseHandler({renderer:this.renderer,camera:this.camera,rootObject:this.selectableObjects,fallbackTarget:this.cameraControls});this.highlighter=new ROS3D.Highlighter({mouseHandler:m}),this.stopped=!0,this.animationRequestId=void 0,document.getElementById(b).appendChild(this.renderer.domElement),this.start()},ROS3D.Viewer.prototype.start=function(){this.stopped=!1,this.draw()},ROS3D.Viewer.prototype.draw=function(){this.stopped||(this.cameraControls.update(),this.directionalLight.position=this.camera.localToWorld(new THREE.Vector3(-1,1,0)),this.directionalLight.position.normalize(),this.renderer.clear(!0,!0,!0),this.renderer.render(this.scene,this.camera),this.highlighter.renderHighlight(this.renderer,this.scene,this.camera),this.animationRequestId=requestAnimationFrame(this.draw.bind(this)))},ROS3D.Viewer.prototype.stop=function(){this.stopped||cancelAnimationFrame(this.animationRequestId),this.stopped=!0},ROS3D.Viewer.prototype.addObject=function(a,b){b?this.selectableObjects.add(a):this.scene.add(a)},ROS3D.Viewer.prototype.resize=function(a,b){this.camera.aspect=a/b,this.camera.updateProjectionMatrix(),this.renderer.setSize(a,b)}; \ No newline at end of file +function setFrame(a,b){null===a.sn&&(a.sn=new ROS3D.SceneNode({frameID:b,tfClient:a.tfClient,object:a.ps}),a.rootObject.add(a.sn))}function finishedUpdate(a,b){null===a.first_size&&(a.first_size=b,a.max_pts=Math.max(a.max_pts,b));for(var c=b;ca.max_pts&&console.error("Attempted to draw more points than max_pts allows")}function read_point(a,b,c){for(var d=[],e=a.point_step*b,f=0;f=8&&(d-=8,b.push(c>>d),c&=Math.pow(2,d)-1),c<<=6;return b}var ROS3D=ROS3D||{REVISION:"0.17.0-SNAPSHOT"};ROS3D.MARKER_ARROW=0,ROS3D.MARKER_CUBE=1,ROS3D.MARKER_SPHERE=2,ROS3D.MARKER_CYLINDER=3,ROS3D.MARKER_LINE_STRIP=4,ROS3D.MARKER_LINE_LIST=5,ROS3D.MARKER_CUBE_LIST=6,ROS3D.MARKER_SPHERE_LIST=7,ROS3D.MARKER_POINTS=8,ROS3D.MARKER_TEXT_VIEW_FACING=9,ROS3D.MARKER_MESH_RESOURCE=10,ROS3D.MARKER_TRIANGLE_LIST=11,ROS3D.INTERACTIVE_MARKER_KEEP_ALIVE=0,ROS3D.INTERACTIVE_MARKER_POSE_UPDATE=1,ROS3D.INTERACTIVE_MARKER_MENU_SELECT=2,ROS3D.INTERACTIVE_MARKER_BUTTON_CLICK=3,ROS3D.INTERACTIVE_MARKER_MOUSE_DOWN=4,ROS3D.INTERACTIVE_MARKER_MOUSE_UP=5,ROS3D.INTERACTIVE_MARKER_NONE=0,ROS3D.INTERACTIVE_MARKER_MENU=1,ROS3D.INTERACTIVE_MARKER_BUTTON=2,ROS3D.INTERACTIVE_MARKER_MOVE_AXIS=3,ROS3D.INTERACTIVE_MARKER_MOVE_PLANE=4,ROS3D.INTERACTIVE_MARKER_ROTATE_AXIS=5,ROS3D.INTERACTIVE_MARKER_MOVE_ROTATE=6,ROS3D.INTERACTIVE_MARKER_INHERIT=0,ROS3D.INTERACTIVE_MARKER_FIXED=1,ROS3D.INTERACTIVE_MARKER_VIEW_FACING=2,ROS3D.COLLADA_LOADER=1,ROS3D.COLLADA_LOADER_2=2,ROS3D.makeColorMaterial=function(a,b,c,d){var e=new THREE.Color;return e.setRGB(a,b,c),d<=.99?new THREE.MeshBasicMaterial({color:e.getHex(),opacity:d,transparent:!0,depthWrite:!0,blendSrc:THREE.SrcAlphaFactor,blendDst:THREE.OneMinusSrcAlphaFactor,blendEquation:THREE.ReverseSubtractEquation,blending:THREE.NormalBlending}):new THREE.MeshPhongMaterial({color:e.getHex(),opacity:d,blending:THREE.NormalBlending})},ROS3D.intersectPlane=function(a,b,c){var d=new THREE.Vector3,e=new THREE.Vector3;d.subVectors(b,a.origin);var f=a.direction.dot(c);if(!(Math.abs(f) (1.0 - 3.0/255.0) )"," {"," vec4 depthColor2 = texture2D( map, vUv2 );"," float depth2 = ( depthColor2.r + depthColor2.g + depthColor2.b ) / 3.0 ;"," depth += depth2;"," }"," "," return depth;"," }","","float median(float a, float b, float c)"," {"," float r=a;"," "," if ( (a0.5) || (vUvP.y<0.5) || (vUvP.y>0.0))"," {"," vec2 smp = decodeDepth(vec2(position.x, position.y));"," float depth = smp.x;"," depthVariance = smp.y;"," "," float z = -depth;"," "," pos = vec4("," ( position.x / width - 0.5 ) * z *0.5 * (1000.0/focallength) * -1.0,"," ( position.y / height - 0.5 ) * z *0.5 * (1000.0/focallength),"," (- z + zOffset / 1000.0) * 1.0,"," 1.0);"," "," vec2 maskP = vec2( position.x / (width*2.0), position.y / (height*2.0) );"," vec4 maskColor = texture2D( map, maskP );"," maskVal = ( maskColor.r + maskColor.g + maskColor.b ) / 3.0 ;"," }"," "," gl_PointSize = pointSize;"," gl_Position = projectionMatrix * modelViewMatrix * pos;"," ","}"].join("\n"),this.fragment_shader=["uniform sampler2D map;","uniform float varianceThreshold;","uniform float whiteness;","","varying vec2 vUvP;","varying vec2 colorP;","","varying float depthVariance;","varying float maskVal;","","","void main() {"," "," vec4 color;"," "," if ( (depthVariance>varianceThreshold) || (maskVal>0.5) ||(vUvP.x<0.0)|| (vUvP.x>0.5) || (vUvP.y<0.5) || (vUvP.y>1.0))"," { "," discard;"," }"," else "," {"," color = texture2D( map, colorP );"," "," float fader = whiteness /100.0;"," "," color.r = color.r * (1.0-fader)+ fader;"," "," color.g = color.g * (1.0-fader)+ fader;"," "," color.b = color.b * (1.0-fader)+ fader;"," "," color.a = 1.0;//smoothstep( 20000.0, -20000.0, gl_FragCoord.z / gl_FragCoord.w );"," }"," "," gl_FragColor = vec4( color.r, color.g, color.b, color.a );"," ","}"].join("\n")},ROS3D.CloseCloud.prototype=Object.create(THREE.Object3D.prototype),ROS3D.CloseCloud.prototype.constructor=ROS3D.CloseCloud,ROS3D.CloseCloud.prototype.clone=function(a,b){return void 0===a&&(a=new ROS3D.CloseCloud(this.options)),THREE.Object3D.prototype.clone.call(this,a,b),a},ROS3D.CloseCloud.prototype.metaLoaded=function(){this.metaLoaded=!0,this.initStreamer()},ROS3D.CloseCloud.prototype.initStreamer=function(){if(this.metaLoaded){this.dctexture=new THREE.Texture(this.video),this.dcgeometry=new THREE.Geometry;for(var a=this.width/2,b=this.height/2,c=0,d=a*b;c0.99)"," {"," vec4 depthColor2 = texture2D( map, vUv2 );"," float depth2 = ( depthColor2.r + depthColor2.g + depthColor2.b ) / 3.0 ;"," depth = 0.99+depth2;"," }"," "," return depth;"," }","","float median(float a, float b, float c)"," {"," float r=a;"," "," if ( (a0.5) || (vUvP.y<0.5) || (vUvP.y>0.0))"," {"," vec2 smp = decodeDepth(vec2(position.x, position.y));"," float depth = smp.x;"," depthVariance = smp.y;"," "," float z = -depth;"," "," pos = vec4("," ( position.x / width - 0.5 ) * z * (1000.0/focallength) * -1.0,"," ( position.y / height - 0.5 ) * z * (1000.0/focallength),"," (- z + zOffset / 1000.0) * 2.0,"," 1.0);"," "," vec2 maskP = vec2( position.x / (width*2.0), position.y / (height*2.0) );"," vec4 maskColor = texture2D( map, maskP );"," maskVal = ( maskColor.r + maskColor.g + maskColor.b ) / 3.0 ;"," }"," "," gl_PointSize = pointSize;"," gl_Position = projectionMatrix * modelViewMatrix * pos;"," ","}"].join("\n"),this.fragment_shader=["uniform sampler2D map;","uniform float varianceThreshold;","uniform float whiteness;","","varying vec2 vUvP;","varying vec2 colorP;","","varying float depthVariance;","varying float maskVal;","","","void main() {"," "," vec4 color;"," "," if ( (depthVariance>varianceThreshold) || (maskVal>0.5) ||(vUvP.x<0.0)|| (vUvP.x>0.5) || (vUvP.y<0.5) || (vUvP.y>1.0))"," { "," discard;"," }"," else "," {"," color = texture2D( map, colorP );"," "," float fader = whiteness /100.0;"," "," color.r = color.r * (1.0-fader)+ fader;"," "," color.g = color.g * (1.0-fader)+ fader;"," "," color.b = color.b * (1.0-fader)+ fader;"," "," color.a = 1.0;//smoothstep( 20000.0, -20000.0, gl_FragCoord.z / gl_FragCoord.w );"," }"," "," gl_FragColor = vec4( color.r, color.g, color.b, color.a );"," ","}"].join("\n")},ROS3D.DepthCloud.prototype.__proto__=THREE.Object3D.prototype,ROS3D.DepthCloud.prototype.metaLoaded=function(){this.metaLoaded=!0,this.initStreamer()},ROS3D.DepthCloud.prototype.initStreamer=function(){if(this.metaLoaded){this.texture=new THREE.Texture(this.video),this.geometry=new THREE.Geometry;for(var a=0,b=this.width*this.height;a0&&(this.menu=new ROS3D.InteractiveMarkerMenu({menuEntries:c.menuEntries,menuFontSize:c.menuFontSize}),this.menu.addEventListener("menu-select",function(a){b.dispatchEvent(a)}))},ROS3D.InteractiveMarker.prototype.__proto__=THREE.Object3D.prototype,ROS3D.InteractiveMarker.prototype.showMenu=function(a,b){this.menu&&this.menu.show(a,b)},ROS3D.InteractiveMarker.prototype.moveAxis=function(a,b,c){if(this.dragging){var d=a.currentControlOri,e=b.clone().applyQuaternion(d),f=this.dragStart.event3d.intersection.point,g=e.clone().applyQuaternion(this.dragStart.orientationWorld.clone()),h=new THREE.Ray(f,g),i=ROS3D.closestAxisPoint(h,c.camera,c.mousePos),j=new THREE.Vector3;j.addVectors(this.dragStart.position,e.clone().applyQuaternion(this.dragStart.orientation).multiplyScalar(i)),this.setPosition(a,j),c.stopPropagation()}},ROS3D.InteractiveMarker.prototype.movePlane=function(a,b,c){if(this.dragging){var d=a.currentControlOri,e=b.clone().applyQuaternion(d),f=this.dragStart.event3d.intersection.point,g=e.clone().applyQuaternion(this.dragStart.orientationWorld),h=ROS3D.intersectPlane(c.mouseRay,f,g),i=new THREE.Vector3;i.subVectors(h,f),i.add(this.dragStart.positionWorld),this.setPosition(a,i),c.stopPropagation()}},ROS3D.InteractiveMarker.prototype.rotateAxis=function(a,b,c){if(this.dragging){a.updateMatrixWorld();var d=a.currentControlOri,e=d.clone().multiply(b.clone()),f=new THREE.Vector3(1,0,0).applyQuaternion(e),g=this.dragStart.event3d.intersection.point,h=f.applyQuaternion(this.dragStart.orientationWorld),i=ROS3D.intersectPlane(c.mouseRay,g,h),j=new THREE.Ray(this.dragStart.positionWorld,h),k=ROS3D.intersectPlane(j,g,h),l=this.dragStart.orientationWorld.clone().multiply(e),m=l.clone().inverse();i.sub(k),i.applyQuaternion(m);var n=this.dragStart.event3d.intersection.point.clone();n.sub(k),n.applyQuaternion(m);var o=Math.atan2(i.y,i.z),p=Math.atan2(n.y,n.z),q=p-o,r=new THREE.Quaternion;r.setFromAxisAngle(f,q),this.setOrientation(a,r.multiply(this.dragStart.orientationWorld)),c.stopPropagation()}},ROS3D.InteractiveMarker.prototype.feedbackEvent=function(a,b){this.dispatchEvent({type:a,position:this.position.clone(),orientation:this.quaternion.clone(),controlName:b.name})},ROS3D.InteractiveMarker.prototype.startDrag=function(a,b){if(0===b.domEvent.button){b.stopPropagation(),this.dragging=!0,this.updateMatrixWorld(!0);var c=new THREE.Vector3;this.matrixWorld.decompose(this.dragStart.positionWorld,this.dragStart.orientationWorld,c),this.dragStart.position=this.position.clone(),this.dragStart.orientation=this.quaternion.clone(),this.dragStart.event3d=b,this.feedbackEvent("user-mousedown",a)}},ROS3D.InteractiveMarker.prototype.stopDrag=function(a,b){0===b.domEvent.button&&(b.stopPropagation(),this.dragging=!1,this.dragStart.event3d={},this.onServerSetPose(this.bufferedPoseEvent),this.bufferedPoseEvent=void 0,this.feedbackEvent("user-mouseup",a))},ROS3D.InteractiveMarker.prototype.buttonClick=function(a,b){b.stopPropagation(),this.feedbackEvent("user-button-click",a)},ROS3D.InteractiveMarker.prototype.setPosition=function(a,b){this.position.copy(b),this.feedbackEvent("user-pose-change",a)},ROS3D.InteractiveMarker.prototype.setOrientation=function(a,b){b.normalize(),this.quaternion.copy(b),this.feedbackEvent("user-pose-change",a)},ROS3D.InteractiveMarker.prototype.onServerSetPose=function(a){if(void 0!==a)if(this.dragging)this.bufferedPoseEvent=a;else{var b=a.pose;this.position.x=b.position.x,this.position.y=b.position.y,this.position.z=b.position.z,this.quaternion.copy(new THREE.Quaternion(b.orientation.x,b.orientation.y,b.orientation.z,b.orientation.w)),this.updateMatrixWorld(!0)}},ROS3D.InteractiveMarker.prototype.dispose=function(){var a=this;this.children.forEach(function(b){b.children.forEach(function(a){a.dispose(),b.remove(a)}),a.remove(b)})},THREE.EventDispatcher.prototype.apply(ROS3D.InteractiveMarker.prototype),ROS3D.InteractiveMarkerClient=function(a){a=a||{},this.ros=a.ros,this.tfClient=a.tfClient,this.topicName=a.topic,this.path=a.path||"/",this.camera=a.camera,this.rootObject=a.rootObject||new THREE.Object3D,this.loader=a.loader||ROS3D.COLLADA_LOADER_2,this.menuFontSize=a.menuFontSize||"0.8em",this.interactiveMarkers={},this.updateTopic=null,this.feedbackTopic=null,this.topicName&&this.subscribe(this.topicName)},ROS3D.InteractiveMarkerClient.prototype.subscribe=function(a){this.unsubscribe(),this.updateTopic=new ROSLIB.Topic({ros:this.ros,name:a+"/tunneled/update",messageType:"visualization_msgs/InteractiveMarkerUpdate",compression:"png"}),this.updateTopic.subscribe(this.processUpdate.bind(this)),this.feedbackTopic=new ROSLIB.Topic({ros:this.ros,name:a+"/feedback",messageType:"visualization_msgs/InteractiveMarkerFeedback",compression:"png"}),this.feedbackTopic.advertise(),this.initService=new ROSLIB.Service({ros:this.ros,name:a+"/tunneled/get_init",serviceType:"demo_interactive_markers/GetInit"});var b=new ROSLIB.ServiceRequest({});this.initService.callService(b,this.processInit.bind(this))},ROS3D.InteractiveMarkerClient.prototype.unsubscribe=function(){this.updateTopic&&this.updateTopic.unsubscribe(),this.feedbackTopic&&this.feedbackTopic.unadvertise();for(var a in this.interactiveMarkers)this.eraseIntMarker(a);this.interactiveMarkers={}},ROS3D.InteractiveMarkerClient.prototype.processInit=function(a){var b=a.msg;b.erases=[];for(var c in this.interactiveMarkers)b.erases.push(c);b.poses=[],this.processUpdate(b)},ROS3D.InteractiveMarkerClient.prototype.processUpdate=function(a){var b=this;a.erases.forEach(function(a){b.eraseIntMarker(a)}),a.poses.forEach(function(a){var c=b.interactiveMarkers[a.name];c&&c.setPoseFromServer(a.pose)}),a.markers.forEach(function(a){var c=b.interactiveMarkers[a.name];c&&b.eraseIntMarker(c.name);var d=new ROS3D.InteractiveMarkerHandle({message:a,feedbackTopic:b.feedbackTopic,tfClient:b.tfClient,menuFontSize:b.menuFontSize});b.interactiveMarkers[a.name]=d;var e=new ROS3D.InteractiveMarker({handle:d,camera:b.camera,path:b.path,loader:b.loader});e.name=a.name,b.rootObject.add(e),d.on("pose",function(a){e.onServerSetPose({pose:a})}),e.addEventListener("user-pose-change",d.setPoseFromClientBound),e.addEventListener("user-mousedown",d.onMouseDownBound),e.addEventListener("user-mouseup",d.onMouseUpBound),e.addEventListener("user-button-click",d.onButtonClickBound),e.addEventListener("menu-select",d.onMenuSelectBound),d.subscribeTf()})},ROS3D.InteractiveMarkerClient.prototype.eraseIntMarker=function(a){if(this.interactiveMarkers[a]){var b=this.rootObject.getObjectByName(a);this.rootObject.remove(b);var c=this.interactiveMarkers[a];c.unsubscribeTf(),b.removeEventListener("user-pose-change",c.setPoseFromClientBound),b.removeEventListener("user-mousedown",c.onMouseDownBound),b.removeEventListener("user-mouseup",c.onMouseUpBound),b.removeEventListener("user-button-click",c.onButtonClickBound),b.removeEventListener("menu-select",c.onMenuSelectBound),delete this.interactiveMarkers[a],b.dispose()}},ROS3D.InteractiveMarkerControl=function(a){function b(a){a.stopPropagation()}var c=this;THREE.Object3D.call(this),a=a||{},this.parent=a.parent;var d=a.handle,e=a.message;this.name=e.name,this.camera=a.camera,this.path=a.path||"/",this.loader=a.loader||ROS3D.COLLADA_LOADER_2,this.dragging=!1,this.startMousePos=new THREE.Vector2;var f=new THREE.Quaternion(e.orientation.x,e.orientation.y,e.orientation.z,e.orientation.w);f.normalize();var g=new THREE.Vector3(1,0,0);switch(g.applyQuaternion(f),this.currentControlOri=new THREE.Quaternion,e.interaction_mode){case ROS3D.INTERACTIVE_MARKER_MOVE_AXIS:this.addEventListener("mousemove",this.parent.moveAxis.bind(this.parent,this,g)),this.addEventListener("touchmove",this.parent.moveAxis.bind(this.parent,this,g));break;case ROS3D.INTERACTIVE_MARKER_ROTATE_AXIS:this.addEventListener("mousemove",this.parent.rotateAxis.bind(this.parent,this,f));break;case ROS3D.INTERACTIVE_MARKER_MOVE_PLANE:this.addEventListener("mousemove",this.parent.movePlane.bind(this.parent,this,g));break;case ROS3D.INTERACTIVE_MARKER_BUTTON:this.addEventListener("click",this.parent.buttonClick.bind(this.parent,this))}e.interaction_mode!==ROS3D.INTERACTIVE_MARKER_NONE&&(this.addEventListener("mousedown",this.parent.startDrag.bind(this.parent,this)),this.addEventListener("mouseup",this.parent.stopDrag.bind(this.parent,this)),this.addEventListener("contextmenu",this.parent.showMenu.bind(this.parent,this)),this.addEventListener("mouseup",function(a){0===c.startMousePos.distanceToSquared(a.mousePos)&&(a.type="contextmenu",c.dispatchEvent(a))}),this.addEventListener("mouseover",b),this.addEventListener("mouseout",b),this.addEventListener("click",b),this.addEventListener("mousedown",function(a){c.startMousePos=a.mousePos}),this.addEventListener("touchstart",function(a){1===a.domEvent.touches.length&&(a.type="mousedown",a.domEvent.button=0,c.dispatchEvent(a))}),this.addEventListener("touchmove",function(a){1===a.domEvent.touches.length&&(a.type="mousemove",a.domEvent.button=0,c.dispatchEvent(a))}),this.addEventListener("touchend",function(a){0===a.domEvent.touches.length&&(a.domEvent.button=0,a.type="mouseup",c.dispatchEvent(a),a.type="click",c.dispatchEvent(a))}));var h=new THREE.Quaternion,i=this.parent.position.clone().multiplyScalar(-1);switch(e.orientation_mode){case ROS3D.INTERACTIVE_MARKER_INHERIT:h=this.parent.quaternion.clone().inverse(),this.updateMatrixWorld=function(a){ROS3D.InteractiveMarkerControl.prototype.updateMatrixWorld.call(c,a),c.currentControlOri.copy(c.quaternion),c.currentControlOri.normalize()};break;case ROS3D.INTERACTIVE_MARKER_FIXED:this.updateMatrixWorld=function(a){c.quaternion.copy(c.parent.quaternion.clone().inverse()),c.updateMatrix(),c.matrixWorldNeedsUpdate=!0,ROS3D.InteractiveMarkerControl.prototype.updateMatrixWorld.call(c,a),c.currentControlOri.copy(c.quaternion)};break;case ROS3D.INTERACTIVE_MARKER_VIEW_FACING:var j=e.independent_marker_orientation;this.updateMatrixWorld=function(a){c.camera.updateMatrixWorld();var b=(new THREE.Matrix4).extractRotation(c.camera.matrixWorld),d=new THREE.Matrix4,e=.5*Math.PI,f=new THREE.Euler(-e,0,e);d.makeRotationFromEuler(f);var g=new THREE.Matrix4;g.getInverse(c.parent.matrixWorld),b.multiplyMatrices(b,d),b.multiplyMatrices(g,b),c.currentControlOri.setFromRotationMatrix(b),j||(c.quaternion.copy(c.currentControlOri),c.updateMatrix(),c.matrixWorldNeedsUpdate=!0),ROS3D.InteractiveMarkerControl.prototype.updateMatrixWorld.call(c,a)};break;default:console.error("Unkown orientation mode: "+e.orientation_mode)}var k=new ROSLIB.TFClient({ros:d.tfClient.ros,fixedFrame:d.message.header.frame_id,serverName:d.tfClient.serverName});e.markers.forEach(function(a){var b=function(b){var d=new ROS3D.Marker({message:a,path:c.path,loader:c.loader});if(null!==b){var e=new ROSLIB.Pose({position:d.position,orientation:d.quaternion});e.applyTransform(new ROSLIB.Transform(b));var f=new ROS3D.Marker({message:a,path:c.path,loader:c.loader});f.position.add(i),f.position.applyQuaternion(h),f.quaternion.multiplyQuaternions(h,f.quaternion);var g=new THREE.Vector3(f.position.x,f.position.y,f.position.z),j=new ROSLIB.Transform({translation:g,orientation:f.quaternion});e.applyTransform(j),d.setPose(e),d.updateMatrixWorld(),k.unsubscribe(a.header.frame_id)}c.add(d)};""!==a.header.frame_id?k.subscribe(a.header.frame_id,b):b(null)})},ROS3D.InteractiveMarkerControl.prototype.__proto__=THREE.Object3D.prototype,ROS3D.InteractiveMarkerHandle=function(a){a=a||{},this.message=a.message,this.feedbackTopic=a.feedbackTopic,this.tfClient=a.tfClient,this.menuFontSize=a.menuFontSize||"0.8em",this.name=this.message.name,this.header=this.message.header,this.controls=this.message.controls,this.menuEntries=this.message.menu_entries,this.dragging=!1,this.timeoutHandle=null,this.tfTransform=new ROSLIB.Transform,this.pose=new ROSLIB.Pose,this.setPoseFromClientBound=this.setPoseFromClient.bind(this),this.onMouseDownBound=this.onMouseDown.bind(this),this.onMouseUpBound=this.onMouseUp.bind(this),this.onButtonClickBound=this.onButtonClick.bind(this),this.onMenuSelectBound=this.onMenuSelect.bind(this),this.setPoseFromServer(this.message.pose), +this.tfUpdateBound=this.tfUpdate.bind(this)},ROS3D.InteractiveMarkerHandle.prototype.__proto__=EventEmitter2.prototype,ROS3D.InteractiveMarkerHandle.prototype.subscribeTf=function(){0===this.message.header.stamp.secs&&0===this.message.header.stamp.nsecs&&this.tfClient.subscribe(this.message.header.frame_id,this.tfUpdateBound)},ROS3D.InteractiveMarkerHandle.prototype.unsubscribeTf=function(){this.tfClient.unsubscribe(this.message.header.frame_id,this.tfUpdateBound)},ROS3D.InteractiveMarkerHandle.prototype.emitServerPoseUpdate=function(){var a=new ROSLIB.Pose(this.pose);a.applyTransform(this.tfTransform),this.emit("pose",a)},ROS3D.InteractiveMarkerHandle.prototype.setPoseFromServer=function(a){this.pose=new ROSLIB.Pose(a),this.emitServerPoseUpdate()},ROS3D.InteractiveMarkerHandle.prototype.tfUpdate=function(a){this.tfTransform=new ROSLIB.Transform(a),this.emitServerPoseUpdate()},ROS3D.InteractiveMarkerHandle.prototype.setPoseFromClient=function(a){this.pose=new ROSLIB.Pose(a);var b=this.tfTransform.clone();b.rotation.invert(),b.translation.multiplyQuaternion(b.rotation),b.translation.x*=-1,b.translation.y*=-1,b.translation.z*=-1,this.pose.applyTransform(b),this.sendFeedback(ROS3D.INTERACTIVE_MARKER_POSE_UPDATE,void 0,0,a.controlName),this.dragging&&(this.timeoutHandle&&clearTimeout(this.timeoutHandle),this.timeoutHandle=setTimeout(this.setPoseFromClient.bind(this,a),250))},ROS3D.InteractiveMarkerHandle.prototype.onButtonClick=function(a){this.sendFeedback(ROS3D.INTERACTIVE_MARKER_BUTTON_CLICK,a.clickPosition,0,a.controlName)},ROS3D.InteractiveMarkerHandle.prototype.onMouseDown=function(a){this.sendFeedback(ROS3D.INTERACTIVE_MARKER_MOUSE_DOWN,a.clickPosition,0,a.controlName),this.dragging=!0},ROS3D.InteractiveMarkerHandle.prototype.onMouseUp=function(a){this.sendFeedback(ROS3D.INTERACTIVE_MARKER_MOUSE_UP,a.clickPosition,0,a.controlName),this.dragging=!1,this.timeoutHandle&&clearTimeout(this.timeoutHandle)},ROS3D.InteractiveMarkerHandle.prototype.onMenuSelect=function(a){this.sendFeedback(ROS3D.INTERACTIVE_MARKER_MENU_SELECT,void 0,a.id,a.controlName)},ROS3D.InteractiveMarkerHandle.prototype.sendFeedback=function(a,b,c,d){var e=void 0!==b;b=b||{x:0,y:0,z:0};var f={header:this.header,client_id:this.clientID,marker_name:this.name,control_name:d,event_type:a,pose:this.pose,mouse_point:b,mouse_point_valid:e,menu_entry_id:c};this.feedbackTopic.publish(f)},ROS3D.InteractiveMarkerMenu=function(a){function b(a,b){this.dispatchEvent({type:"menu-select",domEvent:b,id:a.id,controlName:this.controlName}),this.hide(b)}function c(a,e){var f=document.createElement("ul");a.appendChild(f);for(var g=e.children,h=0;h0?(c(i,g[h]),j.addEventListener("click",d.hide.bind(d)),j.addEventListener("touchstart",d.hide.bind(d))):(j.addEventListener("click",b.bind(d,g[h])),j.addEventListener("touchstart",b.bind(d,g[h])),j.className="default-interactive-marker-menu-entry")}}var d=this;a=a||{};var e=a.menuEntries,f=a.className||"default-interactive-marker-menu",g=(a.entryClassName||"default-interactive-marker-menu-entry",a.overlayClassName||"default-interactive-marker-overlay"),h=a.menuFontSize||"0.8em",i=[];if(i[0]={children:[]},THREE.EventDispatcher.call(this),null===document.getElementById("default-interactive-marker-menu-css")){var j=document.createElement("style");j.id="default-interactive-marker-menu-css",j.type="text/css",j.innerHTML=".default-interactive-marker-menu {background-color: #444444;border: 1px solid #888888;border: 1px solid #888888;padding: 0px 0px 0px 0px;color: #FFFFFF;font-family: sans-serif;font-size: "+h+";z-index: 1002;}.default-interactive-marker-menu ul {padding: 0px 0px 5px 0px;margin: 0px;list-style-type: none;}.default-interactive-marker-menu ul li div {-webkit-touch-callout: none;-webkit-user-select: none;-khtml-user-select: none;-moz-user-select: none;-ms-user-select: none;user-select: none;cursor: default;padding: 3px 10px 3px 10px;}.default-interactive-marker-menu-entry:hover { background-color: #666666; cursor: pointer;}.default-interactive-marker-menu ul ul { font-style: italic; padding-left: 10px;}.default-interactive-marker-overlay { position: absolute; top: 0%; left: 0%; width: 100%; height: 100%; background-color: black; z-index: 1001; -moz-opacity: 0.0; opacity: .0; filter: alpha(opacity = 0);}",document.getElementsByTagName("head")[0].appendChild(j)}this.menuDomElem=document.createElement("div"),this.menuDomElem.style.position="absolute",this.menuDomElem.className=f,this.menuDomElem.addEventListener("contextmenu",function(a){a.preventDefault()}),this.overlayDomElem=document.createElement("div"),this.overlayDomElem.className=g,this.hideListener=this.hide.bind(this),this.overlayDomElem.addEventListener("contextmenu",this.hideListener),this.overlayDomElem.addEventListener("click",this.hideListener),this.overlayDomElem.addEventListener("touchstart",this.hideListener);var k,l,m;for(k=0;k0){var W=this.msgColor,X=document.createElement("canvas"),Y=X.getContext("2d"),Z=100,$="normal "+Z+"px sans-serif";Y.font=$;var _=Y.measureText(c.text),aa=_.width;X.width=aa,X.height=1.5*Z,Y.font=$,Y.fillStyle="rgba("+Math.round(255*W.r)+", "+Math.round(255*W.g)+", "+Math.round(255*W.b)+", "+W.a+")",Y.textAlign="left",Y.textBaseline="middle",Y.fillText(c.text,0,X.height/2);var ba=new THREE.Texture(X);ba.needsUpdate=!0;var ca=new THREE.SpriteMaterial({map:ba,useScreenCoordinates:!1}),da=new THREE.Sprite(ca),ea=c.scale.x;da.scale.set(aa/X.height*ea,ea,1),this.add(da)}break;case ROS3D.MARKER_MESH_RESOURCE:var fa=null;0===c.color.r&&0===c.color.g&&0===c.color.b&&0===c.color.a||(fa=e),this.msgMesh=c.mesh_resource.substr(10);var ga=new ROS3D.MeshResource({path:b,resource:this.msgMesh,material:fa,loader:d});this.add(ga);break;case ROS3D.MARKER_TRIANGLE_LIST:var ha=new ROS3D.TriangleList({material:e,vertices:c.points,colors:c.colors});ha.scale.set(c.scale.x,c.scale.y,c.scale.z),this.add(ha);break;default:console.error("Currently unsupported marker type: "+c.type)}},ROS3D.Marker.prototype.__proto__=THREE.Object3D.prototype,ROS3D.Marker.prototype.setPose=function(a){this.position.x=a.position.x,this.position.y=a.position.y,this.position.z=a.position.z,this.quaternion.set(a.orientation.x,a.orientation.y,a.orientation.z,a.orientation.w),this.quaternion.normalize(),this.updateMatrixWorld()},ROS3D.Marker.prototype.update=function(a){if(this.setPose(a.pose),a.color.r!==this.msgColor.r||a.color.g!==this.msgColor.g||a.color.b!==this.msgColor.b||a.color.a!==this.msgColor.a){var b=ROS3D.makeColorMaterial(a.color.r,a.color.g,a.color.b,a.color.a);switch(a.type){case ROS3D.MARKER_LINE_STRIP:case ROS3D.MARKER_LINE_LIST:case ROS3D.MARKER_POINTS:break;case ROS3D.MARKER_ARROW:case ROS3D.MARKER_CUBE:case ROS3D.MARKER_SPHERE:case ROS3D.MARKER_CYLINDER:case ROS3D.MARKER_TRIANGLE_LIST:case ROS3D.MARKER_TEXT_VIEW_FACING:this.traverse(function(a){a instanceof THREE.Mesh&&(a.material=b)});break;case ROS3D.MARKER_MESH_RESOURCE:var c=null;0===a.color.r&&0===a.color.g&&0===a.color.b&&0===a.color.a||(c=this.colorMaterial),this.traverse(function(a){a instanceof THREE.Mesh&&(a.material=c)});break;case ROS3D.MARKER_CUBE_LIST:case ROS3D.MARKER_SPHERE_LIST:return!1;default:return!1}this.msgColor=a.color}var d=Math.abs(this.msgScale[0]-a.scale.x)>1e-6||Math.abs(this.msgScale[1]-a.scale.y)>1e-6||Math.abs(this.msgScale[2]-a.scale.z)>1e-6;switch(this.msgScale=[a.scale.x,a.scale.y,a.scale.z],a.type){case ROS3D.MARKER_CUBE:case ROS3D.MARKER_SPHERE:case ROS3D.MARKER_CYLINDER:if(d)return!1;break;case ROS3D.MARKER_TEXT_VIEW_FACING:if(d||this.text!==a.text)return!1;break;case ROS3D.MARKER_MESH_RESOURCE:var e=a.mesh_resource.substr(10);if(e!==this.msgMesh)return!1;if(d)return!1;break;case ROS3D.MARKER_ARROW:case ROS3D.MARKER_LINE_STRIP:case ROS3D.MARKER_LINE_LIST:case ROS3D.MARKER_CUBE_LIST:case ROS3D.MARKER_SPHERE_LIST:case ROS3D.MARKER_POINTS:case ROS3D.MARKER_TRIANGLE_LIST:return!1}return!0},ROS3D.Marker.prototype.dispose=function(){this.children.forEach(function(a){a instanceof ROS3D.MeshResource?a.children.forEach(function(b){void 0!==b.material&&b.material.dispose(),b.children.forEach(function(a){void 0!==a.geometry&&a.geometry.dispose(),void 0!==a.material&&a.material.dispose(),b.remove(a)}),a.remove(b)}):(void 0!==a.geometry&&a.geometry.dispose(),void 0!==a.material&&a.material.dispose()),a.parent.remove(a)})},ROS3D.MarkerArrayClient=function(a){a=a||{},this.ros=a.ros,this.topicName=a.topic,this.tfClient=a.tfClient,this.rootObject=a.rootObject||new THREE.Object3D,this.path=a.path||"/",this.loader=a.loader||ROS3D.COLLADA_LOADER_2,this.markers={},this.rosTopic=void 0,this.subscribe()},ROS3D.MarkerArrayClient.prototype.__proto__=EventEmitter2.prototype,ROS3D.MarkerArrayClient.prototype.subscribe=function(){this.unsubscribe(),this.rosTopic=new ROSLIB.Topic({ros:this.ros,name:this.topicName,messageType:"visualization_msgs/MarkerArray",compression:"png",queue_length:2}),this.rosTopic.subscribe(this.processMessage.bind(this))},ROS3D.MarkerArrayClient.prototype.processMessage=function(a){a.markers.forEach(function(a){if(0===a.action){var b=!1;if(a.ns+a.id in this.markers&&(b=this.markers[a.ns+a.id].children[0].update(a),b||(this.markers[a.ns+a.id].unsubscribeTf(),this.rootObject.remove(this.markers[a.ns+a.id]))),!b){var c=new ROS3D.Marker({message:a,path:this.path,loader:this.loader});this.markers[a.ns+a.id]=new ROS3D.SceneNode({frameID:a.header.frame_id.replace(/^\//,""),tfClient:this.tfClient,object:c}),this.rootObject.add(this.markers[a.ns+a.id])}}else if(1===a.action)console.warn('Received marker message with deprecated action identifier "1"');else if(2===a.action)a.ns+a.id in this.markers&&(this.markers[a.ns+a.id].unsubscribeTf(),this.rootObject.remove(this.markers[a.ns+a.id]),delete this.markers[a.ns+a.id]);else if(3===a.action){for(var d in this.markers)this.markers[d].unsubscribeTf(),this.rootObject.remove(this.markers[d]);this.markers={}}else console.warn('Received marker message with unknown action identifier "'+a.action+'"')}.bind(this)),this.emit("change")},ROS3D.MarkerArrayClient.prototype.unsubscribe=function(){this.rosTopic&&this.rosTopic.unsubscribe()},ROS3D.MarkerArrayClient.prototype.removeArray=function(){this.rosTopic.unsubscribe();for(var a in this.markers)this.markers.hasOwnProperty(a)&&(this.markers[a].unsubscribeTf(),this.rootObject.remove(this.markers[a]));this.markers={}},ROS3D.MarkerClient=function(a){a=a||{},this.ros=a.ros,this.topicName=a.topic,this.tfClient=a.tfClient,this.rootObject=a.rootObject||new THREE.Object3D,this.path=a.path||"/",this.loader=a.loader||ROS3D.COLLADA_LOADER_2,this.markers={},this.rosTopic=void 0,this.subscribe()},ROS3D.MarkerClient.prototype.__proto__=EventEmitter2.prototype,ROS3D.MarkerClient.prototype.unsubscribe=function(){this.rosTopic&&this.rosTopic.unsubscribe()},ROS3D.MarkerClient.prototype.subscribe=function(){this.unsubscribe(),this.rosTopic=new ROSLIB.Topic({ros:this.ros,name:this.topicName,messageType:"visualization_msgs/Marker",compression:"png"}),this.rosTopic.subscribe(this.processMessage.bind(this))},ROS3D.MarkerClient.prototype.processMessage=function(a){var b=new ROS3D.Marker({message:a,path:this.path,loader:this.loader}),c=this.markers[a.ns+a.id];c&&(c.unsubscribeTf(),this.rootObject.remove(c)),this.markers[a.ns+a.id]=new ROS3D.SceneNode({frameID:a.header.frame_id.replace(/^\//,""),tfClient:this.tfClient,object:b}),this.rootObject.add(this.markers[a.ns+a.id]),this.emit("change")},ROS3D.Arrow=function(a){a=a||{};var b=a.origin||new THREE.Vector3(0,0,0),c=a.direction||new THREE.Vector3(1,0,0),d=a.length||1,e=a.headLength||.2,f=a.shaftDiameter||.05,g=a.headDiameter||.1,h=a.material||new THREE.MeshBasicMaterial,i=d-e,j=new THREE.CylinderGeometry(.5*f,.5*f,i,12,1),k=new THREE.Matrix4;k.setPosition(new THREE.Vector3(0,.5*i,0)),j.applyMatrix(k);var l=new THREE.CylinderGeometry(0,.5*g,e,12,1);k.setPosition(new THREE.Vector3(0,i+.5*e,0)),l.applyMatrix(k),j.merge(l),THREE.Mesh.call(this,j,h),this.position.copy(b),this.setDirection(c)},ROS3D.Arrow.prototype.__proto__=THREE.Mesh.prototype,ROS3D.Arrow.prototype.setDirection=function(a){var b=new THREE.Vector3(0,1,0).cross(a),c=Math.acos(new THREE.Vector3(0,1,0).dot(a.clone().normalize()));this.matrix=(new THREE.Matrix4).makeRotationAxis(b.normalize(),c),this.rotation.setFromRotationMatrix(this.matrix,this.rotation.order)},ROS3D.Arrow.prototype.setLength=function(a){this.scale.set(a,a,a)},ROS3D.Arrow.prototype.setColor=function(a){this.material.color.setHex(a)},ROS3D.Arrow.prototype.dispose=function(){void 0!==this.geometry&&this.geometry.dispose(),void 0!==this.material&&this.material.dispose()},ROS3D.Arrow2=function(a){a=a||{};var b=a.origin||new THREE.Vector3(0,0,0),c=a.direction||new THREE.Vector3(1,0,0),d=a.length||1;a.headLength||.2,a.shaftDiameter||.05,a.headDiameter||.1,a.material||new THREE.MeshBasicMaterial;THREE.ArrowHelper.call(this,c,b,d,16711680)},ROS3D.Arrow2.prototype.__proto__=THREE.ArrowHelper.prototype,ROS3D.Arrow2.prototype.dispose=function(){void 0!==this.line&&(this.line.material.dispose(),this.line.geometry.dispose()),void 0!==this.cone&&(this.cone.material.dispose(),this.cone.geometry.dispose())},ROS3D.Axes=function(a){function b(a){var b=new THREE.Color;b.setRGB(a.x,a.y,a.z);var d=new THREE.MeshBasicMaterial({color:b.getHex()}),e=new THREE.Vector3;e.crossVectors(a,new THREE.Vector3(0,-1,0));var f=new THREE.Quaternion;f.setFromAxisAngle(e,.5*Math.PI);var g=new THREE.Mesh(c.headGeom,d);g.position.copy(a),g.position.multiplyScalar(.95),g.quaternion.copy(f),g.updateMatrix(),c.add(g);var h=new THREE.Mesh(c.lineGeom,d);h.position.copy(a),h.position.multiplyScalar(.45),h.quaternion.copy(f),h.updateMatrix(),c.add(h)}var c=this;a=a||{};var d=a.shaftRadius||.008,e=a.headRadius||.023,f=a.headLength||.1,g=a.scale||1;THREE.Object3D.call(this),this.scale.set(g,g,g),this.lineGeom=new THREE.CylinderGeometry(d,d,1-f),this.headGeom=new THREE.CylinderGeometry(0,e,f),b(new THREE.Vector3(1,0,0)),b(new THREE.Vector3(0,1,0)),b(new THREE.Vector3(0,0,1))},ROS3D.Axes.prototype.__proto__=THREE.Object3D.prototype,ROS3D.Grid=function(a){a=a||{};var b=a.num_cells||10,c=a.color||"#cccccc",d=a.lineWidth||1,e=a.cellSize||1;THREE.Object3D.call(this);for(var f=new THREE.LineBasicMaterial({color:c,linewidth:d}),g=0;g<=b;++g){var h=e*b/2,i=h-g*e,j=new THREE.Geometry;j.vertices.push(new THREE.Vector3(-h,i,0),new THREE.Vector3(h,i,0));var k=new THREE.Geometry;k.vertices.push(new THREE.Vector3(i,-h,0),new THREE.Vector3(i,h,0)),this.add(new THREE.Line(j,f)),this.add(new THREE.Line(k,f))}},ROS3D.Grid.prototype.__proto__=THREE.Object3D.prototype,ROS3D.MeshResource=function(a){var b=this;a=a||{};var c=a.path||"/",d=a.resource,e=a.material||null;this.warnings=a.warnings;var f=a.loader||ROS3D.COLLADA_LOADER_2;THREE.Object3D.call(this),"/"!==c.substr(c.length-1)&&(this.path+="/");var g,h=c+d,i=h.substr(-4).toLowerCase();".dae"===i?(g=f===ROS3D.COLLADA_LOADER?new THREE.ColladaLoader:new ColladaLoader2,g.log=function(a){b.warnings&&console.warn(a)},g.load(h,function(a){if(f===ROS3D.COLLADA_LOADER_2&&a.dae.asset.unit){var c=a.dae.asset.unit;a.scene.scale.set(c,c,c)}if(null!==e){var d=function(a,b){if(a.material=b,a.children)for(var c=0;c=this.keep&&(this.sns[0].unsubscribeTf(),this.rootObject.remove(this.sns[0]),this.sns.shift()),this.options.origin=new THREE.Vector3(a.pose.pose.position.x,a.pose.pose.position.y,a.pose.pose.position.z);var b=new THREE.Quaternion(a.pose.pose.orientation.x,a.pose.pose.orientation.y,a.pose.pose.orientation.z,a.pose.pose.orientation.w);this.options.direction=new THREE.Vector3(1,0,0),this.options.direction.applyQuaternion(b),this.options.material=new THREE.MeshBasicMaterial({color:this.color});var c=new ROS3D.Arrow(this.options);this.sns.push(new ROS3D.SceneNode({frameID:a.header.frame_id,tfClient:this.tfClient,object:c})),this.rootObject.add(this.sns[this.sns.length-1])},ROS3D.Path=function(a){a=a||{},this.ros=a.ros,this.topicName=a.topic||"/path",this.tfClient=a.tfClient,this.color=a.color||13369599,this.rootObject=a.rootObject||new THREE.Object3D,THREE.Object3D.call(this),this.sn=null,this.line=null,this.rosTopic=void 0,this.subscribe()},ROS3D.Path.prototype.__proto__=THREE.Object3D.prototype,ROS3D.Path.prototype.unsubscribe=function(){this.rosTopic&&this.rosTopic.unsubscribe()},ROS3D.Path.prototype.subscribe=function(){this.unsubscribe(),this.rosTopic=new ROSLIB.Topic({ros:this.ros,name:this.topicName,messageType:"nav_msgs/Path"}),this.rosTopic.subscribe(this.processMessage.bind(this))},ROS3D.Path.prototype.processMessage=function(a){null!==this.sn&&(this.sn.unsubscribeTf(),this.rootObject.remove(this.sn));for(var b=new THREE.Geometry,c=0;ca.range_max)this.particles.alpha[c]=0;else{var e=a.angle_min+c*a.angle_increment;this.particles.points[c]=new THREE.Vector3(d*Math.cos(e),d*Math.sin(e),0),this.particles.alpha[c]=1}this.particles.colors[c]=new THREE.Color(this.color)}finishedUpdate(this.particles,b)},ROS3D.Particles=function(a){a=a||{},this.tfClient=a.tfClient;var b=a.texture||"https://upload.wikimedia.org/wikipedia/commons/a/a2/Pixel-white.png",c=a.size||.05;this.max_pts=a.max_pts||1e4,this.first_size=null,this.prev_pts=0,this.rootObject=a.rootObject||new THREE.Object3D;THREE.Object3D.call(this),this.vertex_shader=["attribute vec3 customColor;","attribute float alpha;","varying vec3 vColor;","varying float falpha;","void main() ","{"," vColor = customColor; // set color associated to vertex; use later in fragment shader"," vec4 mvPosition = modelViewMatrix * vec4( position, 1.0 );"," falpha = alpha; ",""," // option (1): draw particles at constant size on screen"," // gl_PointSize = size;"," // option (2): scale particles as objects in 3D space"," gl_PointSize = ",c,"* ( 300.0 / length( mvPosition.xyz ) );"," gl_Position = projectionMatrix * mvPosition;","}"].join("\n"),this.fragment_shader=["uniform sampler2D texture;","varying vec3 vColor; // colors associated to vertices; assigned by vertex shader","varying float falpha;","void main() ","{"," // THREE.Material.alphaTest is not evaluated for ShaderMaterial, so we"," // have to take care of this ourselves."," if (falpha < 0.5) discard;"," // calculates a color for the particle"," gl_FragColor = vec4( vColor, falpha );"," // sets particle texture to desired color"," gl_FragColor = gl_FragColor * texture2D( texture, gl_PointCoord );","}"].join("\n"),this.geom=new THREE.Geometry;for(var d=0;d=0;f--)if(d[f].object===b[e]){c.push(d[f]);break}this.getWebglObjects(a,b[e].children,c)}},ROS3D.Highlighter.prototype.renderHighlight=function(a,b,c){var d=[];if(this.getWebglObjects(b,this.hoverObjs,d),0!==d.length){b.overrideMaterial=new THREE.MeshBasicMaterial({fog:!1,opacity:.5,depthTest:!0,depthWrite:!1,polygonOffset:!0,polygonOffsetUnits:-1,side:THREE.DoubleSide});var e=b.__webglObjects;b.__webglObjects=d,a.render(b,c),b.__webglObjects=e,b.overrideMaterial=null}},ROS3D.MouseHandler=function(a){THREE.EventDispatcher.call(this),this.renderer=a.renderer,this.camera=a.camera,this.rootObject=a.rootObject,this.fallbackTarget=a.fallbackTarget,this.lastTarget=this.fallbackTarget,this.dragging=!1;var b=["contextmenu","click","dblclick","mouseout","mousedown","mouseup","mousemove","mousewheel","DOMMouseScroll","touchstart","touchend","touchcancel","touchleave","touchmove"];this.listeners={},b.forEach(function(a){this.listeners[a]=this.processDomEvent.bind(this),this.renderer.domElement.addEventListener(a,this.listeners[a],!1)},this)},ROS3D.MouseHandler.prototype.processDomEvent=function(a){a.preventDefault();var b,c,d=a.target,e=d.getBoundingClientRect();if(a.type.indexOf("touch")!==-1){b=0,c=0;for(var f=0;f0?(d=o[0].object,n.intersection=this.lastIntersection=o[0]):d=this.fallbackTarget,d!==this.lastTarget&&a.type.match(/mouse/)){var p=this.notify(d,"mouseover",n);0===p?this.notify(this.lastTarget,"mouseout",n):1===p&&(d=this.fallbackTarget,d!==this.lastTarget&&(this.notify(d,"mouseover",n),this.notify(this.lastTarget,"mouseout",n)))}if(d!==this.lastTarget&&a.type.match(/touch/)){var q=this.notify(d,a.type,n);q?(this.notify(this.lastTarget,"touchleave",n),this.notify(this.lastTarget,"touchend",n)):(d=this.fallbackTarget,d!==this.lastTarget&&(this.notify(this.lastTarget,"touchmove",n),this.notify(this.lastTarget,"touchend",n)))}this.notify(d,a.type,n),"mousedown"!==a.type&&"touchstart"!==a.type&&"touchmove"!==a.type||(this.dragging=!0),this.lastTarget=d},ROS3D.MouseHandler.prototype.notify=function(a,b,c){for(c.type=b,c.cancelBubble=!1,c.continueBubble=!1,c.stopPropagation=function(){c.cancelBubble=!0},c.continuePropagation=function(){c.continueBubble=!0},c.currentTarget=a;c.currentTarget;){if(c.currentTarget.dispatchEvent&&c.currentTarget.dispatchEvent instanceof Function){if(c.currentTarget.dispatchEvent(c),c.cancelBubble)return this.dispatchEvent(c),0;if(c.continueBubble)return 2}c.currentTarget=c.currentTarget.parent}return 1},THREE.EventDispatcher.prototype.apply(ROS3D.MouseHandler.prototype),ROS3D.OrbitControls=function(a){function b(a){var b=a.domEvent;switch(b.preventDefault(),b.button){case 0:A=z.ROTATE,n.set(b.clientX,b.clientY);break;case 1:A=z.MOVE,u=new THREE.Vector3(0,0,1);var c=(new THREE.Matrix4).extractRotation(this.camera.matrix);u.applyMatrix4(c),t=j.center.clone(),v=j.camera.position.clone(),w=d(a.mouseRay,t,u),this.showAxes();break;case 2:A=z.ZOOM,q.set(b.clientX,b.clientY),this.showAxes()}}function c(a){var b=a.domEvent;if(A===z.ROTATE)o.set(b.clientX,b.clientY),p.subVectors(o,n),j.rotateLeft(2*Math.PI*p.x/l*j.userRotateSpeed),j.rotateUp(2*Math.PI*p.y/l*j.userRotateSpeed),n.copy(o);else if(A===z.ZOOM)r.set(b.clientX,b.clientY),s.subVectors(r,q),s.y>0?j.zoomIn():j.zoomOut(),q.copy(r),this.showAxes();else if(A===z.MOVE){var c=d(a.mouseRay,j.center,u);if(!c)return;var e=(new THREE.Vector3).subVectors(w.clone(),c.clone());j.center.addVectors(t.clone(),e.clone()),j.camera.position.addVectors(v.clone(),e.clone()),j.update(),j.camera.updateMatrixWorld(),this.showAxes()}}function d(a,b,c){var d=new THREE.Vector3,e=new THREE.Vector3;d.subVectors(b,a.origin);var f=a.direction.dot(c);if(Math.abs(f)0?j.zoomOut():j.zoomIn(),this.showAxes()}}function g(a){var b=a.domEvent;switch(b.touches.length){case 1:A=z.ROTATE,n.set(b.touches[0].pageX-window.scrollX,b.touches[0].pageY-window.scrollY);break;case 2:A=z.NONE,u=new THREE.Vector3(0,0,1);var c=(new THREE.Matrix4).extractRotation(this.camera.matrix);u.applyMatrix4(c),t=j.center.clone(),v=j.camera.position.clone(),w=d(a.mouseRay,t,u),x[0]=new THREE.Vector2(b.touches[0].pageX,b.touches[0].pageY),x[1]=new THREE.Vector2(b.touches[1].pageX,b.touches[1].pageY),y[0]=new THREE.Vector2(0,0),y[1]=new THREE.Vector2(0,0)}this.showAxes(),b.preventDefault()}function h(a){var b=a.domEvent;if(A===z.ROTATE)o.set(b.touches[0].pageX-window.scrollX,b.touches[0].pageY-window.scrollY),p.subVectors(o,n),j.rotateLeft(2*Math.PI*p.x/l*j.userRotateSpeed),j.rotateUp(2*Math.PI*p.y/l*j.userRotateSpeed),n.copy(o),this.showAxes();else{if(y[0].set(x[0].x-b.touches[0].pageX,x[0].y-b.touches[0].pageY),y[1].set(x[1].x-b.touches[1].pageX,x[1].y-b.touches[1].pageY),y[0].lengthSq()>m&&y[1].lengthSq()>m&&(x[0].set(b.touches[0].pageX,b.touches[0].pageY),x[1].set(b.touches[1].pageX,b.touches[1].pageY),y[0].dot(y[1])>0&&A!==z.ZOOM?A=z.MOVE:y[0].dot(y[1])<0&&A!==z.MOVE&&(A=z.ZOOM),A===z.ZOOM)){var c=new THREE.Vector2;c.subVectors(x[0],x[1]),y[0].dot(c)<0&&y[1].dot(c)>0?j.zoomOut():y[0].dot(c)>0&&y[1].dot(c)<0&&j.zoomIn()}if(A===z.MOVE){var e=d(a.mouseRay,j.center,u);if(!e)return;var f=(new THREE.Vector3).subVectors(w.clone(),e.clone());j.center.addVectors(t.clone(),f.clone()),j.camera.position.addVectors(v.clone(),f.clone()),j.update(),j.camera.updateMatrixWorld()}this.showAxes(),b.preventDefault()}}function i(a){var b=a.domEvent;1===b.touches.length&&A!==z.ROTATE?(A=z.ROTATE,n.set(b.touches[0].pageX-window.scrollX,b.touches[0].pageY-window.scrollY)):A=z.NONE}THREE.EventDispatcher.call(this);var j=this;a=a||{};var k=a.scene;this.camera=a.camera,this.center=new THREE.Vector3,this.userZoom=!0,this.userZoomSpeed=a.userZoomSpeed||1,this.userRotate=!0,this.userRotateSpeed=a.userRotateSpeed||1,this.autoRotate=a.autoRotate,this.autoRotateSpeed=a.autoRotateSpeed||2,this.camera.up=new THREE.Vector3(0,0,1);var l=1800,m=10,n=new THREE.Vector2,o=new THREE.Vector2,p=new THREE.Vector2,q=new THREE.Vector2,r=new THREE.Vector2,s=new THREE.Vector2,t=new THREE.Vector3,u=new THREE.Vector3,v=new THREE.Vector3,w=new THREE.Vector3,x=new Array(2),y=new Array(2);this.phiDelta=0,this.thetaDelta=0,this.scale=1,this.lastPosition=new THREE.Vector3;var z={NONE:-1,ROTATE:0,ZOOM:1,MOVE:2},A=z.NONE;this.axes=new ROS3D.Axes({shaftRadius:.025,headRadius:.07,headLength:.2}),k.add(this.axes),this.axes.traverse(function(a){a.visible=!1}),this.addEventListener("mousedown",b),this.addEventListener("mouseup",e),this.addEventListener("mousemove",c),this.addEventListener("touchstart",g),this.addEventListener("touchmove",h),this.addEventListener("touchend",i),this.addEventListener("mousewheel",f),this.addEventListener("DOMMouseScroll",f)},ROS3D.OrbitControls.prototype.showAxes=function(){var a=this;this.axes.traverse(function(a){a.visible=!0}),this.hideTimeout&&clearTimeout(this.hideTimeout),this.hideTimeout=setTimeout(function(){a.axes.traverse(function(a){a.visible=!1}),a.hideTimeout=!1},300)},ROS3D.OrbitControls.prototype.rotateLeft=function(a){void 0===a&&(a=2*Math.PI/60/60*this.autoRotateSpeed),this.thetaDelta-=a},ROS3D.OrbitControls.prototype.rotateRight=function(a){void 0===a&&(a=2*Math.PI/60/60*this.autoRotateSpeed),this.thetaDelta+=a},ROS3D.OrbitControls.prototype.rotateUp=function(a){void 0===a&&(a=2*Math.PI/60/60*this.autoRotateSpeed),this.phiDelta-=a},ROS3D.OrbitControls.prototype.rotateDown=function(a){void 0===a&&(a=2*Math.PI/60/60*this.autoRotateSpeed),this.phiDelta+=a},ROS3D.OrbitControls.prototype.zoomIn=function(a){void 0===a&&(a=Math.pow(.95,this.userZoomSpeed)),this.scale/=a},ROS3D.OrbitControls.prototype.zoomOut=function(a){void 0===a&&(a=Math.pow(.95,this.userZoomSpeed)),this.scale*=a},ROS3D.OrbitControls.prototype.update=function(){var a=this.camera.position,b=a.clone().sub(this.center),c=Math.atan2(b.y,b.x),d=Math.atan2(Math.sqrt(b.y*b.y+b.x*b.x),b.z);this.autoRotate&&this.rotateLeft(2*Math.PI/60/60*this.autoRotateSpeed),c+=this.thetaDelta,d+=this.phiDelta;var e=1e-6;d=Math.max(e,Math.min(Math.PI-e,d));var f=b.length();b.set(f*Math.sin(d)*Math.cos(c),f*Math.sin(d)*Math.sin(c),f*Math.cos(d)),b.multiplyScalar(this.scale),a.copy(this.center).add(b),this.camera.lookAt(this.center),f=b.length(),this.axes.position.copy(this.center),this.axes.scale.set(.05*f,.05*f,.05*f),this.axes.updateMatrixWorld(!0),this.thetaDelta=0,this.phiDelta=0,this.scale=1,this.lastPosition.distanceTo(this.camera.position)>0&&(this.dispatchEvent({type:"change"}),this.lastPosition.copy(this.camera.position))},THREE.EventDispatcher.prototype.apply(ROS3D.OrbitControls.prototype),ROS3D.SceneNode=function(a){a=a||{};var b=this;this.tfClient=a.tfClient||null,this.frameID=a.frameID;var c=a.object;this.pose=a.pose||new ROSLIB.Pose,THREE.Object3D.call(this),this.visible=!1,c&&this.add(c),this.updatePose(this.pose),this.tfUpdate=function(a){b.transformPose(a)},this.tfClient&&this.tfClient.subscribe(this.frameID,this.tfUpdate)},ROS3D.SceneNode.prototype.__proto__=THREE.Object3D.prototype,ROS3D.SceneNode.prototype.updatePose=function(a){this.position.set(a.position.x,a.position.y,a.position.z),this.quaternion.set(a.orientation.x,a.orientation.y,a.orientation.z,a.orientation.w),this.updateMatrixWorld(!0)},ROS3D.SceneNode.prototype.unsubscribeTf=function(){this.tfClient&&this.tfClient.unsubscribe(this.frameID,this.tfUpdate)},ROS3D.SceneNode.prototype.transformPose=function(a){var b=new ROSLIB.Transform(a),c=new ROSLIB.Pose(this.pose);c.applyTransform(b),this.updatePose(c),this.visible=!0},ROS3D.Viewer=function(a){a=a||{};var b=a.divID,c=a.canvas&&"canvas"===a.canvas.nodeName.toLowerCase()?a.canvas:void 0,d=a.width,e=a.height,f=a.background||"#111111",g=a.antialias,h=a.intensity||.66,i=a.near||.01,j=a.far||1e3,k=a.alpha||1,l=a.cameraPose||{x:3,y:3,z:3},m=a.cameraZoomSpeed||2.5;this.renderer=new THREE.WebGLRenderer({canvas:c,antialias:g,alpha:!0}),this.renderer.setClearColor(parseInt(f.replace("#","0x"),16),k),this.renderer.sortObjects=!1,this.renderer.setSize(d,e),this.renderer.shadowMapEnabled=!1,this.renderer.autoClear=!1,this.scene=new THREE.Scene,this.camera=new THREE.PerspectiveCamera(40,d/e,i,j),this.camera.position.set(l.x,l.y,l.z),this.cameraControls=new ROS3D.OrbitControls({scene:this.scene,camera:this.camera}),this.cameraControls.userZoomSpeed=m,this.scene.add(new THREE.AmbientLight(5592405)),this.directionalLight=new THREE.DirectionalLight(16777215,h),this.scene.add(this.directionalLight),this.selectableObjects=new THREE.Object3D,this.scene.add(this.selectableObjects);var n=new ROS3D.MouseHandler({renderer:this.renderer,camera:this.camera,rootObject:this.selectableObjects,fallbackTarget:this.cameraControls});if(this.highlighter=new ROS3D.Highlighter({mouseHandler:n}),this.stopped=!0,this.animationRequestId=void 0,b)document.getElementById(b).appendChild(this.renderer.domElement);else if(!c)throw new Error("No canvas nor HTML container provided for rendering.");this.start()},ROS3D.Viewer.prototype.start=function(){this.stopped=!1,this.draw()},ROS3D.Viewer.prototype.draw=function(){this.stopped||(this.cameraControls.update(),this.directionalLight.position.copy(this.camera.localToWorld(new THREE.Vector3(-1,1,0))),this.directionalLight.position.normalize(),this.renderer.clear(!0,!0,!0),this.renderer.render(this.scene,this.camera),this.animationRequestId=requestAnimationFrame(this.draw.bind(this)))},ROS3D.Viewer.prototype.stop=function(){this.stopped||cancelAnimationFrame(this.animationRequestId),this.stopped=!0},ROS3D.Viewer.prototype.addObject=function(a,b){b?this.selectableObjects.add(a):this.scene.add(a)},ROS3D.Viewer.prototype.resize=function(a,b){this.camera.aspect=a/b,this.camera.updateProjectionMatrix(),this.renderer.setSize(a,b)}; \ No newline at end of file diff --git a/src/Ros3D.js b/src/Ros3D.js index 6b4ae7e6..6a6863bd 100644 --- a/src/Ros3D.js +++ b/src/Ros3D.js @@ -63,7 +63,7 @@ ROS3D.makeColorMaterial = function(r, g, b, a) { if (a <= 0.99) { return new THREE.MeshBasicMaterial({ color : color.getHex(), - opacity : a + 0.1, + opacity : a, transparent : true, depthWrite : true, blendSrc : THREE.SrcAlphaFactor, @@ -149,9 +149,9 @@ ROS3D.closestAxisPoint = function(axisRay, camera, mousePos) { // project axis onto screen var o = axisRay.origin.clone(); - projector.projectVector(o, camera); + o.project(camera); var o2 = axisRay.direction.clone().add(axisRay.origin); - projector.projectVector(o2, camera); + o2.project(camera); // d is the axis vector in screen space (d = o2-o) var d = o2.clone().sub(o); @@ -167,7 +167,7 @@ ROS3D.closestAxisPoint = function(axisRay, camera, mousePos) { // go back to 3d by shooting a ray var vector = new THREE.Vector3(mp.x, mp.y, 0.5); - projector.unprojectVector(vector, camera); + vector.unproject(camera); var mpRay = new THREE.Ray(camera.position, vector.sub(camera.position).normalize()); return ROS3D.findClosestPoint(axisRay, mpRay); diff --git a/src/depthcloud/CloseCloud.js b/src/depthcloud/CloseCloud.js new file mode 100644 index 00000000..612d2f24 --- /dev/null +++ b/src/depthcloud/CloseCloud.js @@ -0,0 +1,336 @@ +/** + * @author Julius Kammerl - jkammerl@willowgarage.com + */ + +/** + * The CloseCloud object. An alternative DepthCloud implementation which + * has higher resolution when the depth range is less than 2m. + * + * @constructor + * @param options - object with following keys: + * + * * url - the URL of the stream + * * streamType (optional) - the stream type: mjpeg or vp8 video (defaults to vp8) + * * f (optional) - the camera's focal length (defaults to standard Kinect calibration) + * * pointSize (optional) - point size (pixels) for rendered point cloud + * * width (optional) - width of the depthcloud encoded video stream + * * height (optional) - height of the depthcloud encoded video stream + * * whiteness (optional) - blends rgb values to white (0..100) + * * varianceThreshold (optional) - threshold for variance filter, used for compression artifact removal + */ +ROS3D.CloseCloud = function(options) { + THREE.Object3D.call(this); + this.type = 'CloseCloud'; + + this.options = options || {}; + this.url = options.url; + this.streamType = options.streamType || 'vp8'; + this.f = options.f || 526; + this.pointSize = options.pointSize || 3; + this.width = options.width || 1024; + this.height = options.height || 1024; + this.whiteness = options.whiteness || 0; + this.varianceThreshold = options.varianceThreshold || 0.000016667; + + var metaLoaded = false; + + this.isMjpeg = this.streamType.toLowerCase() === 'mjpeg'; + + this.video = document.createElement(this.isMjpeg ? 'img' : 'video'); + this.video.width = this.width; + this.video.height = this.height; + this.video.addEventListener(this.isMjpeg ? 'load' : 'loadedmetadata', this.metaLoaded.bind(this), false); + + if (!this.isMjpeg) { + this.video.loop = true; + } + + this.video.src = this.url; + this.video.crossOrigin = 'Anonymous'; + this.video.setAttribute('crossorigin', 'Anonymous'); + + // define custom shaders + this.vertex_shader = [ + 'uniform sampler2D map;', + '', + 'uniform float width;', + 'uniform float height;', + '', + 'uniform float pointSize;', + 'uniform float zOffset;', + '', + 'uniform float focallength;', + '', + 'varying vec2 vUvP;', + 'varying vec2 colorP;', + '', + 'varying float depthVariance;', + 'varying float maskVal;', + '', + 'float sampleDepth(vec2 pos)', + ' {', + ' float depth;', + ' ', + ' vec2 vUv = vec2( pos.x / (width*2.0), pos.y / (height*2.0)+0.5 );', + ' vec2 vUv2 = vec2( pos.x / (width*2.0)+0.5, pos.y / (height*2.0)+0.5 );', + ' ', + ' vec4 depthColor = texture2D( map, vUv );', + ' ', + ' depth = ( depthColor.r + depthColor.g + depthColor.b ) / 3.0;', + ' ', + ' if (depth > (1.0 - 3.0/255.0) )', // If we're closer than 3 values from saturation, check the next depth image + ' {', + ' vec4 depthColor2 = texture2D( map, vUv2 );', + ' float depth2 = ( depthColor2.r + depthColor2.g + depthColor2.b ) / 3.0 ;', + ' depth += depth2;', + ' }', + ' ', + ' return depth;', + ' }', + '', + 'float median(float a, float b, float c)', + ' {', + ' float r=a;', + ' ', + ' if ( (a0.5) || (vUvP.y<0.5) || (vUvP.y>0.0))', + ' {', + ' vec2 smp = decodeDepth(vec2(position.x, position.y));', + ' float depth = smp.x;', + ' depthVariance = smp.y;', + ' ', + ' float z = -depth;', + ' ', + ' pos = vec4(', + ' ( position.x / width - 0.5 ) * z *0.5 * (1000.0/focallength) * -1.0,', + ' ( position.y / height - 0.5 ) * z *0.5 * (1000.0/focallength),', + ' (- z + zOffset / 1000.0) * 1.0,', + ' 1.0);', + ' ', + ' vec2 maskP = vec2( position.x / (width*2.0), position.y / (height*2.0) );', + ' vec4 maskColor = texture2D( map, maskP );', + ' maskVal = ( maskColor.r + maskColor.g + maskColor.b ) / 3.0 ;', + ' }', + ' ', + ' gl_PointSize = pointSize;', + ' gl_Position = projectionMatrix * modelViewMatrix * pos;', + ' ', + '}' + ].join('\n'); + + this.fragment_shader = [ + 'uniform sampler2D map;', + 'uniform float varianceThreshold;', + 'uniform float whiteness;', + '', + 'varying vec2 vUvP;', + 'varying vec2 colorP;', + '', + 'varying float depthVariance;', + 'varying float maskVal;', + '', + '', + 'void main() {', + ' ', + ' vec4 color;', + ' ', + ' if ( (depthVariance>varianceThreshold) || (maskVal>0.5) ||(vUvP.x<0.0)|| (vUvP.x>0.5) || (vUvP.y<0.5) || (vUvP.y>1.0))', + ' { ', + ' discard;', + ' }', + ' else ', + ' {', + ' color = texture2D( map, colorP );', + ' ', + ' float fader = whiteness /100.0;', + ' ', + ' color.r = color.r * (1.0-fader)+ fader;', + ' ', + ' color.g = color.g * (1.0-fader)+ fader;', + ' ', + ' color.b = color.b * (1.0-fader)+ fader;', + ' ', + ' color.a = 1.0;//smoothstep( 20000.0, -20000.0, gl_FragCoord.z / gl_FragCoord.w );', + ' }', + ' ', + ' gl_FragColor = vec4( color.r, color.g, color.b, color.a );', + ' ', + '}' + ].join('\n'); +}; +ROS3D.CloseCloud.prototype = Object.create( THREE.Object3D.prototype ); +ROS3D.CloseCloud.prototype.constructor = ROS3D.CloseCloud; + +ROS3D.CloseCloud.prototype.clone = function ( object, recursive ) { + + if ( object === undefined ) { object = new ROS3D.CloseCloud( this.options ); } + + THREE.Object3D.prototype.clone.call( this, object, recursive ); + + return object; + +}; + +/** + * Callback called when video metadata is ready + */ +ROS3D.CloseCloud.prototype.metaLoaded = function() { + this.metaLoaded = true; + this.initStreamer(); +}; + +/** + * Callback called when video metadata is ready + */ +ROS3D.CloseCloud.prototype.initStreamer = function() { + + if (this.metaLoaded) { + this.dctexture = new THREE.Texture(this.video); + this.dcgeometry = new THREE.Geometry(); + + var qwidth = this.width / 2; + var qheight = this.height / 2; + // the number of points is a forth of the total image size + for (var i = 0, l = qwidth * qheight; i < l; i++) { + + var vertex = new THREE.Vector3(); + vertex.x = (i % qwidth); + vertex.y = Math.floor(i / qwidth); + + this.dcgeometry.vertices.push(vertex); + } + + this.dcmaterial = new THREE.ShaderMaterial({ + uniforms : { + 'map' : { + type : 't', + value : this.dctexture + }, + 'width' : { + type : 'f', + value : qwidth + }, + 'height' : { + type : 'f', + value : qheight + }, + 'focallength' : { + type : 'f', + value : this.f + }, + 'pointSize' : { + type : 'f', + value : this.pointSize + }, + 'zOffset' : { + type : 'f', + value : 0 + }, + 'whiteness' : { + type : 'f', + value : this.whiteness + }, + 'varianceThreshold' : { + type : 'f', + value : this.varianceThreshold + } + }, + vertexShader : this.vertex_shader, + fragmentShader : this.fragment_shader + }); + this.dcmaterial.color = new THREE.Color( 0xffffff ); + this.mesh = new THREE.PointCloud(this.dcgeometry, this.dcmaterial); + this.mesh.frustumCulled = false; + this.mesh.position.set(0,0,0); + + this.add(this.mesh); + + var that = this; + this.interval = setInterval(function() { + if (that.isMjpeg || that.video.readyState === that.video.HAVE_ENOUGH_DATA) { + that.dctexture.needsUpdate = true; + } + }, 1000 / 30); + } +}; + +/** + * Start video playback + */ +ROS3D.CloseCloud.prototype.startStream = function() { + if (!this.isMjpeg) { + this.video.play(); + } +}; + +/** + * Stop video playback + */ +ROS3D.CloseCloud.prototype.stopStream = function() { + if (!this.isMjpeg) { + this.video.pause(); + } + this.video.src = ''; // forcefully silence the video streaming url. + clearInterval(this.interval); +}; diff --git a/src/depthcloud/DepthCloud.js b/src/depthcloud/DepthCloud.js index 7d7b0746..d95619ec 100644 --- a/src/depthcloud/DepthCloud.js +++ b/src/depthcloud/DepthCloud.js @@ -283,7 +283,7 @@ ROS3D.DepthCloud.prototype.initStreamer = function() { fragmentShader : this.fragment_shader }); - this.mesh = new THREE.ParticleSystem(this.geometry, this.material); + this.mesh = new THREE.PointCloud(this.geometry, this.material); this.mesh.position.x = 0; this.mesh.position.y = 0; this.add(this.mesh); diff --git a/src/depthcloud/PointCloud.js b/src/depthcloud/PointCloud.js new file mode 100644 index 00000000..827373ed --- /dev/null +++ b/src/depthcloud/PointCloud.js @@ -0,0 +1,138 @@ +/** + * @author Julius Kammerl - jkammerl@willowgarage.com + * @author Peter Soetens - peter@thesourceworks.com + */ + +/** + * The PointCloud object. Allows you to visualise PointCloud2 messages. + * + * @constructor + * @param options - object with following keys: + * + * * topic - the topic to subscribe to for PointCloud2 messages + * * pointSize (optional) - point size (pixels) for rendered point cloud + * * width (optional) - width of the video stream + * * height (optional) - height of the video stream + * * whiteness (optional) - blends rgb values to white (0..100) + */ +ROS3D.PointCloud = function(options) { + options = options || {}; + + this.topic = options.topic; + this.pointSize = options.pointSize || 3; + this.width = options.width || 640; + this.height = options.height || 480; + this.whiteness = options.whiteness || 0; + this.queue_length = options.queue_length || 0; + this.opacity = options.opacity || 1; + this.transparent = options.transparent || false; + + var ros = options.ros; + var topic = options.topic; + + var positions = new Float32Array(this.width * this.height * 3); + var colors = new Float32Array(this.width * this.height * 3); + + this.geometry = new THREE.BufferGeometry(); + + this.geometry.addAttribute('position', new THREE.BufferAttribute(positions, 3)); + this.geometry.addAttribute('color', new THREE.BufferAttribute(colors, 3)); + + this.mesh = new THREE.PointCloud( + this.geometry, + new THREE.PointCloudMaterial({ + size: this.pointSize / 1000.0, + opacity: this.opacity, + transparent: this.transparent, + vertexColors: THREE.VertexColors + }) + ); + + // If we don't clear this, the point cloud gets undrawn when we get too close with the camera, + // even if it doesn't seem close. + this.mesh.frustumCulled = false; + + // subscribe to the topic + this.rosTopic = new ROSLIB.Topic({ + ros : ros, + name : topic, + messageType : 'sensor_msgs/PointCloud2', + compression : 'png', + queue_length : this.queue_length, + throttle_rate : 500 + }); + +}; + +/** + * Start video playback + * @todo: move this code + coloring of the point cloud into a vertex+fragment shader. + * The message.data is a base64 encoded string, but also internally an Image object. + * Maybe the image can be passed to the vertex shader and decoded there, which would + * help in speeding up : receiving as png and no longer decompressing in javascript. + */ +ROS3D.PointCloud.prototype.startStream = function() { + var that = this, + position = that.geometry.attributes.position.array, + color = that.geometry.attributes.color.array; + + this.rosTopic.subscribe(function(message) { + var bufferView = new ROS3D.BufferView({ + data: message.data, + type: 'base64', + endian: ROS3D.LITTLE_ENDIAN + }); + var l = message.width * message.height, i = 0, i3 = 0; + + // Guard against empty pointcloud (zero points: zero message width or height). + if (l === 0) { + return; + } + + for (; i < that.width * that.height; i++, i3 += 3) { + + if ( i < l ) { + bufferView.resetSinglePointOffset(); + + position[i3] = bufferView.readFloat32(); + position[i3 + 1] = bufferView.readFloat32(); + position[i3 + 2] = bufferView.readFloat32(); + bufferView.incrementOffset(4); // skip empty channel + + switch (message.point_step) { + // message contains normals; colors are in the third channel + case 48: + bufferView.incrementOffset(16); + /* falls through */ + case 32: + color[i3 + 2] = bufferView.readUint8() / 255.0; // B + color[i3 + 1] = bufferView.readUint8() / 255.0; // G + color[i3] = bufferView.readUint8() / 255.0; // R + break; + // 16-byte point step messages don't have colors + case 16: + /* falls through */ + default: + } + + // adjust offset for the next point + bufferView.incrementOffset(message.point_step - bufferView.singlePointOffset); + } else { + // Converge all other points which should be invisible into the "last" point of the + // "visible" cloud (effectively reset) + position[i3] = position[i3 - 3].x; + position[i3 + 1] = position[i3 - 2].y; + position[i3 + 2] = position[i3 - 1].z; + } + } + that.geometry.attributes.position.needsUpdate = true; + that.geometry.attributes.color.needsUpdate = true; + }); +}; + +/** + * Stop video playback + */ +ROS3D.PointCloud.prototype.stopStream = function() { + this.rosTopic.unsubscribe(); +}; diff --git a/src/helpers/Helpers.js b/src/helpers/Helpers.js new file mode 100644 index 00000000..29079d5f --- /dev/null +++ b/src/helpers/Helpers.js @@ -0,0 +1,54 @@ +/** + * @author Peter Soetens - peter@pickit3d.com + * @author Viktor Kunovski - viktor@pickit3d.com + */ + +ROS3D.BIG_ENDIAN = 0; +ROS3D.LITTLE_ENDIAN = 1; + +ROS3D.BufferView = function(options) { + options = options || {}; + + this.endian = options.endian || ROS3D.BIG_ENDIAN; + this.type = options.type || 'base64'; + + var data = options.data || ''; + + this.view = new DataView(this.base64ToArrayBuffer(data)); + + this.offset = 0; + this.singlePointOffset = 0; +}; + +ROS3D.BufferView.prototype.readFloat32 = function() { + var value = this.view.getFloat32(this.offset, !!this.endian); + this.offset += 4; + this.singlePointOffset += 4; + return value; +}; + +ROS3D.BufferView.prototype.readUint8 = function() { + var value = this.view.getUint8(this.offset); + this.offset += 1; + this.singlePointOffset += 1; + return value; +}; + +ROS3D.BufferView.prototype.incrementOffset = function(offset) { + this.offset += +offset; + this.singlePointOffset += +offset; +}; + +ROS3D.BufferView.prototype.resetSinglePointOffset = function() { + this.singlePointOffset = 0; +}; + +ROS3D.BufferView.prototype.base64ToArrayBuffer = function(base64) { + var binaryString = window.atob(base64); + var len = binaryString.length; + var bytes = new Uint8Array(len); + for (var i = 0; i < len; i++) { + bytes[i] = binaryString.charCodeAt(i); + } + return bytes.buffer; +}; diff --git a/src/interactivemarkers/InteractiveMarker.js b/src/interactivemarkers/InteractiveMarker.js index 73c823e6..4c18d957 100644 --- a/src/interactivemarkers/InteractiveMarker.js +++ b/src/interactivemarkers/InteractiveMarker.js @@ -265,7 +265,7 @@ ROS3D.InteractiveMarker.prototype.buttonClick = function(control, event3d) { * @param event3d - the event that caused this */ ROS3D.InteractiveMarker.prototype.setPosition = function(control, position) { - this.position = position; + this.position.copy( position ); this.feedbackEvent('user-pose-change', control); }; @@ -277,7 +277,7 @@ ROS3D.InteractiveMarker.prototype.setPosition = function(control, position) { */ ROS3D.InteractiveMarker.prototype.setOrientation = function(control, orientation) { orientation.normalize(); - this.quaternion = orientation; + this.quaternion.copy( orientation ); this.feedbackEvent('user-pose-change', control); }; @@ -298,8 +298,8 @@ ROS3D.InteractiveMarker.prototype.onServerSetPose = function(event) { this.position.y = pose.position.y; this.position.z = pose.position.z; - this.quaternion = new THREE.Quaternion(pose.orientation.x, pose.orientation.y, - pose.orientation.z, pose.orientation.w); + this.quaternion.copy( new THREE.Quaternion(pose.orientation.x, pose.orientation.y, + pose.orientation.z, pose.orientation.w) ); this.updateMatrixWorld(true); } diff --git a/src/interactivemarkers/InteractiveMarkerControl.js b/src/interactivemarkers/InteractiveMarkerControl.js index 6bbaa4a6..129937a6 100644 --- a/src/interactivemarkers/InteractiveMarkerControl.js +++ b/src/interactivemarkers/InteractiveMarkerControl.js @@ -129,7 +129,7 @@ ROS3D.InteractiveMarkerControl = function(options) { break; case ROS3D.INTERACTIVE_MARKER_FIXED: this.updateMatrixWorld = function(force) { - that.quaternion = that.parent.quaternion.clone().inverse(); + that.quaternion.copy( that.parent.quaternion.clone().inverse() ); that.updateMatrix(); that.matrixWorldNeedsUpdate = true; ROS3D.InteractiveMarkerControl.prototype.updateMatrixWorld.call(that, force); diff --git a/src/markers/Marker.js b/src/markers/Marker.js index ccd3cfc3..de2761e3 100644 --- a/src/markers/Marker.js +++ b/src/markers/Marker.js @@ -79,7 +79,7 @@ ROS3D.Marker = function(options) { break; case ROS3D.MARKER_CUBE: // set the cube dimensions - var cubeGeom = new THREE.CubeGeometry(message.scale.x, message.scale.y, message.scale.z); + var cubeGeom = new THREE.BoxGeometry(message.scale.x, message.scale.y, message.scale.z); this.add(new THREE.Mesh(cubeGeom, colorMaterial)); break; case ROS3D.MARKER_SPHERE: @@ -96,13 +96,13 @@ ROS3D.Marker = function(options) { var cylinderGeom = new THREE.CylinderGeometry(0.5, 0.5, 1, 16, 1, false); var cylinderMesh = new THREE.Mesh(cylinderGeom, colorMaterial); cylinderMesh.quaternion.setFromAxisAngle(new THREE.Vector3(1, 0, 0), Math.PI * 0.5); - cylinderMesh.scale = new THREE.Vector3(message.scale.x, message.scale.z, message.scale.y); + cylinderMesh.scale.set(message.scale.x, message.scale.z, message.scale.y); this.add(cylinderMesh); break; case ROS3D.MARKER_LINE_STRIP: + var lineStripGeom = new THREE.Geometry(); var lineStripMaterial = new THREE.LineBasicMaterial({ - size : message.scale.x }); // add the points @@ -116,7 +116,7 @@ ROS3D.Marker = function(options) { } // determine the colors for each - if (message.colors.length === message.points.length) { + if ( message.colors.length === message.points.length) { lineStripMaterial.vertexColors = true; for ( j = 0; j < message.points.length; j++) { var clr = new THREE.Color(); @@ -133,7 +133,6 @@ ROS3D.Marker = function(options) { case ROS3D.MARKER_LINE_LIST: var lineListGeom = new THREE.Geometry(); var lineListMaterial = new THREE.LineBasicMaterial({ - size : message.scale.x }); // add the points @@ -174,7 +173,7 @@ ROS3D.Marker = function(options) { // add the points var p, cube, curColor, newMesh; for (p = 0; p < numPoints; p+=stepSize) { - cube = new THREE.CubeGeometry(message.scale.x, message.scale.y, message.scale.z); + cube = new THREE.BoxGeometry(message.scale.x, message.scale.y, message.scale.z); // check the color if(createColors) { @@ -228,7 +227,7 @@ ROS3D.Marker = function(options) { case ROS3D.MARKER_POINTS: // for now, use a particle system for the lists var geometry = new THREE.Geometry(); - var material = new THREE.ParticleBasicMaterial({ + var material = new THREE.PointCloudMaterial({ size : message.scale.x }); @@ -255,7 +254,7 @@ ROS3D.Marker = function(options) { } // add the particle system - this.add(new THREE.ParticleSystem(geometry, material)); + this.add(new THREE.PointCloud(geometry, material)); break; case ROS3D.MARKER_TEXT_VIEW_FACING: // only work on non-empty text @@ -324,7 +323,7 @@ ROS3D.Marker = function(options) { vertices : message.points, colors : message.colors }); - tri.scale = new THREE.Vector3(message.scale.x, message.scale.y, message.scale.z); + tri.scale.set(message.scale.x, message.scale.y, message.scale.z); this.add(tri); break; default: @@ -346,7 +345,7 @@ ROS3D.Marker.prototype.setPose = function(pose) { this.position.z = pose.position.z; // set the rotation - this.quaternion = new THREE.Quaternion(pose.orientation.x, pose.orientation.y, + this.quaternion.set( pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); this.quaternion.normalize(); diff --git a/src/markers/MarkerArrayClient.js b/src/markers/MarkerArrayClient.js index 3d6b416a..8c921747 100644 --- a/src/markers/MarkerArrayClient.js +++ b/src/markers/MarkerArrayClient.js @@ -46,8 +46,10 @@ ROS3D.MarkerArrayClient.prototype.subscribe = function(){ ros : this.ros, name : this.topicName, messageType : 'visualization_msgs/MarkerArray', - compression : 'png' + compression : 'png', + queue_length : 2 }); + this.rosTopic.subscribe(this.processMessage.bind(this)); }; @@ -69,7 +71,7 @@ ROS3D.MarkerArrayClient.prototype.processMessage = function(arrayMessage){ loader : this.loader }); this.markers[message.ns + message.id] = new ROS3D.SceneNode({ - frameID : message.header.frame_id, + frameID : message.header.frame_id.replace(/^\//, ''), tfClient : this.tfClient, object : newMarker }); @@ -80,9 +82,11 @@ ROS3D.MarkerArrayClient.prototype.processMessage = function(arrayMessage){ console.warn('Received marker message with deprecated action identifier "1"'); } else if(message.action === 2) { // "DELETE" - this.markers[message.ns + message.id].unsubscribeTf(); - this.rootObject.remove(this.markers[message.ns + message.id]); - delete this.markers[message.ns + message.id]; + if(message.ns + message.id in this.markers) { + this.markers[message.ns + message.id].unsubscribeTf(); + this.rootObject.remove(this.markers[message.ns + message.id]); + delete this.markers[message.ns + message.id]; + } } else if(message.action === 3) { // "DELETE ALL" for (var m in this.markers){ @@ -104,3 +108,14 @@ ROS3D.MarkerArrayClient.prototype.unsubscribe = function(){ this.rosTopic.unsubscribe(); } }; + +ROS3D.MarkerArrayClient.prototype.removeArray = function() { + this.rosTopic.unsubscribe(); + for (var key in this.markers) { + if (this.markers.hasOwnProperty(key)) { + this.markers[key].unsubscribeTf(); + this.rootObject.remove( this.markers[key] ); + } + } + this.markers = {}; +}; diff --git a/src/markers/MarkerClient.js b/src/markers/MarkerClient.js index 61d00782..237c0e48 100644 --- a/src/markers/MarkerClient.js +++ b/src/markers/MarkerClient.js @@ -71,7 +71,7 @@ ROS3D.MarkerClient.prototype.processMessage = function(message){ } this.markers[message.ns + message.id] = new ROS3D.SceneNode({ - frameID : message.header.frame_id, + frameID : message.header.frame_id.replace(/^\//, ''), tfClient : this.tfClient, object : newMarker }); diff --git a/src/models/Arrow.js b/src/models/Arrow.js index c20e5edf..561c1c12 100644 --- a/src/models/Arrow.js +++ b/src/models/Arrow.js @@ -41,11 +41,11 @@ ROS3D.Arrow = function(options) { coneGeometry.applyMatrix(m); // put the arrow together - THREE.GeometryUtils.merge(geometry, coneGeometry); + geometry.merge(coneGeometry); THREE.Mesh.call(this, geometry, material); - this.position = origin; + this.position.copy( origin ); this.setDirection(direction); }; ROS3D.Arrow.prototype.__proto__ = THREE.Mesh.prototype; diff --git a/src/models/Axes.js b/src/models/Axes.js index f8041ede..44d0f2d4 100644 --- a/src/models/Axes.js +++ b/src/models/Axes.js @@ -18,9 +18,12 @@ ROS3D.Axes = function(options) { var shaftRadius = options.shaftRadius || 0.008; var headRadius = options.headRadius || 0.023; var headLength = options.headLength || 0.1; + var scaleArg = options.scale || 1.0; THREE.Object3D.call(this); + this.scale.set(scaleArg,scaleArg,scaleArg); + // create the cylinders for the objects this.lineGeom = new THREE.CylinderGeometry(shaftRadius, shaftRadius, 1.0 - headLength); this.headGeom = new THREE.CylinderGeometry(0, headRadius, headLength); @@ -46,17 +49,17 @@ ROS3D.Axes = function(options) { // create the arrow var arrow = new THREE.Mesh(that.headGeom, material); - arrow.position = axis.clone(); + arrow.position.copy( axis ); arrow.position.multiplyScalar(0.95); - arrow.quaternion = rot; + arrow.quaternion.copy( rot ); arrow.updateMatrix(); that.add(arrow); // create the line var line = new THREE.Mesh(that.lineGeom, material); - line.position = axis.clone(); + line.position.copy( axis ); line.position.multiplyScalar(0.45); - line.quaternion = rot; + line.quaternion.copy( rot ); line.updateMatrix(); that.add(line); } diff --git a/src/models/MeshResource.js b/src/models/MeshResource.js index 42adada5..4d850cce 100644 --- a/src/models/MeshResource.js +++ b/src/models/MeshResource.js @@ -53,7 +53,7 @@ ROS3D.MeshResource = function(options) { // check for a scale factor in ColladaLoader2 if(loaderType === ROS3D.COLLADA_LOADER_2 && collada.dae.asset.unit) { var scale = collada.dae.asset.unit; - collada.scene.scale = new THREE.Vector3(scale, scale, scale); + collada.scene.scale.set(scale, scale, scale); } // add a texture to anything that is missing one diff --git a/src/models/TriangleList.js b/src/models/TriangleList.js index 4cd9f172..0622f9d5 100644 --- a/src/models/TriangleList.js +++ b/src/models/TriangleList.js @@ -61,7 +61,6 @@ ROS3D.TriangleList = function(options) { geometry.computeBoundingBox(); geometry.computeBoundingSphere(); - geometry.computeCentroids(); geometry.computeFaceNormals(); this.add(new THREE.Mesh(geometry, material)); diff --git a/src/sensors/Particles.js b/src/sensors/Particles.js index 5e13bbc4..8faf63eb 100644 --- a/src/sensors/Particles.js +++ b/src/sensors/Particles.js @@ -86,7 +86,7 @@ ROS3D.Particles = function(options) { transparent: true, }); - this.ps = new THREE.ParticleSystem( this.geom, this.shaderMaterial ); + this.ps = new THREE.PointCloud( this.geom, this.shaderMaterial ); this.sn = null; this.points = this.geom.vertices; diff --git a/src/urdf/Urdf.js b/src/urdf/Urdf.js index 7402794c..58293086 100644 --- a/src/urdf/Urdf.js +++ b/src/urdf/Urdf.js @@ -61,7 +61,7 @@ ROS3D.Urdf = function(options) { // check for a scale if(link.visuals[i].geometry.scale) { - mesh.scale = new THREE.Vector3( + mesh.scale.set( visual.geometry.scale.x, visual.geometry.scale.y, visual.geometry.scale.z @@ -88,7 +88,7 @@ ROS3D.Urdf = function(options) { switch (visual.geometry.type) { case ROSLIB.URDF_BOX: var dimension = visual.geometry.dimension; - var cube = new THREE.CubeGeometry(dimension.x, dimension.y, dimension.z); + var cube = new THREE.BoxGeometry(dimension.x, dimension.y, dimension.z); shapeMesh = new THREE.Mesh(cube, colorMaterial); break; case ROSLIB.URDF_CYLINDER: diff --git a/src/visualization/SceneNode.js b/src/visualization/SceneNode.js index b59541b4..a95548a6 100644 --- a/src/visualization/SceneNode.js +++ b/src/visualization/SceneNode.js @@ -9,7 +9,7 @@ * @constructor * @param options - object with following keys: * - * * tfClient - a handle to the TF client + * * tfClient (optional) - a handle to the TF client * * frameID - the frame ID this object belongs to * * pose (optional) - the pose associated with this object * * object - the THREE 3D object to be rendered @@ -17,7 +17,7 @@ ROS3D.SceneNode = function(options) { options = options || {}; var that = this; - this.tfClient = options.tfClient; + this.tfClient = options.tfClient || null; this.frameID = options.frameID; var object = options.object; this.pose = options.pose || new ROSLIB.Pose(); @@ -27,26 +27,21 @@ ROS3D.SceneNode = function(options) { this.visible = false; // add the model - this.add(object); - + if (object) { + this.add(object); + } // set the inital pose this.updatePose(this.pose); // save the TF handler so we can remove it later this.tfUpdate = function(msg) { - - // apply the transform - var tf = new ROSLIB.Transform(msg); - var poseTransformed = new ROSLIB.Pose(that.pose); - poseTransformed.applyTransform(tf); - - // update the world - that.updatePose(poseTransformed); - that.visible = true; + that.transformPose(msg); }; // listen for TF updates - this.tfClient.subscribe(this.frameID, this.tfUpdate); + if (this.tfClient) { + this.tfClient.subscribe(this.frameID, this.tfUpdate); + } }; ROS3D.SceneNode.prototype.__proto__ = THREE.Object3D.prototype; @@ -63,5 +58,22 @@ ROS3D.SceneNode.prototype.updatePose = function(pose) { }; ROS3D.SceneNode.prototype.unsubscribeTf = function() { - this.tfClient.unsubscribe(this.frameID, this.tfUpdate); + if (this.tfClient) { + this.tfClient.unsubscribe(this.frameID, this.tfUpdate); + } +}; + +/** + * Transform the pose of the associated model. + * @param transform - A ROS Transform like object which has a translation and orientation property. + */ +ROS3D.SceneNode.prototype.transformPose = function(transform) { + // apply the transform + var tf = new ROSLIB.Transform( transform ); + var poseTransformed = new ROSLIB.Pose(this.pose); + poseTransformed.applyTransform(tf); + + // update the world + this.updatePose(poseTransformed); + this.visible = true; }; diff --git a/src/visualization/Viewer.js b/src/visualization/Viewer.js index 4a7a67ef..4907fc59 100644 --- a/src/visualization/Viewer.js +++ b/src/visualization/Viewer.js @@ -22,6 +22,10 @@ ROS3D.Viewer = function(options) { options = options || {}; var divID = options.divID; + var canvas = (!!options.canvas && + options.canvas.nodeName.toLowerCase() === 'canvas') + ? options.canvas + : undefined; var width = options.width; var height = options.height; var background = options.background || '#111111'; @@ -35,10 +39,11 @@ ROS3D.Viewer = function(options) { y : 3, z : 3 }; - var cameraZoomSpeed = options.cameraZoomSpeed || 0.5; + var cameraZoomSpeed = options.cameraZoomSpeed || 2.5; // create the canvas to render to this.renderer = new THREE.WebGLRenderer({ + canvas: canvas, antialias : antialias, alpha: true }); @@ -53,9 +58,8 @@ ROS3D.Viewer = function(options) { // create the global camera this.camera = new THREE.PerspectiveCamera(40, width / height, near, far); - this.camera.position.x = cameraPosition.x; - this.camera.position.y = cameraPosition.y; - this.camera.position.z = cameraPosition.z; + this.camera.position.set( cameraPosition.x, cameraPosition.y, cameraPosition.z ); + // add controls to the camera this.cameraControls = new ROS3D.OrbitControls({ scene : this.scene, @@ -87,7 +91,11 @@ ROS3D.Viewer = function(options) { this.animationRequestId = undefined; // add the renderer to the page - document.getElementById(divID).appendChild(this.renderer.domElement); + if (divID) { + document.getElementById(divID).appendChild(this.renderer.domElement); + } else if (!canvas) { + throw new Error('No canvas nor HTML container provided for rendering.'); + } // begin the render loop this.start(); @@ -114,7 +122,7 @@ ROS3D.Viewer.prototype.draw = function(){ this.cameraControls.update(); // put light to the top-left of the camera - this.directionalLight.position = this.camera.localToWorld(new THREE.Vector3(-1, 1, 0)); + this.directionalLight.position.copy( this.camera.localToWorld(new THREE.Vector3(-1, 1, 0)) ); this.directionalLight.position.normalize(); // set the scene @@ -122,7 +130,7 @@ ROS3D.Viewer.prototype.draw = function(){ this.renderer.render(this.scene, this.camera); // render any mouseovers - this.highlighter.renderHighlight(this.renderer, this.scene, this.camera); + //this.highlighter.renderHighlight(this.renderer, this.scene, this.camera); // draw the frame this.animationRequestId = requestAnimationFrame(this.draw.bind(this)); diff --git a/src/visualization/interaction/Highlighter.js b/src/visualization/interaction/Highlighter.js index f6febc26..26566fd7 100644 --- a/src/visualization/interaction/Highlighter.js +++ b/src/visualization/interaction/Highlighter.js @@ -74,6 +74,10 @@ ROS3D.Highlighter.prototype.renderHighlight = function(renderer, scene, camera) var renderList = []; this.getWebglObjects(scene, this.hoverObjs, renderList); + if ( renderList.length === 0 ) { + return; + } + // define highlight material scene.overrideMaterial = new THREE.MeshBasicMaterial({ fog : false, diff --git a/src/visualization/interaction/MouseHandler.js b/src/visualization/interaction/MouseHandler.js index cac25ee3..f0d04f2b 100644 --- a/src/visualization/interaction/MouseHandler.js +++ b/src/visualization/interaction/MouseHandler.js @@ -21,7 +21,6 @@ ROS3D.MouseHandler = function(options) { this.fallbackTarget = options.fallbackTarget; this.lastTarget = this.fallbackTarget; this.dragging = false; - this.projector = new THREE.Projector(); // listen to DOM events var eventNames = [ 'contextmenu', 'click', 'dblclick', 'mouseout', 'mousedown', 'mouseup', @@ -69,7 +68,7 @@ ROS3D.MouseHandler.prototype.processDomEvent = function(domEvent) { var deviceX = left / target.clientWidth * 2 - 1; var deviceY = -top / target.clientHeight * 2 + 1; var vector = new THREE.Vector3(deviceX, deviceY, 0.5); - this.projector.unprojectVector(vector, this.camera); + vector.unproject(this.camera); // THREE r69 // use the THREE raycaster var mouseRaycaster = new THREE.Raycaster(this.camera.position.clone(), vector.sub( this.camera.position).normalize()); diff --git a/src/visualization/interaction/OrbitControls.js b/src/visualization/interaction/OrbitControls.js index 20d02196..8fbf29d0 100644 --- a/src/visualization/interaction/OrbitControls.js +++ b/src/visualization/interaction/OrbitControls.js @@ -99,14 +99,15 @@ ROS3D.OrbitControls = function(options) { moveStartIntersection = intersectViewPlane(event3D.mouseRay, moveStartCenter, moveStartNormal); + this.showAxes(); break; case 2: state = STATE.ZOOM; zoomStart.set(event.clientX, event.clientY); + this.showAxes(); break; } - this.showAxes(); } /** @@ -125,7 +126,7 @@ ROS3D.OrbitControls = function(options) { that.rotateUp(2 * Math.PI * rotateDelta.y / pixelsPerRound * that.userRotateSpeed); rotateStart.copy(rotateEnd); - this.showAxes(); + } else if (state === STATE.ZOOM) { zoomEnd.set(event.clientX, event.clientY); zoomDelta.subVectors(zoomEnd, zoomStart); @@ -217,9 +218,9 @@ ROS3D.OrbitControls = function(options) { delta = -event.detail; } if (delta > 0) { - that.zoomIn(); - } else { that.zoomOut(); + } else { + that.zoomIn(); } this.showAxes(); @@ -366,6 +367,7 @@ ROS3D.OrbitControls.prototype.showAxes = function() { this.axes.traverse(function(obj) { obj.visible = true; }); + if (this.hideTimeout) { clearTimeout(this.hideTimeout); } @@ -374,7 +376,7 @@ ROS3D.OrbitControls.prototype.showAxes = function() { obj.visible = false; }); that.hideTimeout = false; - }, 1000); + }, 300); }; /** @@ -475,9 +477,11 @@ ROS3D.OrbitControls.prototype.update = function() { phi = Math.max(eps, Math.min(Math.PI - eps, phi)); var radius = offset.length(); - offset.y = radius * Math.sin(phi) * Math.sin(theta); - offset.z = radius * Math.cos(phi); - offset.x = radius * Math.sin(phi) * Math.cos(theta); + offset.set( + radius * Math.sin(phi) * Math.cos(theta), + radius * Math.sin(phi) * Math.sin(theta), + radius * Math.cos(phi) + ); offset.multiplyScalar(this.scale); position.copy(this.center).add(offset); @@ -485,8 +489,8 @@ ROS3D.OrbitControls.prototype.update = function() { this.camera.lookAt(this.center); radius = offset.length(); - this.axes.position = this.center.clone(); - this.axes.scale.x = this.axes.scale.y = this.axes.scale.z = radius * 0.05; + this.axes.position.copy( this.center ); + this.axes.scale.set( radius * 0.05, radius * 0.05, radius *0.05); this.axes.updateMatrixWorld(true); this.thetaDelta = 0;