From 18b8acc2eabfeb2fd926bfb6ec834638088f7c0b Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 15 Jul 2020 15:38:05 -0400
Subject: [PATCH] Added wifi_signal_pub.py example

---
 CMakeLists.txt             |  2 ++
 scripts/wifi_signal_pub.py | 46 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 48 insertions(+)
 create mode 100755 scripts/wifi_signal_pub.py

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 913f5c60..8f8dc571 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -516,6 +516,7 @@ catkin_install_python(PROGRAMS
   scripts/point_to_tf.py
   scripts/transform_to_tf.py
   scripts/yaml_to_camera_info.py
+  scripts/wifi_signal_pub.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
@@ -538,6 +539,7 @@ install(TARGETS
    rtabmap_camera
    rtabmap_rgbd_sync
    rtabmap_rgbd_relay
+   rtabmap_wifi_signal_sub
    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
diff --git a/scripts/wifi_signal_pub.py b/scripts/wifi_signal_pub.py
new file mode 100755
index 00000000..7aefef5d
--- /dev/null
+++ b/scripts/wifi_signal_pub.py
@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+import rospy
+import struct
+import os
+from rtabmap_ros.msg import UserData
+
+def loop():
+    rospy.init_node('wifi_signal_pub', anonymous=True)
+    pub = rospy.Publisher('wifi_signal', UserData, queue_size=10)
+    rate = rospy.Rate(0.5) # 0.5hz
+    while not rospy.is_shutdown():
+    
+        myCmd = os.popen('nmcli dev wifi | grep "^*"').read()
+        cmdList = myCmd.split()
+        
+        if len(cmdList) > 6:
+            quality = float(cmdList[6])
+            msg = UserData()
+            
+            # To make it compatible with c++ sub example, use dBm
+            dBm = quality/2-100
+            rospy.loginfo("Network \"%s\": Quality=%d, %f dBm", cmdList[1], quality, dBm)
+            
+            # Create user data [level, stamp].
+            # Any format is accepted.
+            # However, if CV_8UC1 format is used, make sure rows > 1 as 
+            # rtabmap will think it is already compressed.
+            msg.rows = 1
+            msg.cols = 2
+            msg.type = 6 # Use OpenCV type (here 6=CV_64FC1): http://ninghang.blogspot.com/2012/11/list-of-mat-type-in-opencv.html
+            
+            # We should set stamp in data to be able to
+            # retrieve it from the saved user data as we need 
+            # to get precise position in the graph afterward.
+            msg.data = struct.pack(b'dd', dBm, rospy.get_time())
+
+            pub.publish(msg)
+        else:
+            rospy.logerr("Cannot get info from wireless!")
+        rate.sleep()
+
+if __name__ == '__main__':
+    try:
+        loop()
+    except rospy.ROSInterruptException:
+        pass