!

Note: This tutorial assumes that you have completed the previous tutorials: RGB-D Hand-Held Mapping With a Kinect.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Wifi Signal Strength Mapping (User Data Usage)

Description: This tutorial shows how to add user data during mapping that will be saved directly in RTAB-Map's database for convenience.

Tutorial Level: INTERMEDIATE

Introduction

Version >0.11.14 required.

This tutorial will show how to create a map including information taken with any other sensor (e.g., the user data). For convenience, we will map Wifi signal strength of the environment. The screenshot below shows how the Wifi signal strength is visualized. It is just a level bar used (sensor_msgs/Pointcloud2), but more fancy output like Markers can be used to customize the look. Wifi mapping 1 Wifi mapping 2

How to Publish User Data (wifi_signal_pub)

To add user data to map, rtabmap node has topics called user_data and user_data_async. The first one is used when parameter subscribe_user_data:=true, and it will be synchronized with other sensor data. It means that the publishing rate of this topic should be high (at least as fast as the images received). The second topic user_data_async is asynchronous. It should be used only when user data is published slower than the detection rate of rtabmap (default 1 Hz). The received user data will be saved in the next node created in rtabmap (if user data is published faster than nodes are created, old user data will be overwritten).

For this example, we use user_data_async with a Wifi signal data published each 2 seconds. Here is the basic information needed to publish a rtabmap_msgs/UserData topic (the full code is here):

   1 #include <rtabmap_conversions/MsgConversion.h>
   2 #include <rtabmap_msgs/UserData.h>
   3 
   4 int main(int argc, char** argv)
   5 {
   6    //... node initialization stuff
   7 
   8    ros::Rate rate(0.5); 
   9 
  10    ros::Publisher wifiPub = nh.advertise<rtabmap_msgs::UserData>("wifi_signal", 1);
  11    
  12    while(ros::ok())
  13    {
  14       ros::Time stamp = ros::Time::now();
  15 
  16       // Get signal strength...
  17       int dBm = ...
  18 
  19       // Create user data [level, stamp].
  20       // OpenCV matrix is used for convenience. Any format is accepted.
  21       // However, if CV_8UC1 format is used, make sure rows > 1 as 
  22       // rtabmap will think it is already compressed.
  23       cv::Mat data(1, 2, CV_64FC1);
  24       data.at<double>(0) = double(dBm);
  25 
  26       // We should set stamp in data to be able to
  27       // retrieve it from the saved user data as we need 
  28       // to get precise position in the graph afterward.
  29       data.at<double>(1) = stamp.toSec();
  30 
  31       rtabmap_msgs::UserData dataMsg;
  32       dataMsg.header.frame_id = "wifi_link";
  33       dataMsg.header.stamp = stamp;
  34 
  35       // Convert OpenCV matrix to UserData msg. Optionally, 
  36       // we can choose to compress the data now (here we keep uncompressed).
  37       rtabmap_conversions::userDataToROS(data, dataMsg, false);
  38       
  39       // Publish data!
  40       wifiPub.publish<rtabmap_msgs::UserData>(dataMsg);
  41       
  42       ros::spinOnce();
  43       rate.sleep();
  44    }
  45    return 0;
  46 }

Publisher example in Python

   1 #!/usr/bin/env python
   2 import rospy
   3 import struct
   4 import os
   5 from rtabmap_msgs.msg import UserData
   6 
   7 def loop():
   8     rospy.init_node('wifi_signal_pub', anonymous=True)
   9     pub = rospy.Publisher('wifi_signal', UserData, queue_size=10)
  10     rate = rospy.Rate(0.5) # 0.5hz
  11     while not rospy.is_shutdown():
  12     
  13         myCmd = os.popen('nmcli dev wifi | grep "^*"').read()
  14         cmdList = myCmd.split()
  15         
  16         if len(cmdList) > 6:
  17             quality = float(cmdList[6])
  18             msg = UserData()
  19             
  20             # To make it compatible with c++ sub example, use dBm
  21             dBm = quality/2-100
  22             rospy.loginfo("Network \"%s\": Quality=%d, %f dBm", cmdList[1], quality, dBm)
  23             
  24             # Create user data [level, stamp].
  25             # Any format is accepted.
  26             # However, if CV_8UC1 format is used, make sure rows > 1 as 
  27             # rtabmap will think it is already compressed.
  28             msg.rows = 1
  29             msg.cols = 2
  30             msg.type = 6 # Use OpenCV type (here 6=CV_64FC1): http://ninghang.blogspot.com/2012/11/list-of-mat-type-in-opencv.html
  31             
  32             # We should set stamp in data to be able to
  33             # retrieve it from the saved user data as we need 
  34             # to get precise position in the graph afterward.
  35             msg.data = struct.pack(b'dd', dBm, rospy.get_time())
  36 
  37             pub.publish(msg)
  38         else:
  39             rospy.logerr("Cannot get info from wireless!")
  40         rate.sleep()
  41 
  42 if __name__ == '__main__':
  43     try:
  44         loop()
  45     except rospy.ROSInterruptException:
  46         pass

