diff --git a/src/devices/ControlBoardWrapper/ControlBoard_nws_ros.cpp b/src/devices/ControlBoardWrapper/ControlBoard_nws_ros.cpp index bcc716e37b1..db617b8f0bf 100644 --- a/src/devices/ControlBoardWrapper/ControlBoard_nws_ros.cpp +++ b/src/devices/ControlBoardWrapper/ControlBoard_nws_ros.cpp @@ -16,6 +16,7 @@ #include "ControlBoardLogComponent.h" #include +#include 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 index 86182dbef6a..f9c83c4c81a 100644 --- a/src/devices/ControlBoardWrapper/ControlBoard_nws_yarp.cpp +++ b/src/devices/ControlBoardWrapper/ControlBoard_nws_yarp.cpp @@ -14,7 +14,7 @@ #include #include - +#include 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;