From fdc5efdf6942248553fae0c7809834c7145fc8c9 Mon Sep 17 00:00:00 2001
From: Romain Reignier <rre@robopec.com>
Date: Thu, 4 Jan 2018 09:39:40 +0100
Subject: [PATCH] Add a latency property to get the latency of the connection

---
 src/ros_lib/node_handle.cpp   | 20 +++++++++++++++++++-
 src/ros_lib/ros/node_handle.h |  5 +++++
 2 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/src/ros_lib/node_handle.cpp b/src/ros_lib/node_handle.cpp
index a7299ae..d987aab 100644
--- a/src/ros_lib/node_handle.cpp
+++ b/src/ros_lib/node_handle.cpp
@@ -46,7 +46,8 @@ ros::NodeHandle::NodeHandle(uint32_t input_size, uint32_t output_size, QObject *
     INPUT_SIZE(input_size),
     OUTPUT_SIZE(output_size),
     configured_(false),
-    isActive_(false)
+    isActive_(false),
+    latency_(NAN)
 {
     message_in.resize(INPUT_SIZE);
     message_out.resize(OUTPUT_SIZE);
@@ -233,6 +234,8 @@ void ros::NodeHandle::requestSyncTime()
 
 void ros::NodeHandle::syncTime(uint8_t *data)
 {
+    uint32_t latency_ms = hardware_.time() - rt_time;
+
     std_msgs::Time t;
     uint32_t offset = hardware_.time() - rt_time;
 
@@ -242,6 +245,10 @@ void ros::NodeHandle::syncTime(uint8_t *data)
 
     this->setNow(t.data);
     last_sync_receive_time = hardware_.time();
+
+    // update latency property
+    latency_ = latency_ms/1000.;
+    emit latencyChanged(latency_);
 }
 
 ros::Time ros::NodeHandle::now()
@@ -427,6 +434,11 @@ bool ros::NodeHandle::getParam(const std::string &name, std::string *param, unsi
     return false;
 }
 
+float NodeHandle::latency() const
+{
+    return latency_;
+}
+
 void NodeHandle::setConfigured(bool isConnected)
 {
     if (configured_ == isConnected)
@@ -434,4 +446,10 @@ void NodeHandle::setConfigured(bool isConnected)
 
     configured_ = isConnected;
     emit isConnectedChanged(configured_);
+
+    if(!isConnected)
+    {
+        latency_ = NAN;
+        emit latencyChanged(latency_);
+    }
 }
diff --git a/src/ros_lib/ros/node_handle.h b/src/ros_lib/ros/node_handle.h
index 80e9f3c..ba2fbeb 100644
--- a/src/ros_lib/ros/node_handle.h
+++ b/src/ros_lib/ros/node_handle.h
@@ -47,6 +47,7 @@
 namespace ros {
 
 class NodeHandleBase_ : public QObject{
+  Q_OBJECT
 public:
   NodeHandleBase_(QObject *parent=0) : QObject(parent){}
 
@@ -72,6 +73,7 @@ class NodeHandle : public NodeHandleBase_
   Q_OBJECT  
 
   Q_PROPERTY(bool isConnected READ connected NOTIFY isConnectedChanged)
+  Q_PROPERTY(float latency READ latency NOTIFY latencyChanged)
 
 protected:
   RosQtSocket hardware_;
@@ -115,6 +117,7 @@ class NodeHandle : public NodeHandleBase_
 
   bool configured_;
   bool isActive_; // true if we should try to connect
+  float latency_;
 
   /* used for syncing the time */
   uint32_t last_sync_time;
@@ -228,9 +231,11 @@ private slots:
   bool getParam(const std::string &name, float* param, unsigned int length=1, int timeout = 1000);
   bool getParam(const std::string &name, std::string *param, unsigned int length=1, int timeout = 1000);
   bool isConnected() const;
+  float latency() const;
 
 signals:
   void isConnectedChanged(bool isConnected);
+  void latencyChanged(float latency);
 };
 
 }