diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8f8dc571..ea637846 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -106,6 +106,7 @@ add_message_files(
    UserData.msg
    GPS.msg
    Path.msg
+   EnvSensor.msg
 )
 
 ## Generate services in the 'srv' folder
diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h
index bc2b393d..9a267928 100644
--- a/include/rtabmap_ros/MsgConversion.h
+++ b/include/rtabmap_ros/MsgConversion.h
@@ -98,6 +98,11 @@ void globalDescriptorToROS(const rtabmap::GlobalDescriptor & desc, rtabmap_ros::
 std::vector<rtabmap::GlobalDescriptor> globalDescriptorsFromROS(const std::vector<rtabmap_ros::GlobalDescriptor> & msg);
 void globalDescriptorsToROS(const std::vector<rtabmap::GlobalDescriptor> & desc, std::vector<rtabmap_ros::GlobalDescriptor> & msg);
 
+rtabmap::EnvSensor envSensorFromROS(const rtabmap_ros::EnvSensor & msg);
+void envSensorToROS(const rtabmap::EnvSensor & sensor, rtabmap_ros::EnvSensor & msg);
+rtabmap::EnvSensors envSensorsFromROS(const std::vector<rtabmap_ros::EnvSensor> & msg);
+void envSensorsToROS(const rtabmap::EnvSensors & sensors, std::vector<rtabmap_ros::EnvSensor> & msg);
+
 cv::Point2f point2fFromROS(const rtabmap_ros::Point2f & msg);
 void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg);
 
diff --git a/msg/EnvSensor.msg b/msg/EnvSensor.msg
new file mode 100644
index 00000000..23240d9d
--- /dev/null
+++ b/msg/EnvSensor.msg
@@ -0,0 +1,6 @@
+
+Header header
+
+# EnvSensor
+int32 type
+float64 value
\ No newline at end of file
diff --git a/msg/NodeData.msg b/msg/NodeData.msg
index 83dcb6d5..9a1123fe 100644
--- a/msg/NodeData.msg
+++ b/msg/NodeData.msg
@@ -64,3 +64,5 @@ Point3f[] wordPts
 uint8[] wordDescriptors
 
 GlobalDescriptor[] globalDescriptors
+
+EnvSensor[] env_sensors
diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp
index 5fda9139..316c9a18 100644
--- a/src/MsgConversion.cpp
+++ b/src/MsgConversion.cpp
@@ -566,6 +566,46 @@ void globalDescriptorsToROS(const std::vector<rtabmap::GlobalDescriptor> & desc,
 	}
 }
 
+rtabmap::EnvSensor envSensorFromROS(const rtabmap_ros::EnvSensor & msg)
+{
+	return rtabmap::EnvSensor((rtabmap::EnvSensor::Type)msg.type, msg.value, timestampFromROS(msg.header.stamp));
+}
+
+void envSensorToROS(const rtabmap::EnvSensor & sensor, rtabmap_ros::EnvSensor & msg)
+{
+	msg.type = sensor.type();
+	msg.value = sensor.value();
+	msg.header.stamp = ros::Time(sensor.stamp());
+}
+
+rtabmap::EnvSensors envSensorsFromROS(const std::vector<rtabmap_ros::EnvSensor> & msg)
+{
+	rtabmap::EnvSensors v;
+	if(!msg.empty())
+	{
+		for(unsigned int i=0; i<msg.size(); ++i)
+		{
+			rtabmap::EnvSensor s = envSensorFromROS(msg[i]);
+			v.insert(std::make_pair(s.type(), envSensorFromROS(msg[i])));
+		}
+	}
+	return v;
+}
+
+void envSensorsToROS(const rtabmap::EnvSensors & sensors, std::vector<rtabmap_ros::EnvSensor> & msg)
+{
+	msg.clear();
+	if(!sensors.empty())
+	{
+		msg.resize(sensors.size());
+		int i=0;
+		for(rtabmap::EnvSensors::const_iterator iter=sensors.begin(); iter!=sensors.end(); ++iter)
+		{
+			envSensorToROS(iter->second, msg[i++]);
+		}
+	}
+}
+
 cv::Point2f point2fFromROS(const rtabmap_ros::Point2f & msg)
 {
 	return cv::Point2f(msg.x, msg.y);
@@ -994,6 +1034,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 	s.setWords3(words3D);
 	s.setWordsDescriptors(wordsDescriptors);
 	s.sensorData().setGlobalDescriptors(rtabmap_ros::globalDescriptorsFromROS(msg.globalDescriptors));
+	s.sensorData().setEnvSensors(rtabmap_ros::envSensorsFromROS(msg.env_sensors));
 	s.sensorData().setOccupancyGrid(
 			compressedMatFromBytes(msg.grid_ground),
 			compressedMatFromBytes(msg.grid_obstacles),
@@ -1132,6 +1173,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 	}
 
 	rtabmap_ros::globalDescriptorsToROS(signature.sensorData().globalDescriptors(), msg.globalDescriptors);
+	rtabmap_ros::envSensorsToROS(signature.sensorData().envSensors(), msg.env_sensors);
 }
 
 rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg)
