From a62a3a36b3a3b63fa6134fe3926c8131dbaa7d94 Mon Sep 17 00:00:00 2001
From: J-Rojas <jrojas@redlinesolutions.co>
Date: Fri, 18 Sep 2020 10:38:59 -0700
Subject: [PATCH] Update occupancy grid rendering (#339)

* Use raw probabilities values from the occupancy grid as part of the color scale instead of encoding them as only 3 discrete values:
0 will be rendered as black, 100 as full color.

Do not use linear filtering, instead use nearest filter so the texture appears as a grid.

* Do not duplicate SceneNode when multiple messages are delivered.
---
 src/navigation/OccupancyGrid.js       | 15 ++++-----------
 src/navigation/OccupancyGridClient.js | 23 ++++++++++++++---------
 2 files changed, 18 insertions(+), 20 deletions(-)

diff --git a/src/navigation/OccupancyGrid.js b/src/navigation/OccupancyGrid.js
index 683db7ac..442b98dd 100644
--- a/src/navigation/OccupancyGrid.js
+++ b/src/navigation/OccupancyGrid.js
@@ -31,15 +31,8 @@ ROS3D.OccupancyGrid = function(options) {
       var mapI = col + ((height - row - 1) * width);
       // determine the value
       var data = message.data[mapI];
-      var val;
-      if (data === 100) {
-        val = 0;
-      } else if (data === 0) {
-        val = 255;
-      } else {
-        val = 127;
-      }
-
+      var val = data * 255 / 100;
+      
       // determine the index into the image data array
       var i = (col + (row * width)) * 3;
       // r
@@ -53,8 +46,8 @@ ROS3D.OccupancyGrid = function(options) {
 
   var texture = new THREE.DataTexture(imageData, width, height, THREE.RGBFormat);
   texture.flipY = true;
-  texture.minFilter = THREE.LinearFilter;
-  texture.magFilter = THREE.LinearFilter;
+  texture.minFilter = THREE.NearestFilter;
+  texture.magFilter = THREE.NearestFilter;
   texture.needsUpdate = true;
 
   var material = new THREE.MeshBasicMaterial({
diff --git a/src/navigation/OccupancyGridClient.js b/src/navigation/OccupancyGridClient.js
index 7b708449..9a07099f 100644
--- a/src/navigation/OccupancyGridClient.js
+++ b/src/navigation/OccupancyGridClient.js
@@ -61,6 +61,7 @@ ROS3D.OccupancyGridClient.prototype.subscribe = function(){
     queue_length : 1,
     compression : this.compression
   });
+  this.sceneNode = null;
   this.rosTopic.subscribe(this.processMessage.bind(this));
 };
 
@@ -72,7 +73,7 @@ ROS3D.OccupancyGridClient.prototype.processMessage = function(message){
       // grid is of type ROS3D.SceneNode
       this.currentGrid.unsubscribeTf();
     }
-    this.rootObject.remove(this.currentGrid);
+    this.sceneNode.remove(this.currentGrid);
   }
 
   var newGrid = new ROS3D.OccupancyGrid({
@@ -84,18 +85,22 @@ ROS3D.OccupancyGridClient.prototype.processMessage = function(message){
   // check if we care about the scene
   if (this.tfClient) {
     this.currentGrid = newGrid;
-    this.sceneNode = new ROS3D.SceneNode({
-      frameID : message.header.frame_id,
-      tfClient : this.tfClient,
-      object : newGrid,
-      pose : this.offsetPose
-    });
+    if (this.sceneNode === null) {
+      this.sceneNode = new ROS3D.SceneNode({
+        frameID : message.header.frame_id,
+        tfClient : this.tfClient,
+        object : newGrid,
+        pose : this.offsetPose
+      });
+      this.rootObject.add(this.sceneNode);
+    } else {
+      this.sceneNode.add(this.currentGrid);
+    }
   } else {
     this.sceneNode = this.currentGrid = newGrid;
+    this.rootObject.add(this.currentGrid);
   }
 
-  this.rootObject.add(this.sceneNode);
-
   this.emit('change');
 
   // check if we should unsubscribe