Add possibility to save data

robotology · Feb 22, 2024 · 46d8d1c · 46d8d1c
1 parent 9f3fc77
commit 46d8d1c
Show file tree

Hide file tree

Showing 3 changed files with 49 additions and 6 deletions.
diff --git a/src/imu/CMakeLists.txt b/src/imu/CMakeLists.txt
@@ -1,14 +1,16 @@
 add_definitions(-D_USE_MATH_DEFINES)
+find_package(robometry)
 
 robottestingframework_add_plugin(imu HEADERS imu.h
                                      SOURCES imu.cpp)
 
 # add required libraries
-target_link_libraries(imu RobotTestingFramework::RTF
-                                      RobotTestingFramework::RTF_dll
-                                      YARP::YARP_robottestingframework
-                                      iDynTree::idyntree-high-level 
-                                      iDynTree::idyntree-estimation)
+target_link_libraries(imu   RobotTestingFramework::RTF
+                            RobotTestingFramework::RTF_dll
+                            YARP::YARP_robottestingframework
+                            iDynTree::idyntree-high-level 
+                            iDynTree::idyntree-estimation
+                            robometry::robometry)
 # set the installation options
 install(TARGETS imu
         EXPORT imu

diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp
@@ -9,6 +9,9 @@
 #include <yarp/os/Stamp.h>
 #include <yarp/os/ResourceFinder.h>
 
+#include <robometry/BufferConfig.h>
+#include <robometry/BufferManager.h>
+
 #include "imu.h"
 
 using namespace robottestingframework;
@@ -144,6 +147,8 @@ bool Imu::setup(yarp::os::Property& property)
         gravity
     );
 
+    setupTelemetry();
+
     return true;
 }
 
@@ -252,13 +257,46 @@ bool Imu::moveJoint(int ax, double pos, int sensorIndex)
         errorTot.push_back(mag);
 
         sendData(expectedImuSignal.asRPY(), imuSignal.asRPY());
+
+        bufferManager.push_back(positions, "joints_state::positions");
+        bufferManager.push_back(velocities, "joints_state::velocities");
+        bufferManager.push_back({expectedImuSignal.asRPY()[0], expectedImuSignal.asRPY()[1], expectedImuSignal.asRPY()[2]}, "orientations::" + sensorName + "::expected");
+        bufferManager.push_back({imuSignal.asRPY()[0], imuSignal.asRPY()[1], imuSignal.asRPY()[2]}, "orientations::" + sensorName + "::measured");
+
         ipos->checkMotionDone(&done);
     }
 
     double maxError = *max_element(errorTot.begin(), errorTot.end());
+    bufferManager.push_back(maxError, "orientations::" + sensorName + "::error::" + axesVec[ax]);
 
     ROBOTTESTINGFRAMEWORK_TEST_CHECK(maxError < errorMean, Asserter::format("Testing %s for sensor %s: the max rotation angle error is %f rad!", axesVec[ax].c_str(), sensorName.c_str(), maxError));
     ipos->positionMove(ax, 0.0);
 
     return true;
-}
+}
+
+bool Imu::setupTelemetry()
+{
+    robometry::BufferConfig bufferConfig;
+    bufferConfig.auto_save = true;
+    bufferConfig.yarp_robot_name = std::getenv("YARP_ROBOT_NAME");
+    bufferConfig.filename = "test_imu";
+    bufferConfig.file_indexing = "%Y_%m_%d_%H_%M_%S";
+    bufferConfig.n_samples = 100000;
+
+    bufferManager.addChannel({"joints_state::positions", {axesVec.size(), 1}, axesVec});
+    bufferManager.addChannel({"joints_state::velocities", {axesVec.size(), 1}, axesVec});
+
+    for(auto it = sensorMap.begin(); it != sensorMap.end(); it++) 
+    {
+        bufferManager.addChannel({"orientations::" + it->first + "::expected", {3, 1}, {"r", "p", "y"}});
+        bufferManager.addChannel({"orientations::" + it->first + "::measured", {3, 1}, {"r", "p", "y"}});
+
+        for(int i = 0; i < sensorMap[it->first].size(); i ++)
+        {
+            bufferManager.addChannel({"orientations::" + it->first + "::error::" + sensorMap[it->first].get(i).toString(), {1, 1}, {"max_error"}});
+        }
+    }
+
+    return bufferManager.configure(bufferConfig);
+}
diff --git a/src/imu/imu.h b/src/imu/imu.h
@@ -85,8 +85,11 @@ class Imu : public yarp::robottestingframework::TestCase {
         iDynTree::Transform I_T_base;
         iDynTree::Rotation I_R_I_IMU;
 
+        robometry::BufferManager bufferManager;
+
         bool sendData(iDynTree::Vector3 expectedValues, iDynTree::Vector3 imuSignal);
         bool moveJoint(int ax, double pos, int sensIndex);
+        bool setupTelemetry();
     };
 
 #endif //IMU_H