diff --git a/src/WifiSignalSubNode.cpp b/src/WifiSignalSubNode.cpp
index c0abe87b..8fcd4f8f 100644
--- a/src/WifiSignalSubNode.cpp
+++ b/src/WifiSignalSubNode.cpp
@@ -42,6 +42,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
 
+bool hueSymbol = false;
+int min_dbm = -100;
+int max_dbm = -50;
+
 // A percentage value that represents the signal quality
 // of the network. WLAN_SIGNAL_QUALITY is of type ULONG.
 // This member contains a value between 0 and 100. A value
@@ -52,12 +56,63 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 inline int dBm2Quality(int dBm)
 {
 	// dBm to Quality:
-    if(dBm <= -100)
+    if(dBm <= min_dbm)
         return 0;
-    else if(dBm >= -50)
+    else if(dBm >= max_dbm)
     	return 100;
     else
-    	return 2 * (dBm + 100);
+    {
+    	return -(dBm-min_dbm)*100/(min_dbm-max_dbm);
+    }
+}
+
+void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
+{
+	int i;
+	float f, p, q, t;
+	if( s == 0 ) {
+		// achromatic (grey)
+		*r = *g = *b = v;
+		return;
+	}
+	h /= 60;			// sector 0 to 5
+	i = floor( h );
+	f = h - i;			// factorial part of h
+	p = v * ( 1 - s );
+	q = v * ( 1 - s * f );
+	t = v * ( 1 - s * ( 1 - f ) );
+	switch( i ) {
+		case 0:
+			*r = v;
+			*g = t;
+			*b = p;
+			break;
+		case 1:
+			*r = q;
+			*g = v;
+			*b = p;
+			break;
+		case 2:
+			*r = p;
+			*g = v;
+			*b = t;
+			break;
+		case 3:
+			*r = p;
+			*g = q;
+			*b = v;
+			break;
+		case 4:
+			*r = t;
+			*g = p;
+			*b = v;
+			break;
+		default:		// case 5:
+			*r = v;
+			*g = p;
+			*b = q;
+			break;
+	}
 }
 
 ros::Publisher wifiSignalCloudPub;
@@ -82,7 +137,12 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
 
 		nodeStamps_.insert(std::make_pair(node.getStamp(), node.id()));
 
