From 35b45c8e4677bdde2f69e535b7938e109d069550 Mon Sep 17 00:00:00 2001 From: Lorenzo <lore.moretti94@gmail.com> Date: Mon, 2 Dec 2024 10:18:33 +0100 Subject: [PATCH 01/11] add rpc commands for YarpLoggerDevice --- devices/YarpRobotLoggerDevice/CMakeLists.txt | 6 ++++++ .../BipedalLocomotion/YarpRobotLoggerDevice.h | 13 +++++++++++- .../src/YarpRobotLoggerDevice.cpp | 21 +++++++++++++++++++ .../YarpRobotLoggerDeviceCommands.thrift | 14 +++++++++++++ 4 files changed, 53 insertions(+), 1 deletion(-) create mode 100644 devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift diff --git a/devices/YarpRobotLoggerDevice/CMakeLists.txt b/devices/YarpRobotLoggerDevice/CMakeLists.txt index 06f01941f5..5cdcb51263 100644 --- a/devices/YarpRobotLoggerDevice/CMakeLists.txt +++ b/devices/YarpRobotLoggerDevice/CMakeLists.txt @@ -34,6 +34,11 @@ if(FRAMEWORK_COMPILE_YarpRobotLoggerDevice) endif() endif() + add_bipedal_locomotion_yarp_thrift( + NAME YarpRobotLoggerDeviceCommands + THRIFT thrifts/YarpRobotLoggerDeviceCommands.thrift + INSTALLATION_FOLDER YarpRobotLoggerDevice) + # Warning: the <package> option of yarp_configure_plugins_installation should be different from the plugin name add_bipedal_yarp_device( NAME YarpRobotLoggerDevice @@ -55,6 +60,7 @@ if(FRAMEWORK_COMPILE_YarpRobotLoggerDevice) BipedalLocomotion::PerceptionInterfaceYarpImplementation BipedalLocomotion::TextLoggingYarpImplementation BipedalLocomotion::SystemYarpImplementation + BipedalLocomotion::YarpRobotLoggerDeviceCommands tiny-process-library::tiny-process-library CONFIGURE_PACKAGE_NAME yarp_robot_logger_device) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 63f09af1db..bdbf3c91e6 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -32,12 +32,15 @@ #include <BipedalLocomotion/YarpUtilities/VectorsCollectionClient.h> #include <BipedalLocomotion/YarpUtilities/VectorsCollectionServer.h> +#include <YarpRobotLoggerDeviceCommands.h> + namespace BipedalLocomotion { class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, public yarp::dev::IMultipleWrapper, - public yarp::os::PeriodicThread + public yarp::os::PeriodicThread, + public YarpRobotLoggerDeviceCommands { public: YarpRobotLoggerDevice(double period, @@ -177,6 +180,10 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::mutex m_bufferManagerMutex; robometry::BufferManager m_bufferManager; + const std::string m_rpcPortName{"/YarpLoggerDevice/commands/rpc:i"}; /**< name of Remote + Procedure Call port. */ + yarp::os::Port m_rpcPort; /**< Remote Procedure Call port. */ + void lookForNewLogs(); void lookForExogenousSignals(); @@ -254,6 +261,10 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::string robotDescriptionList = "description_list"; const std::string timestampsName = "timestamps"; + + virtual bool lastCallSave(); + + virtual bool periodicSave(); }; } // namespace BipedalLocomotion diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index ad60885224..2ad467f4c1 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -394,6 +394,13 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) return false; } + this->yarp().attachAsServer(this->m_rpcPort); + if (!m_rpcPort.open(m_rpcPortName)) + { + log()->error("{} Could not open", logPrefix); + return false; + } + return true; } @@ -1998,3 +2005,17 @@ bool YarpRobotLoggerDevice::close() return true; } + +bool YarpRobotLoggerDevice::lastCallSave() +{ + std::string fileName; + m_bufferManager.saveToFile(fileName); + return this->saveCallback(fileName, robometry::SaveCallbackSaveMethod::last_call); +} + +bool YarpRobotLoggerDevice::periodicSave() +{ + std::string fileName; + m_bufferManager.saveToFile(fileName); + return this->saveCallback(fileName, robometry::SaveCallbackSaveMethod::periodic); +} diff --git a/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift b/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift new file mode 100644 index 0000000000..94febeedca --- /dev/null +++ b/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift @@ -0,0 +1,14 @@ +/** + * @file YarpRobotLoggerDeviceCommands.thrift + * @authors Lorenzo Moretti <lorenzo.moretti@iit.it> + * @copyright 2024 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2024 + */ + +service YarpRobotLoggerDeviceCommands +{ + bool lastCallSave(); + + bool periodicSave(); +} From a11a9f44e7ec62801366dc6418b33ef72516178c Mon Sep 17 00:00:00 2001 From: Lorenzo <lore.moretti94@gmail.com> Date: Mon, 2 Dec 2024 10:24:59 +0100 Subject: [PATCH 02/11] update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 112f02c0fb..fbb58868c3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ All notable changes to this project are documented in this file. - Implement `velMANN` class to perform inference on MANN model with velocity-based features in `ML` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/889) - Implement `velMANNAutoregressive`, `velMANNAutoregressiveInputBuilder`, and `velMANNTrajectoryGenerator` to generate trajectories using MANN model with velocity-based features in `ML` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/889) - Added option `FRAMEWORK_COMPILE_Ros1Publisher` to enable or disable the compilation of the `BipedalLocomotion::YarpUtilities::RosPublisher` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/914) +- Added rpc commands to `YarpLoggerDevice` to trigger the saving of data to a local file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/915) ### Changed - Change device jtcvc to use motor velocity and joint velocity in input to PINN models (https://github.com/ami-iit/bipedal-locomotion-framework/pull/903) From ce708e882a14af013acc05b773a9dd794339a47b Mon Sep 17 00:00:00 2001 From: Lorenzo <lore.moretti94@gmail.com> Date: Mon, 2 Dec 2024 17:46:59 +0100 Subject: [PATCH 03/11] implement a unique rpc command --- .../include/BipedalLocomotion/YarpRobotLoggerDevice.h | 4 +--- .../YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 9 +-------- .../thrifts/YarpRobotLoggerDeviceCommands.thrift | 4 +--- 3 files changed, 3 insertions(+), 14 deletions(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index bdbf3c91e6..003257ddf2 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -262,9 +262,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, const std::string timestampsName = "timestamps"; - virtual bool lastCallSave(); - - virtual bool periodicSave(); + virtual bool saveData(); }; } // namespace BipedalLocomotion diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 2ad467f4c1..2c9a52cd18 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -2006,14 +2006,7 @@ bool YarpRobotLoggerDevice::close() return true; } -bool YarpRobotLoggerDevice::lastCallSave() -{ - std::string fileName; - m_bufferManager.saveToFile(fileName); - return this->saveCallback(fileName, robometry::SaveCallbackSaveMethod::last_call); -} - -bool YarpRobotLoggerDevice::periodicSave() +bool YarpRobotLoggerDevice::saveData() { std::string fileName; m_bufferManager.saveToFile(fileName); diff --git a/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift b/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift index 94febeedca..df4a7e1f09 100644 --- a/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift +++ b/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift @@ -8,7 +8,5 @@ service YarpRobotLoggerDeviceCommands { - bool lastCallSave(); - - bool periodicSave(); + bool saveData(); } From 67e2f0f1f20a6d15af0a3ccb22354af27a586ee6 Mon Sep 17 00:00:00 2001 From: Lorenzo Moretti <107630048+LoreMoretti@users.noreply.github.com> Date: Mon, 2 Dec 2024 17:58:49 +0100 Subject: [PATCH 04/11] Update devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift Co-authored-by: Silvio Traversaro <silvio@traversaro.it> --- .../thrifts/YarpRobotLoggerDeviceCommands.thrift | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift b/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift index df4a7e1f09..d28cf57b40 100644 --- a/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift +++ b/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift @@ -2,7 +2,7 @@ * @file YarpRobotLoggerDeviceCommands.thrift * @authors Lorenzo Moretti <lorenzo.moretti@iit.it> * @copyright 2024 iCub Facility - Istituto Italiano di Tecnologia - * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * Released under the terms of the BSD-3-Clause license. * @date 2024 */ From ae1b79103ce47107d0ff02ab8e4a1876d3fbee93 Mon Sep 17 00:00:00 2001 From: Lorenzo <lore.moretti94@gmail.com> Date: Tue, 3 Dec 2024 11:54:05 +0100 Subject: [PATCH 05/11] add port prefix to rpc port name --- .../src/YarpRobotLoggerDevice.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 2c9a52cd18..fe5e3f05f2 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -394,8 +394,19 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) return false; } + // open rpc port for YarpRobotLoggerDevice commands + std::string portPrefix{}; + + if (!params->getParameter("port_prefix", portPrefix)) + { + + log()->info("{} The 'port_prefix' is not provided. It willnot be used.", logPrefix); + } + + std::string rpcPortFullName = portPrefix + m_rpcPortName; + this->yarp().attachAsServer(this->m_rpcPort); - if (!m_rpcPort.open(m_rpcPortName)) + if (!m_rpcPort.open(rpcPortFullName)) { log()->error("{} Could not open", logPrefix); return false; From 9c5e992653e0947e7394e8e711b2a8a628fe747f Mon Sep 17 00:00:00 2001 From: Lorenzo <lore.moretti94@gmail.com> Date: Tue, 3 Dec 2024 12:09:49 +0100 Subject: [PATCH 06/11] change rpc port name --- .../include/BipedalLocomotion/YarpRobotLoggerDevice.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 003257ddf2..c63e986086 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -180,7 +180,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::mutex m_bufferManagerMutex; robometry::BufferManager m_bufferManager; - const std::string m_rpcPortName{"/YarpLoggerDevice/commands/rpc:i"}; /**< name of Remote + const std::string m_rpcPortName{"/YarpRobotLoggerDevice/commands/rpc:i"}; /**< name of Remote Procedure Call port. */ yarp::os::Port m_rpcPort; /**< Remote Procedure Call port. */ From 152ee52a0ca69969b9f68b521b87d6a420b2b8dd Mon Sep 17 00:00:00 2001 From: Lorenzo Moretti <107630048+LoreMoretti@users.noreply.github.com> Date: Tue, 3 Dec 2024 13:50:49 +0100 Subject: [PATCH 07/11] Update devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h Co-authored-by: Stefano Dafarra <stefano.dafarra@gmail.com> --- .../include/BipedalLocomotion/YarpRobotLoggerDevice.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index c63e986086..598e543c09 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -180,7 +180,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, std::mutex m_bufferManagerMutex; robometry::BufferManager m_bufferManager; - const std::string m_rpcPortName{"/YarpRobotLoggerDevice/commands/rpc:i"}; /**< name of Remote + const std::string m_rpcPortName{"/commands/rpc:i"}; /**< name of Remote Procedure Call port. */ yarp::os::Port m_rpcPort; /**< Remote Procedure Call port. */ From e80cdad4288c7003bd0bb04f29b40f76c4793296 Mon Sep 17 00:00:00 2001 From: Lorenzo Moretti <107630048+LoreMoretti@users.noreply.github.com> Date: Wed, 4 Dec 2024 09:04:36 +0100 Subject: [PATCH 08/11] Update devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift Co-authored-by: Giulio Romualdi <giulio.romualdi@gmail.com> --- .../thrifts/YarpRobotLoggerDeviceCommands.thrift | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift b/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift index d28cf57b40..9d0c1ebc82 100644 --- a/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift +++ b/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift @@ -1,7 +1,7 @@ /** * @file YarpRobotLoggerDeviceCommands.thrift * @authors Lorenzo Moretti <lorenzo.moretti@iit.it> - * @copyright 2024 iCub Facility - Istituto Italiano di Tecnologia + * @copyright 2024 Artificial and Mechanical Intelligence - Istituto Italiano di Tecnologia * Released under the terms of the BSD-3-Clause license. * @date 2024 */ From 296f78662b7ac881d78cf3c673ac94b4245e30b5 Mon Sep 17 00:00:00 2001 From: Lorenzo <lore.moretti94@gmail.com> Date: Wed, 4 Dec 2024 09:10:18 +0100 Subject: [PATCH 09/11] add default port prefix --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index fe5e3f05f2..6d7aad33ad 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -395,7 +395,7 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) } // open rpc port for YarpRobotLoggerDevice commands - std::string portPrefix{}; + std::string portPrefix{"/yarp-robot-logger"}; if (!params->getParameter("port_prefix", portPrefix)) { From 7da6d5e18e3e053ddca9a71ccf9e3ffdc9c24f9a Mon Sep 17 00:00:00 2001 From: Lorenzo Moretti <107630048+LoreMoretti@users.noreply.github.com> Date: Wed, 4 Dec 2024 12:57:46 +0100 Subject: [PATCH 10/11] Update devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift Co-authored-by: Silvio Traversaro <silvio@traversaro.it> --- .../thrifts/YarpRobotLoggerDeviceCommands.thrift | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift b/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift index 9d0c1ebc82..fa6b12f4a9 100644 --- a/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift +++ b/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift @@ -1,7 +1,7 @@ /** * @file YarpRobotLoggerDeviceCommands.thrift * @authors Lorenzo Moretti <lorenzo.moretti@iit.it> - * @copyright 2024 Artificial and Mechanical Intelligence - Istituto Italiano di Tecnologia + * @copyright 2024 Fondazione Istituto Italiano di Tecnologia * Released under the terms of the BSD-3-Clause license. * @date 2024 */ From db018df80e5363a6ab336acdfffcdbad7283cbea Mon Sep 17 00:00:00 2001 From: Lorenzo <lore.moretti94@gmail.com> Date: Thu, 5 Dec 2024 10:01:35 +0100 Subject: [PATCH 11/11] Fix info string --- devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 6d7aad33ad..cdcc546654 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -400,7 +400,9 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) if (!params->getParameter("port_prefix", portPrefix)) { - log()->info("{} The 'port_prefix' is not provided. It willnot be used.", logPrefix); + log()->info("{} The 'port_prefix' is not provided. The default prefix {} will be used.", + logPrefix, + portPrefix); } std::string rpcPortFullName = portPrefix + m_rpcPortName;