diff --git a/CHANGELOG.md b/CHANGELOG.md index bed9742c52..2c1d17764f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,14 @@ # Changelog All notable changes to this project are documented in this file. +## [unreleased] +### Added +- Add a feature to reduce latency of `YarpLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/883) + +### Changed +### Fixed +### Removed + ## [0.19.0] - 2024-09-06 ### Added - Added Vector Collection Server for publishing information for real-time users in the YARPRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/796) diff --git a/devices/YarpRobotLoggerDevice/README.md b/devices/YarpRobotLoggerDevice/README.md index 75689e8e9a..7a1960092e 100644 --- a/devices/YarpRobotLoggerDevice/README.md +++ b/devices/YarpRobotLoggerDevice/README.md @@ -138,3 +138,28 @@ robot-log-visualizer Then, you can open the mat file generated by the logger and explore the logged data as in the following video: [robot-log-visualizer.webm](https://github.com/ami-iit/robot-log-visualizer/assets/16744101/3fd5c516-da17-4efa-b83b-392b5ce1383b) + +## How to reduce latency +If you are experiencing some latency in logged data, you can try to enable different real time scheduling strategies through the `real_time_scheduling_strategy` in the [yarp-robot-logger.xml](https://github.com/ami-iit/bipedal-locomotion-framework/blob/master/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml). For example: + +```xml + + + ergocub + 0.001 + early_wakeup_and_fifo +``` + +The available strategies are `none` (which is the default and means no strategy will be applied), `early_wakeup`, `fifo`, and `early_wakeup_and_fifo`. +The `early_wakeup` strategy makes the YarpLoggerDevice run thread wake up earlier and then busy wait until it is time to resume. +The `fifo` strategy increases the YarpLoggerDevice run thread priority and changes its scheduling policy to SCHED-FIFO. +The `early_wakeup_and_fifo` combines the `early_wakeup` and `fifo` strategies. + +Note that the `fifo` and `early_wakeup_and_fifo` strategies are only available for Linux. Moreover, you should run the `YarpLoggerDevice` with elevated privileges when deploying them. +For example: + +```console +sudo -E /yarprobotinterface --config launch-yarp-robot-logger.xml +``` + +with `` being the directory containing `yarprobotinterface`, that you can determine with the terminal command `which yarprobotinterface`. \ No newline at end of file diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 63f09af1db..41c677aa59 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -55,6 +55,8 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, private: std::chrono::nanoseconds m_previousTimestamp; std::chrono::nanoseconds m_acceptableStep{std::chrono::nanoseconds::max()}; + std::chrono::nanoseconds m_resumeTime{std::chrono::nanoseconds(0)}; + std::chrono::nanoseconds m_awakeningTime{std::chrono::microseconds(500)}; bool m_firstRun{true}; using ft_t = Eigen::Matrix; @@ -171,12 +173,23 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, bool m_streamCartesianWrenches{false}; bool m_streamFTSensors{false}; bool m_streamTemperatureSensors{false}; + + enum class RealTimeSchedulingStrategy + { + None, + EarlyWakeUp, + FIFO, + EarlyWakeUpAndFIFO, + }; + RealTimeSchedulingStrategy m_RealTimeSchedulingStrategy{RealTimeSchedulingStrategy::None}; std::vector m_textLoggingSubnames; std::vector m_codeStatusCmdPrefixes; std::mutex m_bufferManagerMutex; robometry::BufferManager m_bufferManager; + bool threadInit() final; + void lookForNewLogs(); void lookForExogenousSignals(); diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index ad60885224..0e51b68bb6 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -16,6 +16,11 @@ #include #include +#if defined(__linux__) +#include +#include +#endif + #include #include #include @@ -124,6 +129,51 @@ YarpRobotLoggerDevice::YarpRobotLoggerDevice() YarpRobotLoggerDevice::~YarpRobotLoggerDevice() = default; +bool YarpRobotLoggerDevice::threadInit() +{ + constexpr auto logPrefix = "[YarpRobotLoggerDevice::threadInit]"; + + if (m_RealTimeSchedulingStrategy == RealTimeSchedulingStrategy::None + || m_RealTimeSchedulingStrategy == RealTimeSchedulingStrategy::EarlyWakeUp) + { + return true; + } + + // Set the scheduling policy to SCHED_FIFO and priority +#if defined(__linux__) + // Get the native handle + pthread_t native_handle = pthread_self(); + + // Set thread scheduling parameters + sched_param param; + param.sched_priority = 80; + + // Set the scheduling policy to SCHED_FIFO and priority + int ret = pthread_setschedparam(native_handle, SCHED_FIFO, ¶m); + if (ret != 0) + { + log()->error("{} Failed to set scheduling policy, with error: {}", logPrefix, ret); + if (ret == EPERM) + { + log()->error("{} The calling thread does not have the appropriate privileges to set " + "the requested scheduling policy and parameters. Try to run the " + "YarpLoggerDevice with 'sudo -E'.", + logPrefix); + } + return false; + } else + { + log()->info("{} Scheduling policy set to SCHED_FIFO with priority {}", + logPrefix, + param.sched_priority); + return true; + } +#else + log()->warn("{} Real-time scheduling is not supported on this platform.", logPrefix); +#endif + return true; +} + bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) { @@ -146,9 +196,50 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) log()->info("{} Real time logging not activated", logPrefix); } + std::string realTimeSchedulingStrategy{"none"}; + if (!params->getParameter("real_time_scheduling_strategy", realTimeSchedulingStrategy)) + { + log()->info("{} The 'real_time_scheduling_strategy' parameter is not found. " + "YarpLoggerDevice will run without any real time strategy.", + logPrefix); + } + if (realTimeSchedulingStrategy == "none") + { + m_RealTimeSchedulingStrategy = RealTimeSchedulingStrategy::None; + } else if (realTimeSchedulingStrategy == "early_wakeup") + { + m_RealTimeSchedulingStrategy = RealTimeSchedulingStrategy::EarlyWakeUp; + } else if (realTimeSchedulingStrategy == "fifo") + { + m_RealTimeSchedulingStrategy = RealTimeSchedulingStrategy::FIFO; + } else if (realTimeSchedulingStrategy == "early_wakeup_and_fifo") + { + m_RealTimeSchedulingStrategy = RealTimeSchedulingStrategy::EarlyWakeUpAndFIFO; + } else + { + log()->error("{} The 'real_time_scheduling_strategy' parameter is not valid. Available " + "options are 'none', 'early_wakeup', 'fifo', 'early_wakeup_and_fifo'.", + logPrefix); + return false; + } + double devicePeriod{0.01}; if (params->getParameter("sampling_period_in_s", devicePeriod)) { + if (m_RealTimeSchedulingStrategy == RealTimeSchedulingStrategy::EarlyWakeUp + || m_RealTimeSchedulingStrategy == RealTimeSchedulingStrategy::EarlyWakeUpAndFIFO) + { + if (devicePeriod > m_awakeningTime.count() * 1e-9) + { + devicePeriod = devicePeriod - m_awakeningTime.count() * 1e-9; + } else + { + log()->error("{} The sampling period is smaller than the awakening time. Cannot " + "use the 'early_wakeup' strategy.", + logPrefix); + return false; + } + } this->setPeriod(devicePeriod); } @@ -1189,22 +1280,21 @@ void YarpRobotLoggerDevice::lookForExogenousSignals() continue; } - log()->info("[YarpRobotLoggerDevice::lookForExogenousSignals] Attempt to get the " - "metadata for the vectors collection signal named: {}", + log()->info("[YarpRobotLoggerDevice::lookForExogenousSignals] Attempt to get " + "the metadata for the vectors collection signal named: {}", name); if (!signal.client.getMetadata(signal.metadata)) { - log()->warn("[YarpRobotLoggerDevice::lookForExogenousSignals] Unable to get " - "the metadata for the signal named: {}. The exogenous signal will " - "not contain the metadata.", + log()->warn("[YarpRobotLoggerDevice::lookForExogenousSignals] Unable to " + "get the metadata for the signal named: {}. The exogenous " + "signal will not contain the metadata.", name); } } } signal.connected = connectionDone; - } }; @@ -1431,6 +1521,30 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit void YarpRobotLoggerDevice::run() { + + bool wait = (m_RealTimeSchedulingStrategy == RealTimeSchedulingStrategy::EarlyWakeUp + || m_RealTimeSchedulingStrategy == RealTimeSchedulingStrategy::EarlyWakeUpAndFIFO) + ? true + : false; + std::chrono::nanoseconds now = BipedalLocomotion::clock().now(); + + while (wait) + { + + if (now >= m_resumeTime) + { + wait = false; + } else + { + if ((m_resumeTime - now) > (m_awakeningTime / 2)) + { + // still have time to sleep + BipedalLocomotion::clock().sleepFor(m_awakeningTime / 10); + } + } + now = BipedalLocomotion::clock().now(); + } + auto logData = [this](const std::string& name, const auto& data, const double time) { m_bufferManager.push_back(data, time, name); std::string rtName = robotRtRootName + treeDelim + name; @@ -1718,12 +1832,11 @@ void YarpRobotLoggerDevice::run() yarp::os::Bottle* b = m_textLoggingPort.read(false); if (b != nullptr) { - msg = BipedalLocomotion::TextLoggingEntry::deserializeMessage(*b, - std::to_string(time)); + msg = BipedalLocomotion::TextLoggingEntry::deserializeMessage(*b, std::to_string(time)); if (msg.isValid) { signalFullName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName - + "::p" + msg.processPID; + + "::p" + msg.processPID; // matlab does not support the character - as a key of a struct findAndReplaceAll(signalFullName, "-", "_"); @@ -1736,7 +1849,7 @@ void YarpRobotLoggerDevice::run() m_bufferManager.addChannel({signalFullName, {1, 1}}); m_textLogsStoredInManager.insert(signalFullName); } - //Not using logData here because we don't want to stream the data to RT + // Not using logData here because we don't want to stream the data to RT m_bufferManager.push_back(msg, time, signalFullName); } bufferportSize = m_textLoggingPort.getPendingReads(); @@ -1753,6 +1866,9 @@ void YarpRobotLoggerDevice::run() m_previousTimestamp = t; m_firstRun = false; + + m_resumeTime = t + std::chrono::nanoseconds(static_cast(this->getPeriod() * 1e9)) + + m_awakeningTime; } bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName,