Added user data usage example with wifi signal

CMakeLists.txt

@@ -293,6 +293,11 @@ target_link_libraries(map_optimizer rtabmap_ros ${Libraries})
 add_executable(map_assembler src/MapAssemblerNode.cpp)
 target_link_libraries(map_assembler rtabmap_ros ${Libraries})
 
+add_executable(wifi_signal_pub src/WifiSignalPubNode.cpp)
+target_link_libraries(wifi_signal_pub rtabmap_ros ${Libraries})
+add_executable(wifi_signal_sub src/WifiSignalSubNode.cpp)
+target_link_libraries(wifi_signal_sub rtabmap_ros ${Libraries})
+
 add_executable(camera src/CameraNode.cpp)
 add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_link_libraries(camera ${Libraries})

include/rtabmap_ros/CoreWrapper.h

@@ -115,6 +115,8 @@ private:
 
 	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
 
+	void userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg);
+
 	void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
 	void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
 	void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
@@ -252,6 +254,9 @@ private:
 	// for loop closure detection only
 	image_transport::Subscriber defaultSub_;
 
+	ros::Subscriber userDataAsyncSub_;
+	cv::Mat userData_;
+
 	bool stereoToDepth_;
 	bool odomSensorSync_;
 	float rate_;

launch/rtabmap.launch

@@ -70,6 +70,9 @@
   <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
   <arg name="odom_args"                default="$(arg rtabmap_args)"/>
   
+  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
+  <arg name="user_data_topic"          default="/user_data"/>
+  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
   
   <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
   <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
@@ -129,6 +132,7 @@
       <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
       <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
       <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
+      <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
       <param name="frame_id"             type="string" value="$(arg frame_id)"/>
       <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
       <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
@@ -150,6 +154,8 @@
       
       <remap from="scan"                   to="$(arg scan_topic)"/>
       <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
+      <remap from="user_data"              to="$(arg user_data_topic)"/>
+      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
       <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
       
       <!-- localization mode -->

src/CoreWrapper.cpp

@@ -501,6 +501,8 @@ void CoreWrapper::onInit()
 
 		NODELET_INFO("\n%s subscribed to:\n   %s", getName().c_str(), defaultSub_.getTopic().c_str());
 	}
+
+	userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
 }
 
 CoreWrapper::~CoreWrapper()
@@ -937,6 +939,16 @@ void CoreWrapper::commonDepthCallbackImpl(
 	if(userDataMsg.get())
 	{
 		userData = rtabmap_ros::userDataFromROS(*userDataMsg);
+		if(!userData_.empty())
+		{
+			ROS_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
+			userData_ = cv::Mat();
+		}
+	}
+	else
+	{
+		userData = userData_;
+		userData_ = cv::Mat();
 	}
 	SensorData data(scan,
 			LaserScanInfo(
@@ -1119,6 +1131,9 @@ void CoreWrapper::commonStereoCallback(
 		}
 	}
 
+	cv::Mat userData = userData_;
+	userData_ = cv::Mat();
+
 	Transform groundTruthPose;
 	if(!groundTruthFrameId_.empty())
 	{
@@ -1134,7 +1149,8 @@ void CoreWrapper::commonStereoCallback(
 			right,
 			stereoModel,
 			lastPoseIntermediate_?-1:leftImageMsg->header.seq,
-			rtabmap_ros::timestampFromROS(lastPoseStamp_));
+			rtabmap_ros::timestampFromROS(lastPoseStamp_),
+			userData);
 	data.setGroundTruth(groundTruthPose);
 
 	process(lastPoseStamp_,
@@ -1308,11 +1324,29 @@ void CoreWrapper::process(
 		NODELET_WARN("Ignoring received image because its sequence ID=0. Please "
 				 "set \"Mem/GenerateIds\"=\"true\" to ignore ros generated sequence id. "
 				 "Use only \"Mem/GenerateIds\"=\"false\" for once-time run of RTAB-Map and "
-				 "when you need to have IDs output of RTAB-map synchronised with the source "
+				 "when you need to have IDs output of RTAB-map synchronized with the source "
 				 "image sequence ID.");
 	}
 }
 
+void CoreWrapper::userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg)
+{
+	if(!paused_)
+	{
+		if(!userData_.empty())
+		{
+			ROS_WARN("Overwriting previous user data set. Asynchronous user "
+					"data input topic should be used with user data published at "
+					"lower rate than map update rate (current %s=%f).",
+					Parameters::kRtabmapDetectionRate().c_str(), rate_);
+		}
+		else
+		{
+			userData_ = rtabmap_ros::userDataFromROS(*dataMsg);
+		}
+	}
+}
+
 void CoreWrapper::goalCommonCallback(
 		int id,
 		const std::string & label,
@@ -1527,6 +1561,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 	latestNodeWasReached_ = false;
 	mapsManager_.clear();
 	previousStamp_ = ros::Time(0);
+	userData_ = cv::Mat();
 	return true;
 }
 
@@ -1580,6 +1615,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
 	lastPose_.setIdentity();
 	currentMetricGoal_.setNull();
 	latestNodeWasReached_ = false;
+	userData_ = cv::Mat();
 
 	NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
 	UFile::copy(databasePath_, databasePath_+".back");

src/WifiSignalPubNode.cpp

@@ -0,0 +1,149 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <ros/publisher.h>
+
+#include <sys/socket.h>
+#include <linux/wireless.h>
+#include <sys/ioctl.h>
+
+#include <rtabmap_ros/MsgConversion.h>
+#include <rtabmap_ros/UserData.h>
+
+// Demo:
+// $ roslaunch freenect_launch freenect.launch depth_registration:=true
+// $ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" user_data_async_topic:=/wifi_signal rtabmapviz:=false rviz:=true
+// $ rosrun rtabmap_ros wifi_signal_pub interface:="wlan0"
+// $ rosrun rtabmap_ros wifi_signal_sub
+// In RVIZ add PointCloud2 "wifi_signals"
+
+// 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
+// of 0 implies an actual RSSI signal strength of -100 dbm.
+// A value of 100 implies an actual RSSI signal strength of -50 dbm.
+// You can calculate the RSSI signal strength value for wlanSignalQuality
+// values between 1 and 99 using linear interpolation.
+inline int quality2dBm(int quality)
+{
+	// Quality to dBm:
+	if(quality <= 0)
+		return -100;
+	else if(quality >= 100)
+		return -50;
+	else
+		return (quality / 2) - 100;
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "wifi_signal_pub");
+
+	ros::NodeHandle nh;
+	ros::NodeHandle pnh("~");
+
+	std::string interface = "wlan0";
+	double rateHz = 0.5; // Hz
+	std::string frameId = "base_link";
+
+	pnh.param("interface", interface, interface);
+	pnh.param("rate", rateHz, rateHz);
+	pnh.param("frame_id", frameId, frameId);
+
+	ros::Rate rate(rateHz);
+
+	ros::Publisher wifiPub = nh.advertise<rtabmap_ros::UserData>("wifi_signal", 1);
+
+	while(ros::ok())
+	{
+		int dBm = 0;
+
+		// Code inspired from http://blog.ajhodges.com/2011/10/using-ioctl-to-gather-wifi-information.html
+
+		//have to use a socket for ioctl
+		int sockfd;
+		/* Any old socket will do, and a datagram socket is pretty cheap */
+		if((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
+			ROS_ERROR("Could not create simple datagram socket");
+			return -1;
+		}
+
+		struct iwreq req;
+		struct iw_statistics stats;
+
+		strncpy(req.ifr_name, interface.c_str(), IFNAMSIZ);
+
+		//make room for the iw_statistics object
+		req.u.data.pointer = (caddr_t) &stats;
+		req.u.data.length = sizeof(stats);
+		// clear updated flag
+		req.u.data.flags = 1;
+
+		//this will gather the signal strength
+		if(ioctl(sockfd, SIOCGIWSTATS, &req) == -1)
+		{
+			//die with error, invalid interface
+			ROS_ERROR("Invalid interface (\"%s\"). Tip: Try with sudo!", interface.c_str());
+		}
+		else if(((iw_statistics *)req.u.data.pointer)->qual.updated & IW_QUAL_DBM)
+		{
+			//signal is measured in dBm and is valid for us to use
+			dBm = ((iw_statistics *)req.u.data.pointer)->qual.level - 256;
+		}
+		else
+		{
+			ROS_ERROR("Could not get signal level.");
+		}
+
+		close(sockfd);
+
+		if(dBm != 0)
+		{
+			ros::Time stamp = ros::Time::now();
+
+			// Create user data [level] with the value
+			cv::Mat data(1, 2, CV_64FC1);
+			data.at<double>(0) = double(dBm);
+
+			// we should set stamp in data to be able to
+			// retrieve it from rtabmap map data to get precise
+			// position in the graph afterward
+			data.at<double>(1) = stamp.toSec();
+
+			rtabmap_ros::UserData dataMsg;
+			dataMsg.header.frame_id = frameId;
+			dataMsg.header.stamp = stamp;
+			rtabmap_ros::userDataToROS(data, dataMsg, false);
+			wifiPub.publish<rtabmap_ros::UserData>(dataMsg);
+		}
+		ros::spinOnce();
+		rate.sleep();
+	}
+
+	return 0;
+}

src/WifiSignalSubNode.cpp

@@ -0,0 +1,207 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <ros/publisher.h>
+#include <ros/subscriber.h>
+
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/core/util3d_transforms.h>
+
+#include <rtabmap_ros/MapData.h>
+#include <rtabmap_ros/MsgConversion.h>
+
+#include <sensor_msgs/PointCloud2.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+// 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
+// of 0 implies an actual RSSI signal strength of -100 dbm.
+// A value of 100 implies an actual RSSI signal strength of -50 dbm.
+// You can calculate the RSSI signal strength value for wlanSignalQuality
+// values between 1 and 99 using linear interpolation.
+inline int dBm2Quality(int dBm)
+{
+	// dBm to Quality:
+    if(dBm <= -100)
+        return 0;
+    else if(dBm >= -50)
+    	return 100;
+    else
+    	return 2 * (dBm + 100);
+}
+
+ros::Publisher wifiSignalCloudPub;
+std::map<double, int> wifiLevels;
+
+void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg)
+{
+	ROS_INFO("Received map data!");
+
+	rtabmap::Transform mapToOdom;
+	std::map<int, rtabmap::Transform> poses;
+	std::multimap<int, rtabmap::Link> links;
+	std::map<int, rtabmap::Signature> signatures;
+	rtabmap_ros::mapDataFromROS(*mapDataMsg, poses, links, signatures, mapToOdom);
+
+	std::map<double, int> nodeStamps; // <stamp, id>
+	for(std::map<int, rtabmap::Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+	{
+		cv::Mat data;
+		iter->second.sensorData().uncompressDataConst(0 ,0, 0, &data);
+
+		if(data.type() == CV_64FC1 && data.rows == 1 && data.cols == 2)
+		{
+			// format [int level, double stamp], see wifi_signal_pub_node.cpp
+			int level = data.at<double>(0);
+			double stamp = data.at<double>(1);
+			wifiLevels.insert(std::make_pair(stamp, level));
+		}
+		else if(!data.empty())
+		{
+			ROS_ERROR("Wrong user data format for wifi signal.");
+		}
+
+		// Sort stamps by stamps->id
+		nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
+	}
+
+	if(wifiLevels.size() == 0)
+	{
+		ROS_WARN("No wifi signal detected yet in user data of map data");
+	}
+
+	//============================
+	// Add WIFI symbols
+	//============================
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledWifiSignals(new pcl::PointCloud<pcl::PointXYZRGB>);
+	int id = 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.
+		double stampWifi = iter->first;
+		std::map<double, int>::iterator previousNode = nodeStamps.lower_bound(stampWifi); // lower bound of the stamp
+		if(previousNode!=nodeStamps.end() && previousNode->first > stampWifi && previousNode != nodeStamps.begin())
+		{
+			--previousNode;
+		}
+		std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(stampWifi); // upper bound of the stamp
+
+		if(previousNode != nodeStamps.end() &&
+		   nextNode != nodeStamps.end() &&
+		   previousNode->second != nextNode->second &&
+		   uContains(poses, previousNode->second) && uContains(poses, nextNode->second))
+		{
+			rtabmap::Transform poseA = poses.at(previousNode->second);
+			rtabmap::Transform poseB = poses.at(nextNode->second);
+			double stampA = previousNode->first;
+			double stampB = nextNode->first;
+			UASSERT(stampWifi>=stampA && stampWifi <=stampB);
+
+			rtabmap::Transform v = poseA.inverse() * poseB;
+			double ratio = (stampWifi-stampA)/(stampB-stampA);
+
+			v.x()*=ratio;
+			v.y()*=ratio;
+			v.z()*=ratio;
+
+			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)
+			{
+				// 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)
+				{
+					// green
+					pt.g = 255;
+					if(i<7)
+					{
+						// yellow
+						pt.r = 255;
+					}
+				}
+				else
+				{
+					// gray
+					pt.r = pt.g = pt.b = 100;
+				}
+				cloud->push_back(pt);
+			}
+			pcl::PointXYZRGB anchor(255, 0, 0);
+			cloud->push_back(anchor);
+
+			cloud = rtabmap::util3d::transformPointCloud(cloud, wifiPose);
+
+			if(assembledWifiSignals->size() == 0)
+			{
+				*assembledWifiSignals = *cloud;
+			}
+			else
+			{
+				*assembledWifiSignals += *cloud;
+			}
+		}
+	}
+
+	if(assembledWifiSignals->size())
+	{
+		sensor_msgs::PointCloud2 cloudMsg;
+		pcl::toROSMsg(*assembledWifiSignals, cloudMsg);
+		cloudMsg.header = mapDataMsg->header;
+		wifiSignalCloudPub.publish(cloudMsg);
+	}
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "wifi_signal_sub");
+
+	ros::NodeHandle nh;
+	ros::NodeHandle pnh("~");
+
+	std::string interface = "wlan0";
+	double rateHz = 0.5; // Hz
+	std::string frameId = "base_link";
+
+	wifiSignalCloudPub = nh.advertise<sensor_msgs::PointCloud2>("wifi_signals", 1);
+	ros::Subscriber mapDataSub = nh.subscribe("/rtabmap/mapData", 1, mapDataCallback);
+
+	ros::spin();
+
+	return 0;
+}