From 2e1c4f41aed0aa71ad0e0fdd9bd8935cb2412e16 Mon Sep 17 00:00:00 2001 From: marco-tranzatto Date: Fri, 17 Feb 2017 12:14:39 +0100 Subject: [PATCH] estimate latency with base station using ping command over wifi --- cfg/piksi_driver_settings.yaml | 3 +- msg/PiksiNumCorrections.msg | 1 + src/piksi.py | 50 +++++++++++++++++++++++++++++++--- 3 files changed, 49 insertions(+), 5 deletions(-) diff --git a/cfg/piksi_driver_settings.yaml b/cfg/piksi_driver_settings.yaml index ce5abc4..903f108 100644 --- a/cfg/piksi_driver_settings.yaml +++ b/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,7 +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 +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) diff --git a/msg/PiksiNumCorrections.msg b/msg/PiksiNumCorrections.msg index 745cf73..6023fe7 100644 --- a/msg/PiksiNumCorrections.msg +++ b/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 diff --git a/src/piksi.py b/src/piksi.py index b99dc4b..47fd3f9 100755 --- a/src/piksi.py +++ b/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()