-		if(!node.sensorData().userDataCompressed().empty())
+		if(node.sensorData().envSensors().find(rtabmap::EnvSensor::kWifiSignalStrength) != node.sensorData().envSensors().end())
+		{
+			rtabmap::EnvSensor sensor = node.sensorData().envSensors().at(rtabmap::EnvSensor::kWifiSignalStrength);
+			wifiLevels.insert(std::make_pair(sensor.stamp()>0.0?sensor.stamp():iter->second.getStamp(), sensor.value()));
+		}
+		else if(!node.sensorData().userDataCompressed().empty())
 		{
 			cv::Mat data;
 			node.sensorData().uncompressDataConst(0 ,0, 0, &data);
@@ -124,6 +184,7 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledWifiSignals(new pcl::PointCloud<pcl::PointXYZRGB>);
 	int id = 0;
+	float min=0,max=0;
 	for(std::map<double, int>::iterator iter=wifiLevels.begin(); iter!=wifiLevels.end(); ++iter, ++id)
 	{
 		// The Wifi value may be taken between two nodes, interpolate its position.
@@ -155,34 +216,56 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
 
 			rtabmap::Transform wifiPose = (poseA*v).translation(); // rip off the rotation
 
-			// Make a line with points
-			int quality = dBm2Quality(iter->second)/10;
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-			for(int i=0; i<10; ++i)
+			if(min == 0.0f || min > iter->second)
 			{
-				// 2 cm between each points
-				// the number of points depends on the dBm (which varies from -30 (near) to -80 (far))
-				pcl::PointXYZRGB pt;
-				pt.z = float(i+1)*0.02f;
-				if(i<quality)
+				min = iter->second;
+			}
+			if(max == 0.0f || max < iter->second)
+			{
+				max = iter->second;
+			}
+
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+			if(hueSymbol)
+			{
+				// scale between red -> yellow -> green
+				int quality = dBm2Quality(iter->second)*120/100;
+				float r,g,b;
+				HSVtoRGB(&r,&g,&b,quality,1,1);
+				pcl::PointXYZRGB anchor(r*255, g*255, b*255);
+				cloud->push_back(anchor);
+			}
+			else
+			{
+
+				// Make a line with points
+				int quality = dBm2Quality(iter->second)/10;
+				for(int i=0; i<10; ++i)
 				{
-					// green
-					pt.g = 255;
-					if(i<7)
+					// 2 cm between each points
+					// the number of points depends on the dBm (which varies from -30 (near) to -80 (far))
+					pcl::PointXYZRGB pt;
+					pt.z = float(i+1)*0.02f;
+					if(i<quality)
 					{
-						// yellow
-						pt.r = 255;
+						// green
+						pt.g = 255;
+						if(i<7)
+						{
+							// yellow
+							pt.r = 255;
+						}
 					}
-				}
-				else
-				{
-					// gray
-					pt.r = pt.g = pt.b = 100;
+					else
+					{
+						// gray
+						pt.r = pt.g = pt.b = 100;
+					}
+					cloud->push_back(pt);
 				}
-				cloud->push_back(pt);
+				pcl::PointXYZRGB anchor(255, 0, 0);
+				cloud->push_back(anchor);
 			}
-			pcl::PointXYZRGB anchor(255, 0, 0);
-			cloud->push_back(anchor);
 
 			cloud = rtabmap::util3d::transformPointCloud(cloud, wifiPose);
 
@@ -196,6 +279,7 @@ void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
 			}
 		}
 	}
+	ROS_INFO("Min/Max dBm = %f %f", min, max);
 
 	if(assembledWifiSignals->size())
 	{
@@ -213,9 +297,9 @@ int main(int argc, char** argv)
 	ros::NodeHandle nh;
 	ros::NodeHandle pnh("~");
 
-	std::string interface = "wlan0";
-	double rateHz = 0.5; // Hz
-	std::string frameId = "base_link";
+	pnh.param("hue_symbol", hueSymbol, hueSymbol);
+	pnh.param("min", min_dbm, min_dbm);
+	pnh.param("max", max_dbm, max_dbm);
 
 	wifiSignalCloudPub = nh.advertise<sensor_msgs::PointCloud2>("wifi_signals", 1);
 	ros::Subscriber mapDataSub = nh.subscribe("/rtabmap/mapData", 1, mapDataCallback);