Check if the encoders timestamps are consistent

robotology · May 18, 2022 · 5ca0b54 · 5ca0b54
1 parent 225ed49
commit 5ca0b54
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 3 deletions.
diff --git a/src/devices/ControlBoardWrapper/ControlBoard_nws_ros.cpp b/src/devices/ControlBoardWrapper/ControlBoard_nws_ros.cpp
@@ -16,6 +16,7 @@
 #include "ControlBoardLogComponent.h"
 
 #include <numeric>
+#include <math.h>
 
 using namespace yarp::os;
 using namespace yarp::dev;
@@ -72,7 +73,7 @@ bool ControlBoard_nws_ros::open(Searchable& config)
     }
 
     // Check if we need to create subdevice or if they are
-    // passed later on thorugh attachAll()
+    // passed later on through attachAll()
     if (prop.check("subdevice")) {
         prop.setMonitor(config.getMonitor());
         if (!openAndAttachSubDevice(prop)) {
@@ -310,6 +311,17 @@ void ControlBoard_nws_ros::run()
         YARP_UNUSED(torqueOk);
     }
 
+    // Check if the encoders timestamps are consistent.
+    double tt = *times.data();
+    for (auto it = times.begin(); it != times.end(); it++)
+    {
+        if (fabs(tt - *it) > 1.0)
+        {
+            yCError(CONTROLBOARD, "Encoder Timestamps are not consistent! Data will not be published.");
+            return;
+        }
+    }
+
     // Update the port envelope time by averaging all timestamps
     time.update(std::accumulate(times.begin(), times.end(), 0.0) / subdevice_joints);
     yarp::os::Stamp averageTime = time;

diff --git a/src/devices/ControlBoardWrapper/ControlBoard_nws_yarp.cpp b/src/devices/ControlBoardWrapper/ControlBoard_nws_yarp.cpp
@@ -14,7 +14,7 @@
 #include <yarp/dev/impl/jointData.h>
 
 #include <numeric>
-
+#include <math.h>
 
 using namespace yarp::os;
 using namespace yarp::dev;
@@ -112,7 +112,7 @@ bool ControlBoard_nws_yarp::open(Searchable& config)
     }
 
     // Check if we need to create subdevice or if they are
-    // passed later on thorugh attachAll()
+    // passed later on through attachAll()
     if (prop.check("subdevice")) {
         prop.setMonitor(config.getMonitor());
         if (!openAndAttachSubDevice(prop)) {
@@ -440,6 +440,17 @@ void ControlBoard_nws_yarp::run()
         yCWarning(CONTROLBOARD) << "Number of streaming input messages to be read is " << inputStreamingPort.getPendingReads() << " and can overflow";
     }
 
+    // Check if the encoders timestamps are consistent.
+    double tt = *times.data();
+    for (auto it = times.begin(); it != times.end(); it++)
+    {
+        if (fabs(tt - *it) > 1.0)
+        {
+            yCError(CONTROLBOARD, "Encoder Timestamps are not consistent! Data will not be published.");
+            return;
+        }
+    }
+
     // Update the port envelope time by averaging all timestamps
     time.update(std::accumulate(times.begin(), times.end(), 0.0) / subdevice_joints);
     yarp::os::Stamp averageTime = time;