Merge pull request #4 from marco-tranzatto/feature/record_corrections…

…_latency

Record Latency Over Wifi

cfg/piksi_driver_settings.yaml

@@ -8,6 +8,7 @@ base_station_mode: false
 # use ifconfig to show network broadcast (or calculate it yourself....)
 broadcast_addr: 10.10.50.255
 broadcast_port: 26078
+base_station_ip_for_latency_estimation: 10.10.50.1 # ip base station if you want to estimate latency
 # Coordinate frame of the GPS receiver with respect to the base link
 frame_id: 'base_link'
 
@@ -38,6 +39,7 @@ publish_tracking_state: true
 # Custom debug messages
 publish_piksidebug: true #published only if at least one of the following is pub: heartbeat, tracking_state, navsatfix_rtk
 publish_uart_state: true
+publish_wifi_corrections_received: true # will not be published if base_station_mode is true
 
 # Covariances to be published in the NavSatFix message
 # Single Point Positioning (SPP)

msg/PiksiNumCorrections.msg

@@ -1,3 +1,4 @@
 Header header
 
 uint32 received_corrections # number of correction received from start up
+float32 latency # latency in ms from base station, -1 if not sampled

src/piksi.py

@@ -28,9 +28,10 @@
 
 # networking stuff
 import UdpHelpers
-
 import time
-
+import subprocess
+import re
+import threading, time
 
 class Piksi:
     def __init__(self):
@@ -86,6 +87,9 @@ def __init__(self):
         self._publish_uart_state = rospy.get_param('~publish_uart_state', True)
         self._publish_wifi_corrections_received = rospy.get_param('~publish_wifi_corrections_received', 
                                                                   True)
+        self._base_station_ip_for_latency_estimation = rospy.get_param(
+                                                            '~base_station_ip_for_latency_estimation',
+                                                            '10.10.50.1')
 
         # Generate publisher and callback function for navsatfix messages
         self._pub_rtk_float = rospy.Publisher(rospy.get_name() + '/navsatfix_rtk_float',
@@ -191,19 +195,56 @@ def __init__(self):
             self._pub_piksi_uart_state = rospy.Publisher(rospy.get_name() + '/debug/uart_state',
                                                          msg_uart_state, queue_size=10)
 
-        # corrections over wifi
+        # corrections over wifi message, if we are not the base station
         self._num_wifi_corrections = PiksiNumCorrections()
         self._num_wifi_corrections.header.seq = 0
         self._num_wifi_corrections.received_corrections = 0
-        if self._publish_wifi_corrections_received:
+        self._num_wifi_corrections.latency = -1
+        if self._publish_wifi_corrections_received and not self._base_station_mode:
             self._pub_piksi_wifi_corrections = rospy.Publisher(rospy.get_name() + '/debug/wifi_corrections',
                                                                PiksiNumCorrections, queue_size=10)
+            # start new thread to periodically ping base station
+            threading.Thread(target=self.ping_base_station_over_wifi).start()
 
         self._handler.start()
 
         # Spin
         rospy.spin()
 
+    def ping_base_station_over_wifi(self):
+        """
+        Ping base station pereiodically without blocking the driver
+        """
+
+        ping_deadline_seconds = 3
+        interval_between_pings_seconds = 5
+
+        while not rospy.is_shutdown():
+            # send ping command
+            command = ["ping",
+                       "-w", str(ping_deadline_seconds), # deadline before stopping attempt
+                       "-c", "1", # number of pings to send
+                       self._base_station_ip_for_latency_estimation]
+            ping = subprocess.Popen(command, stdout = subprocess.PIPE)
+
+            out, error = ping.communicate()
+            # search for 'min/avg/max/mdev' rount trip delay time (rtt) numbers
+            matcher = re.compile("(\d+.\d+)/(\d+.\d+)/(\d+.\d+)/(\d+.\d+)")
+
+            if matcher.search(out) == None:
+                # no ping response within ping_deadline_seconds
+                # in python write and read operations on built-in type are atomic.
+                # there's no need to use mutex
+                self._num_wifi_corrections.latency = -1
+            else:
+                groups_rtt = matcher.search(out).groups()
+                avg_rtt = groups_rtt[1]
+                # in python write and read operations on built-in type are atomic.
+                # there's no need to use mutex
+                self._num_wifi_corrections.latency = float(avg_rtt)
+
+            time.sleep(interval_between_pings_seconds)
+
     def make_callback(self, sbp_type, ros_message, pub, attrs):
         """
         Dynamic generator for callback functions for message types from
@@ -268,6 +309,7 @@ def multicast_callback(self, msg, **metadata):
         if self._framer:
             self._framer(msg, **metadata)
 
+            # publish debug message about wifi corrections, if enabled
             if self._publish_wifi_corrections_received:
                 self._num_wifi_corrections.header.seq += 1
                 now = rospy.get_rostime()