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 <[email protected]>

Author: MatthijsBurgh

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'

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

src/navigation/ColorOcTree.js

@@ -0,0 +1,25 @@
+/**
+ * @author Peter Sari - [email protected]
+ */
+
+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;
+};

src/navigation/OcTree.js

@@ -0,0 +1,98 @@
+/**
+ * @author Peter Sari - [email protected]
+ */
+
+/**
+ * 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;
+};