Add rpc commands to YarpLoggerDevice (#915)

ami-iit · Dec 5, 2024 · 40456d1 · 40456d1
1 parent ba2a98a
commit 40456d1
Show file tree

Hide file tree

Showing 5 changed files with 56 additions and 1 deletion.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -13,6 +13,7 @@ All notable changes to this project are documented in this file.
 - 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)
 - Add KF to estimate joint and motor velocities in JoinTorqueControlDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/909)
 - 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)

diff --git 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
@@ -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{"/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,8 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
     const std::string robotDescriptionList = "description_list";
 
     const std::string timestampsName = "timestamps";
+
+    virtual bool saveData();
 };
 
 } // namespace BipedalLocomotion

diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp
@@ -394,6 +394,26 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config)
         return false;
     }
 
+    // open rpc port for YarpRobotLoggerDevice commands
+    std::string portPrefix{"/yarp-robot-logger"};
+
+    if (!params->getParameter("port_prefix", portPrefix))
+    {
+
+        log()->info("{} The 'port_prefix' is not provided. The default prefix {} will be used.",
+                    logPrefix,
+                    portPrefix);
+    }
+
+    std::string rpcPortFullName = portPrefix + m_rpcPortName;
+
+    this->yarp().attachAsServer(this->m_rpcPort);
+    if (!m_rpcPort.open(rpcPortFullName))
+    {
+        log()->error("{} Could not open", logPrefix);
+        return false;
+    }
+
     return true;
 }
 
@@ -1998,3 +2018,10 @@ bool YarpRobotLoggerDevice::close()
 
     return true;
 }
+
+bool YarpRobotLoggerDevice::saveData()
+{
+    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
@@ -0,0 +1,12 @@
+/**
+ * @file YarpRobotLoggerDeviceCommands.thrift
+ * @authors Lorenzo Moretti <[email protected]>
+ * @copyright 2024 Fondazione Istituto Italiano di Tecnologia
+ *            Released under the terms of the BSD-3-Clause license.
+ * @date 2024
+ */
+
+service YarpRobotLoggerDeviceCommands
+{
+    bool saveData();
+}