From d3470f49dc6212c98af1654d677eab42d343b6d3 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 8 Sep 2021 21:44:44 +0200 Subject: [PATCH] Export OcTree (#417) * Update exprts * (transpiler) read revision correctly * Fix typo * Make transpiler more robust More robust to whitespace; fix dot bug (dots are any character in regex); Arguments are optional to constructors * OccupancyMap* -> OcTree* Rename as to similar to OccupancyGrid*; Including seperation of all classes to their own file, needed by transpiler; Remove all seperate class property definitions; not supported by transpiler Co-authored-by: Peter Sari --- es6-support/index.js | 3 + es6-transpiler.js | 36 +- src/navigation/ColorOcTree.js | 25 ++ src/navigation/OcTree.js | 98 +++++ .../{OccupancyMap.js => OcTreeBase.js} | 413 ++++++------------ src/navigation/OcTreeBaseNode.js | 38 ++ ...{OccupancyMapClient.js => OcTreeClient.js} | 20 +- 7 files changed, 317 insertions(+), 316 deletions(-) create mode 100644 src/navigation/ColorOcTree.js create mode 100644 src/navigation/OcTree.js rename src/navigation/{OccupancyMap.js => OcTreeBase.js} (63%) create mode 100644 src/navigation/OcTreeBaseNode.js rename src/navigation/{OccupancyMapClient.js => OcTreeClient.js} (89%) diff --git a/es6-support/index.js b/es6-support/index.js index 67534f65b..edc9160f2 100644 --- a/es6-support/index.js +++ b/es6-support/index.js @@ -23,6 +23,9 @@ export * from './models/TriangleList' export * from './navigation/OccupancyGrid' export * from './navigation/OccupancyGridClient' +export * from './navigation/OcTree' +export * from './navigation/ColorOcTree' +export * from './navigation/OcTreeClient' export * from './navigation/Odometry' export * from './navigation/Path' export * from './navigation/Point' diff --git a/es6-transpiler.js b/es6-transpiler.js index 6a9ef4042..3332443fd 100644 --- a/es6-transpiler.js +++ b/es6-transpiler.js @@ -206,15 +206,18 @@ const transpile = { // Replace initial ROS3D assignment initialROS3DAssignment: [ // from - /var ROS3D = ROS3D \|\| \{\n REVISION \: '0.18.0'\n\};/m, + /var ROS3D = ROS3D \|\| \{\n REVISION \: '([0-9]+\.[0-9]+\.[0-9]+)'\n\};/m, // to - `export var REVISION = '0.18.0';`, + (match, $1) => { + const revision = $1 + return `export var REVISION = '${revision}';` + }, ], // Replace mutations with exported properties - exportedProperites: (filepath) => [ + exportedProperties: (filepath) => [ // from: // ROS3D.MARKER_ARROW = 0; - /\nROS3D\.(.*)\s+?=\s+?(.*)/g, + /\nROS3D\.(.*)\s*=\s*(.*)/g, // to: // export var MARKER_ARROW = 0; (match, $1, $2) => { @@ -268,7 +271,7 @@ const transpile = { buildInheritanceIndexViaProto: [ // from: // ROS3D.PoseWithCovariance.prototype.__proto__ = THREE.Object3D.prototype; - /ROS3D.(\w+).prototype.__proto__ = (.*).prototype;[\r\n]?/g, + /ROS3D\.(\w+)\.prototype\.__proto__ = (.*)\.prototype;[\r\n]?/g, // to: // set PoseWithCovariance to subclass from THREE.Object3D in inheritance index (match, $1, $2) => { @@ -282,7 +285,7 @@ const transpile = { buildInheritanceIndexViaObjectAssign: [ // from: // Object.assign(InteractiveMarker.prototype, THREE.EventDispatcher.prototype); - /Object.assign\((\w+).prototype, (.*).prototype\);/g, + /Object\.assign\((\w+)\.prototype,\s*(.*)\.prototype\);/g, // to: // set InteractiveMarker to subclass from THREE.EventDispatcher in inheritance index (match, $1, $2) => { @@ -295,8 +298,8 @@ const transpile = { // Refactor methods methods: [ // from: - // ROS3D.Arrow2.prototype.dispose = function() { ... }; - /ROS3D.(\w+).prototype.(\w+) = function|function\s+?(\w+)/g, + // ROS3D.Arrow2.prototype.dispose = function () { ... }; + /ROS3D\.(\w+)\.prototype\.(\w+)\s*=\s*function\s*|function\s+(\w+)/g, // to: // dispose() { ... }; (match, $1, $2, $3) => { @@ -324,10 +327,10 @@ const transpile = { constructors: (filepath, state = { foundConstructor: false }) => [ // from: // ROS3D.Arrow2 = function(options) { ... }; - /ROS3D.(\w+)\s*=\s*function/g, + /ROS3D\.(\w+)\s*=\s*function\s*\((.*)\)/g, // to: // constructor(options) { ... }; - (match, $1) => { + (match, $1, $2) => { const isClass = isFileClass(filepath, $1) // if (isClass1 !== isClass2) { // logWarning('class mismatch', { @@ -339,13 +342,14 @@ const transpile = { // } if (isClass) { if (state.foundConstructor) { - logError('already found a constructor in this file...', { match, $1 }) + logError('Already found a constructor in this file...', { match, $1, $2 }) } state.foundConstructor = true if (debugRules.logConstructors) { - logInfo('found constructor', { match, $1 }) + logInfo('Found constructor', { match, $1, $2 }) } - return 'constructor' + const arguments = $2 + return `constructor(${arguments})` } else { return match } @@ -434,7 +438,7 @@ const transpile = { // } // /.*(\*\/).*|[\r\n]+$(?:[\r\n]+$)+((?![\r\n]+))|.*/gm, // /(\/\*\*(?:$|[.\r\n])*\*\/(?:$|[\s\r\n])*constructor\(.*)|[\r\n]+$(?:[\r\n]+$)+((?![\r\n]+))|.*/gm, - /((?:\/\*\*(?:(?:\*[^/]|[^*])+?)\*\/)(?:[\s\r\n])*constructor\(.*)|$(?:[\r\n]$)*((?![\r\n]))|.+/gm, + /((?:\/\*\*(?:(?:\*[^/]|[^*])+?)\*\/)(?:[\s\r\n])*constructor\s*\(.*)|$(?:[\r\n]$)*((?![\r\n]))|.+/gm, // to: // export class Arrow2 extends THREE.ArrowHelper { // constructor(options) { @@ -671,7 +675,7 @@ const transpileToEs6 = function (content, filepath, grunt) { const transpileConstructors = transpile.constructors(filepath) const transpileSuperCalls = transpile.superCalls(filepath) const transpileClasses = transpile.classes(filepath) - const transpileExportedProperites= transpile.exportedProperites(filepath) + const transpileExportedProperties= transpile.exportedProperties(filepath) return transpiled .replace(...transpileInternalDependencies) @@ -682,7 +686,7 @@ const transpileToEs6 = function (content, filepath, grunt) { .replace(...transpileConstructors) .replace(...transpileSuperCalls) .replace(...transpileClasses) - .replace(...transpileExportedProperites) + .replace(...transpileExportedProperties) } // Injects es6 imports based on dependency and export diff --git a/src/navigation/ColorOcTree.js b/src/navigation/ColorOcTree.js new file mode 100644 index 000000000..a212f71ef --- /dev/null +++ b/src/navigation/ColorOcTree.js @@ -0,0 +1,25 @@ +/** + * @author Peter Sari - sari@photoneo.com + */ + +ROS3D.ColorOcTree = function(options) { + ROS3D.OcTree.call(this, options); + this.useOwnColor = (typeof options.palette !== 'undefined') && options.colorMode === ROS3D.OcTreeColorMode.COLOR; +}; + +ROS3D.ColorOcTree.prototype.__proto__ = ROS3D.OcTree.prototype; + +ROS3D.ColorOcTree.prototype._readNodeData = function (dataStream, node) { + node.value = dataStream.readFloat32(); // occupancy + node.color = { + r: dataStream.readUint8(), // red + g: dataStream.readUint8(), // green + b: dataStream.readUint8(), // blue + }; + +}; + +ROS3D.ColorOcTree.prototype._obtainColor = function (node) { + if (!this.useOwnColor) { return ROS3D.OcTree.prototype._obtainColor.call(this, node); } + return node.color; +}; diff --git a/src/navigation/OcTree.js b/src/navigation/OcTree.js new file mode 100644 index 000000000..ef26e434b --- /dev/null +++ b/src/navigation/OcTree.js @@ -0,0 +1,98 @@ +/** + * @author Peter Sari - sari@photoneo.com + */ + +/** + * Toggles voxel visibility + * + * * `occupied` - only voxels that are above or equal to the occupation threshold are shown + * * `free` - only voxels that are below the occupation threshold are shown + * * `all` - all allocated voxels are shown + */ +ROS3D.OcTreeVoxelRenderMode = { + OCCUPIED: 'occupied', + FREE: 'free', + ALL: 'all', +}; + +/** + * Coloring modes for each voxel + * + * * 'solid' - voxels will have a single solid color set by the tree globally + * * 'occupancy' - voxels are false colored by their occupancy value. Fall back for `solid` if not available. + * * 'color' - voxels will colorized by their + */ +ROS3D.OcTreeColorMode = { + SOLID: 'solid', + OCCUPANCY: 'occupancy', + COLOR: 'color' +}; + +// ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- + +/** + * Specilaization of BaseOcTree + * + * @constructor + * @param options - object with following keys: + * * inherited from BaseOctree + * * occupancyThreshold (optional) - threshold value that separates occupied and free voxels from each other. (Default: 0) + * * colorMode (optional) - Coloring mode @see ROS3D.OcTreeColorMode. + * * palette (optional) - Palette used for false-coloring (default: predefined palette) + * * paletteSclae (optional) - Scale of palette to represent a wider range of values (default: 1.) + */ + +ROS3D.OcTree = function(options) { + ROS3D.OcTreeBase.call(this, options); + + this._defaultOccupiedValue = 1.; + this._defaultFreeValue = -1.; + + this.occupancyThreshold = (typeof options.occupancyThreshold !== 'undefined') ? options.occupancyThreshold : 0.0000001; + + this.useFlatColoring = (typeof options.colorMode !== 'undefined') && options.colorMode === ROS3D.OcTreeColorMode.SOLID; + + this.palette = (typeof options.palette !== 'undefined') ? options.palette.map(color => new THREE.Color(color)) : + [ + { r: 0, g: 0, b: 128, }, // dark blue (low) + { r: 0, g: 255, b: 0, }, // green + { r: 255, g: 255, b: 0, }, // yellow (mid) + { r: 255, g: 128, b: 0, }, // orange + { r: 255, g: 0, b: 0, } // red (high) + ]; + + this.paletteScale = (typeof options.paletteScale !== 'undefined') ? options.paletteScale : 1.; +}; + +ROS3D.OcTree.prototype.__proto__ = ROS3D.OcTreeBase.prototype; + +ROS3D.OcTree.prototype._readNodeData = function (dataStream, node) { + node.value = dataStream.readFloat32(); +}; + +ROS3D.OcTree.prototype._obtainColor = function (node) { + if (this.useFlatColoring) { + return this.color; + } + + // Use a simple sigmoid curve to fit values from -inf..inf into 0..1 range + const value = 1. / (1. + Math.exp(-node.value * this.paletteScale)) * this.palette.length; // Normalize + + const intVal = Math.trunc(value); + const fracVal = value - intVal; + + if (intVal < 0) { return this.palette[0]; } + if (intVal >= this.palette.length - 1) { return this.palette[this.palette.length - 1]; } + + // Simple lerp + return { + r: fracVal * this.palette[intVal].r + (1. - fracVal) * this.palette[intVal + 1].r, + g: fracVal * this.palette[intVal].g + (1. - fracVal) * this.palette[intVal + 1].g, + b: fracVal * this.palette[intVal].b + (1. - fracVal) * this.palette[intVal + 1].b, + }; + +}; + +ROS3D.OcTree.prototype._checkOccupied = function (node) { + return node.value >= this.occupancyThreshold; +}; diff --git a/src/navigation/OccupancyMap.js b/src/navigation/OcTreeBase.js similarity index 63% rename from src/navigation/OccupancyMap.js rename to src/navigation/OcTreeBase.js index 34c1800e2..d81589302 100644 --- a/src/navigation/OccupancyMap.js +++ b/src/navigation/OcTreeBase.js @@ -45,70 +45,6 @@ function InStream(data, isLittleEndian) { // ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- -/** - * Base node type that represents one voxel as a node of the tree - */ - -ROS3D.OcTreeBaseNode = function () { - this._children = [null, null, null, null, null, null, null, null]; - this.value = null; -}; - -ROS3D.OcTreeBaseNode.prototype.createChildNodeAt = function (newNode, index) { - this._children[index % 8] = newNode; -}; - -ROS3D.OcTreeBaseNode.prototype.hasChildAt = function (index) { - return this._children[index % 8] !== null; -}; - -ROS3D.OcTreeBaseNode.prototype.getChildAt = function (index) { - return this._children[index % 8]; -}; - -ROS3D.OcTreeBaseNode.prototype.isLeafNode = function () { - for (let i = 0; i < 8; ++i) { - if (this._children[i] !== null) { return false; } - } - return true; -}; - -ROS3D.OcTreeBaseNode.prototype.hasChildren = function () { - for (let i = 0; i < 8; ++i) { - if (this._children[i] !== null) { return true; } - } - return false; -}; - - -// ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- - -/** - * Toggles voxel visibility - * - * * `occupied` - only voxels that are above or equal to the occupation threshold are shown - * * `free` - only voxels that are below the occupation threshold are shown - * * `all` - all allocated voxels are shown - */ -ROS3D.OcTreeVoxelRenderMode = { - OCCUPIED: 'occupied', - FREE: 'free', - ALL: 'all', -}; - -/** - * Coloring modes for each voxel - * - * * 'solid' - voxels will have a single solid color set by the tree globally - * * 'occupancy' - voxels are false colored by their occupancy value. Fall back for `solid` if not available. - * * 'color' - voxels will colorized by their - */ -ROS3D.OcTreeColorMode = { - SOLID: 'solid', - OCCUPANCY: 'occupancy', - COLOR: 'color' -}; - /** * Represensta a BaseTree that can be build from ros message and create a THREE node from it. * Due a tree can be represented different ways in a message, this class is also a base class to @@ -121,7 +57,7 @@ ROS3D.OcTreeColorMode = { * * color - color of the visualized map (if solid coloring option was set) * * voxelRenderMode - toggle between rendering modes @see ROS3D.OcTreeVoxelRenderMode */ -ROS3D.OcTreeBase = function (options) { +ROS3D.OcTreeBase = function(options) { this.resolution = (typeof options.resolution !== 'undefined') ? options.resolution : 1.; this.color = new THREE.Color((typeof options.color !== 'undefined') ? options.color : 'green'); @@ -133,6 +69,122 @@ ROS3D.OcTreeBase = function (options) { this._treeDepth = 16; this._treeMaxKeyVal = 32768; + this._BINARY_UNALLOCATED = 0b00; + this._BINARY_LEAF_FREE = 0b01; + this._BINARY_LEAF_OCCUPIED = 0b10; + this._BINARY_HAS_CHILDREN = 0b11; + + this._BINARY_CHILD_BUILD_TABLE = {}; + + this._BINARY_CHILD_BUILD_TABLE[this._BINARY_LEAF_FREE] = function (child) { + child.value = this._defaultFreeValue; + }; + + this._BINARY_CHILD_BUILD_TABLE[this._BINARY_LEAF_OCCUPIED] = function (child) { + child.value = this._defaultOccupiedValue; + }; + + this._BINARY_CHILD_BUILD_TABLE[this._BINARY_HAS_CHILDREN] = function (child) { + child.value = null; + }; + + /** + * Table which we are building the geometry data from. + */ + this._FACES = [ + { // 0. left (x=0) + normal: [-1, 0, 0,], + vertices: [ + [0, 1, 0], + [0, 0, 0], + [0, 1, 1], + [0, 0, 1], + ], + childIndex: [ + 0b001, + 0b011, + 0b101, + 0b111 + ] + }, + { // 1. right (x=1) + normal: [1, 0, 0,], + vertices: [ + [1, 1, 1], + [1, 0, 1], + [1, 1, 0], + [1, 0, 0], + ], + + childIndex: [ + 0b000, + 0b010, + 0b100, + 0b110 + ] + }, + { // 2. bottom (y=0) + normal: [0, -1, 0,], + vertices: [ + [1, 0, 1], + [0, 0, 1], + [1, 0, 0], + [0, 0, 0], + ], + childIndex: [ + 0b010, + 0b011, + 0b110, + 0b111 + ] + }, + { // 3. top (y=1) + normal: [0, 1, 0,], + vertices: [ + [0, 1, 1], + [1, 1, 1], + [0, 1, 0], + [1, 1, 0], + ], + childIndex: [ + 0b000, + 0b001, + 0b100, + 0b101 + ] + }, + { // 4. back (z=0) + normal: [0, 0, -1,], + vertices: [ + [1, 0, 0], + [0, 0, 0], + [1, 1, 0], + [0, 1, 0], + ], + childIndex: [ + 0b100, + 0b101, + 0b110, + 0b111 + ] + }, + { // 5.front (z=1) + normal: [0, 0, 1,], + vertices: [ + [0, 0, 1], + [1, 0, 1], + [0, 1, 1], + [1, 1, 1], + ], + childIndex: [ + 0b000, + 0b001, + 0b010, + 0b011 + ] + }, + ]; + // Table of voxel size for each level of the tree this.nodeSizeTable = new Array(this._treeDepth); let _val = this.resolution; @@ -214,15 +266,6 @@ ROS3D.OcTreeBase.prototype._adjustKeyAtDepth = function (key, depth) { return key.map(keyVal => (((keyVal - this._treeMaxKeyVal) >> diff) << diff) + (1 << (diff - 1)) + this._treeMaxKeyVal); }; -/** - * - */ -ROS3D.OcTreeBase.prototype._BINARY_UNALLOCATED = 0b00; -ROS3D.OcTreeBase.prototype._BINARY_LEAF_FREE = 0b01; -ROS3D.OcTreeBase.prototype._BINARY_LEAF_OCCUPIED = 0b10; -ROS3D.OcTreeBase.prototype._BINARY_HAS_CHILDREN = 0b11; - - ROS3D.OcTreeBase.prototype._newNode = function () { return new ROS3D.OcTreeBaseNode(); }; /* @@ -270,27 +313,11 @@ ROS3D.OcTreeBase.prototype.readBinary = function (data) { }; -ROS3D.OcTreeBase.prototype._BINARY_CHILD_BUILD_TABLE = {}; - -ROS3D.OcTreeBase.prototype._BINARY_CHILD_BUILD_TABLE[ROS3D.OcTreeBase.prototype._BINARY_LEAF_FREE] = function (child) { - child.value = this._defaultFreeValue; -}; - -ROS3D.OcTreeBase.prototype._BINARY_CHILD_BUILD_TABLE[ROS3D.OcTreeBase.prototype._BINARY_LEAF_OCCUPIED] = function (child) { - child.value = this._defaultOccupiedValue; -}; - -ROS3D.OcTreeBase.prototype._BINARY_CHILD_BUILD_TABLE[ROS3D.OcTreeBase.prototype._BINARY_HAS_CHILDREN] = function (child) { - child.value = null; -}; - - /** * Reads a full tree (with node data) from a message. * A pacjet starts with the node data, followed by the allocation map of their children. * Each type of tree has different data structure @see ROS3DJS.OcTreeBase._readNodeData */ - ROS3D.OcTreeBase.prototype.read = function (data) { if (this._rootNode !== null) { delete this._rootNode; @@ -407,13 +434,13 @@ ROS3D.OcTreeBase.prototype._buildFaces = function () { _insertFace: function (face, pos, size, color) { const indexCount = this.vertices.length / 3; - for (let vertex of face.vertices) { + face.vertices.forEach(function(vertex) { this.vertices.push( pos[0] + vertex[0] * size, pos[1] + vertex[1] * size, pos[2] + vertex[2] * size ); - }; + }); const colorArr = [color.r, color.g, color.b]; @@ -433,8 +460,7 @@ ROS3D.OcTreeBase.prototype._buildFaces = function () { while (stack.length !== 0) { const node = stack.pop(); if (node.hasChildren()) { - for (let childIndex of face.childIndex) { - + face.childIndex.forEach(function(childIndex) { if (node.hasChildAt(childIndex)) { const child = node.getChildAt(childIndex); @@ -447,7 +473,7 @@ ROS3D.OcTreeBase.prototype._buildFaces = function () { else { return true; } - } + }) } } return false; @@ -469,9 +495,7 @@ ROS3D.OcTreeBase.prototype._buildFaces = function () { // Hide occuped voxels if set. if (isOccupied && this.voxelRenderMode === ROS3D.OcTreeVoxelRenderMode.FREE) { return; } - for (let face of this.FACES) - // let face = this.FACES[1] ; - { + this._FACES.forEach(function(face) { // Add geometry where there is no neighbor voxel const neighborKey = [ key[0] + face.normal[0] * diff * diff, @@ -492,7 +516,7 @@ ROS3D.OcTreeBase.prototype._buildFaces = function () { } } - } + }) }); @@ -505,194 +529,3 @@ ROS3D.OcTreeBase.prototype._buildFaces = function () { }; }; - -/** - * Table which we are building the geometry data from. - */ -ROS3D.OcTreeBase.prototype.FACES = [ - { // 0. left (x=0) - normal: [-1, 0, 0,], - vertices: [ - [0, 1, 0], - [0, 0, 0], - [0, 1, 1], - [0, 0, 1], - ], - childIndex: [ - 0b001, - 0b011, - 0b101, - 0b111 - ] - }, - { // 1. right (x=1) - normal: [1, 0, 0,], - vertices: [ - [1, 1, 1], - [1, 0, 1], - [1, 1, 0], - [1, 0, 0], - ], - - childIndex: [ - 0b000, - 0b010, - 0b100, - 0b110 - ] - }, - { // 2. bottom (y=0) - normal: [0, -1, 0,], - vertices: [ - [1, 0, 1], - [0, 0, 1], - [1, 0, 0], - [0, 0, 0], - ], - childIndex: [ - 0b010, - 0b011, - 0b110, - 0b111 - ] - }, - { // 3. top (y=1) - normal: [0, 1, 0,], - vertices: [ - [0, 1, 1], - [1, 1, 1], - [0, 1, 0], - [1, 1, 0], - ], - childIndex: [ - 0b000, - 0b001, - 0b100, - 0b101 - ] - }, - { // 4. back (z=0) - normal: [0, 0, -1,], - vertices: [ - [1, 0, 0], - [0, 0, 0], - [1, 1, 0], - [0, 1, 0], - ], - childIndex: [ - 0b100, - 0b101, - 0b110, - 0b111 - ] - }, - { // 5.front (z=1) - normal: [0, 0, 1,], - vertices: [ - [0, 0, 1], - [1, 0, 1], - [0, 1, 1], - [1, 1, 1], - ], - childIndex: [ - 0b000, - 0b001, - 0b010, - 0b011 - ] - }, -]; - -// ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- -/** - * Specilaization of BaseOcTree - * - * @constructor - * @param options - object with following keys: - * * inherited from BaseOctree - * * occupancyThreshold (optional) - threshold value that separates occupied and free voxels from each other. (Default: 0) - * * colorMode (optional) - Coloring mode @see ROS3D.OcTreeColorMode. - * * palette (optional) - Palette used for false-coloring (default: predefined palette) - * * paletteSclae (optional) - Scale of palette to represent a wider range of values (default: 1.) - */ - -ROS3D.OcTree = function (options) { - ROS3D.OcTreeBase.call(this, options); - - this._defaultOccupiedValue = 1.; - this._defaultFreeValue = -1.; - - this.occupancyThreshold = (typeof options.occupancyThreshold !== 'undefined') ? options.occupancyThreshold : 0.0000001; - - this.useFlatColoring = (typeof options.colorMode !== 'undefined') && options.colorMode === ROS3D.OcTreeColorMode.SOLID; - - this.palette = (typeof options.palette !== 'undefined') ? options.palette.map(color => new THREE.Color(color)) : - [ - { r: 0, g: 0, b: 128, }, // dark blue (low) - { r: 0, g: 255, b: 0, }, // green - { r: 255, g: 255, b: 0, }, // yellow (mid) - { r: 255, g: 128, b: 0, }, // orange - { r: 255, g: 0, b: 0, } // red (high) - ]; - - this.paletteScale = (typeof options.paletteScale !== 'undefined') ? options.paletteScale : 1.; -}; - -ROS3D.OcTree.prototype = Object.create(ROS3D.OcTreeBase.prototype); - -ROS3D.OcTree.prototype._readNodeData = function (dataStream, node) { - node.value = dataStream.readFloat32(); -}; - -ROS3D.OcTree.prototype._obtainColor = function (node) { - if (this.useFlatColoring) { - return this.color; - } - - // Use a simple sigmoid curve to fit values from -inf..inf into 0..1 range - const value = 1. / (1. + Math.exp(-node.value * this.paletteScale)) * this.palette.length; // Normalize - - const intVal = Math.trunc(value); - const fracVal = value - intVal; - - if (intVal < 0) { return this.palette[0]; } - if (intVal >= this.palette.length - 1) { return this.palette[this.palette.length - 1]; } - - // Simple lerp - return { - r: fracVal * this.palette[intVal].r + (1. - fracVal) * this.palette[intVal + 1].r, - g: fracVal * this.palette[intVal].g + (1. - fracVal) * this.palette[intVal + 1].g, - b: fracVal * this.palette[intVal].b + (1. - fracVal) * this.palette[intVal + 1].b, - }; - -}; - -ROS3D.OcTree.prototype._checkOccupied = function (node) { - return node.value >= this.occupancyThreshold; -}; - -ROS3D.ColorOcTree = function (options) { - ROS3D.OcTree.call(this, options); - this.useOwnColor = (typeof options.palette !== 'undefined') && options.colorMode === ROS3D.OcTreeColorMode.COLOR; -}; - -ROS3D.ColorOcTree.prototype = Object.create(ROS3D.OcTree.prototype); - -ROS3D.ColorOcTree.prototype._readNodeData = function (dataStream, node) { - node.value = dataStream.readFloat32(); // occupancy - node.color = { - r: dataStream.readUint8(), // red - g: dataStream.readUint8(), // green - b: dataStream.readUint8(), // blue - }; - -}; - -ROS3D.ColorOcTree.prototype._obtainColor = function (node) { - if (!this.useOwnColor) { return ROS3D.OcTree.prototype._obtainColor.call(this, node); } - return node.color; -}; - -ROS3D.OcTreeBase.prototype._checkOccupied = function (node) { - return node.value < this.freeThreshold; -}; diff --git a/src/navigation/OcTreeBaseNode.js b/src/navigation/OcTreeBaseNode.js new file mode 100644 index 000000000..6f6e364dc --- /dev/null +++ b/src/navigation/OcTreeBaseNode.js @@ -0,0 +1,38 @@ +/** + * @author Peter Sari - sari@photoneo.com + */ + +/** + * Base node type that represents one voxel as a node of the tree + */ + +ROS3D.OcTreeBaseNode = function () { + this._children = [null, null, null, null, null, null, null, null]; + this.value = null; +}; + +ROS3D.OcTreeBaseNode.prototype.createChildNodeAt = function (newNode, index) { + this._children[index % 8] = newNode; +}; + +ROS3D.OcTreeBaseNode.prototype.hasChildAt = function (index) { + return this._children[index % 8] !== null; +}; + +ROS3D.OcTreeBaseNode.prototype.getChildAt = function (index) { + return this._children[index % 8]; +}; + +ROS3D.OcTreeBaseNode.prototype.isLeafNode = function () { + for (let i = 0; i < 8; ++i) { + if (this._children[i] !== null) { return false; } + } + return true; +}; + +ROS3D.OcTreeBaseNode.prototype.hasChildren = function () { + for (let i = 0; i < 8; ++i) { + if (this._children[i] !== null) { return true; } + } + return false; +}; diff --git a/src/navigation/OccupancyMapClient.js b/src/navigation/OcTreeClient.js similarity index 89% rename from src/navigation/OccupancyMapClient.js rename to src/navigation/OcTreeClient.js index 7ba72621b..6f0fff3c3 100644 --- a/src/navigation/OccupancyMapClient.js +++ b/src/navigation/OcTreeClient.js @@ -3,7 +3,7 @@ */ /** - * An occupancy map client that listens to a given OcTree topic. + * An OcTree client that listens to a given OcTree topic. * * Emits the following events: * @@ -28,7 +28,7 @@ * */ -ROS3D.OccupancyMapClient = function (options) { +ROS3D.OcTreeClient = function(options) { EventEmitter2.call(this); options = options || {}; this.ros = options.ros; @@ -58,28 +58,28 @@ ROS3D.OccupancyMapClient = function (options) { this.subscribe(); }; -ROS3D.OccupancyMapClient.prototype.unsubscribe = function () { +ROS3D.OcTreeClient.prototype.__proto__ = EventEmitter2.prototype; + +ROS3D.OcTreeClient.prototype.unsubscribe = function () { if (this.rosTopic) { this.rosTopic.unsubscribe(); } }; -ROS3D.OccupancyMapClient.prototype._MESSAGE_TYPE = 'octomap_msgs/Octomap'; - -ROS3D.OccupancyMapClient.prototype.subscribe = function () { +ROS3D.OcTreeClient.prototype.subscribe = function () { this.unsubscribe(); // subscribe to the topic this.rosTopic = new ROSLIB.Topic({ ros: this.ros, name: this.topicName, - messageType: this._MESSAGE_TYPE, + messageType: 'octomap_msgs/Octomap', queue_length: 1, compression: this.compression }); this.rosTopic.subscribe(this.processMessage.bind(this)); }; -ROS3D.OccupancyMapClient.prototype.processMessage = function (message) { +ROS3D.OcTreeClient.prototype.processMessage = function (message) { // check for an old map if (this.currentMap) { if (this.currentMap.tfClient) { @@ -96,7 +96,7 @@ ROS3D.OccupancyMapClient.prototype.processMessage = function (message) { }; -ROS3D.OccupancyMapClient.prototype._loadOcTree = function (message) { +ROS3D.OcTreeClient.prototype._loadOcTree = function (message) { return new Promise( function (resolve, reject) { @@ -143,7 +143,7 @@ ROS3D.OccupancyMapClient.prototype._loadOcTree = function (message) { }; -ROS3D.OccupancyMapClient.prototype._processMessagePrivate = function (message) { +ROS3D.OcTreeClient.prototype._processMessagePrivate = function (message) { let promise = this._loadOcTree(message); promise.then(