How to Retrieve User Data (wifi_signal_sub)

rtabmap publishes rtabmap_msgs/MapData topic after each update. In this topic, we can get the user data that we published to rtabmap earlier, which is now linked to graph. Here is an example (the full code is here):

   1 #include <rtabmap_msgs/MapData.h>
   2 #include <rtabmap_conversions/MsgConversion.h>
   3 
   4 std::map<double, int> wifiLevels;
   5 void mapDataCallback(const rtabmap_msgs::MapDataConstPtr & mapDataMsg)
   6 {
   7    // Convert MapData message in more user friendly objects
   8    rtabmap::Transform mapToOdom;
   9    std::map<int, rtabmap::Transform> poses;
  10    std::multimap<int, rtabmap::Link> links;
  11    std::map<int, rtabmap::Signature> signatures;
  12 
  13    rtabmap_msgs::mapDataFromROS(*mapDataMsg, poses, links, signatures, mapToOdom);
  14 
  15    // The user data of the latest node added to 
  16    // map is contained in signatures map. Iterate over 
  17    // to get the data. Handle the case where we can 
  18    // receive only latest data, or if all data are published
  19    for(std::map<int, rtabmap::Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
  20    {
  21       int id = iter->first;
  22       rtabmap::Signature & node = iter->second;
  23 
  24       if(!node.sensorData().userDataCompressed().empty())
  25       {
  26          cv::Mat data;
  27          node.sensorData().uncompressDataConst(0 ,0, 0, &data);
  28 
  29          if(data.type() == CV_64FC1 && data.rows == 1 && data.cols == 2)
  30          {
  31             // format [int level, double stamp], see wifi_signal_pub_node.cpp
  32             int level = data.at<double>(0);
  33             double stamp = data.at<double>(1);
  34             wifiLevels.insert(std::make_pair(stamp, level));
  35          }
  36          else if(!data.empty())
  37          {
  38             ROS_ERROR("Wrong user data format for wifi signal.");
  39          }
  40       }
  41    }
  42 
  43    // "wifiLevels" contains all signal values sorted 
  44    // by timestamp. We can then find their exact position 
  45    // on the graph. See referred source code for details...
  46 }
  47 
  48 
  49 
  50 int main(int argc, char** argv)
  51 {
  52    //... node initialization stuff
  53 
  54    ros::Subscriber mapDataSub = nh.subscribe("/rtabmap/mapData", 1, mapDataCallback);
  55 
  56    ros::spin();
  57 }

Example of Usage

From the hand-held tutorial, start your camera and launch rtabmap.launch with argument "user_data_async_topic:=/wifi_signal", then start wifi publisher and subscriber:

$ roslaunch rtabmap_launch rtabmap.launch [...] user_data_async_topic:=/wifi_signal

// C++:
$ rosrun rtabmap_demos wifi_signal_pub _interface:="wlan0"
// Python:
$ python wifi_signal_pub.py

$ rosrun rtabmap_demos wifi_signal_sub

In RVIZ, add PointCloud2 topic called wifi_signals published from wifi_signal_sub node. Note that you may need to start wifi_signal_pub in sudo. To do so:

$ sudo su
$ source /opt/ros/kinetic/setup.bash
$ source /home/{user_name}/catkin_ws/devel/setup.bash (if rtabmap is built from source)
$ rosrun rtabmap_demos wifi_signal_pub interface:="wlan0"

If you move the camera, you should see the signal strength bars like in the screenshots above.

Example of Result

You can show the map of the screenshots above using this database demo_wifi.db (taken with a stereo camera) with the lines below:

$ roslaunch rtabmap_launch rtabmap.launch database_path:=~/Downloads/demo_wifi.db rtabmap_viz:=false rviz:=true
$ rosrun rtabmap_demos wifi_signal_sub
// In RVIZ, add `PointCloud2` topic called `wifi_signals`
$ rosservice call /rtabmap/publish_map 1 1 0

Wiki: rtabmap_ros/Tutorials/WifiSignalStrengthMappingUserDataUsage (last edited 2023-04-19 05:37:25 by MathieuLabbe)