From fb23d1d02f4053fb973a0e4ee51009aca23e31b5 Mon Sep 17 00:00:00 2001
From: Ugo Pattacini <ugo.pattacini@iit.it>
Date: Wed, 25 Sep 2024 09:37:53 +0200
Subject: [PATCH] Fix fingers calibration in ARE (#986)

---
 .../include/iCub/perception/sensors.h            |  1 +
 src/libraries/perceptiveModels/src/sensors.cpp   |  4 ++++
 .../actionsRenderingEngine/src/MotorThread.cpp   | 16 ++++++++++------
 3 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/src/libraries/perceptiveModels/include/iCub/perception/sensors.h b/src/libraries/perceptiveModels/include/iCub/perception/sensors.h
index 75653cb197..43971111f1 100644
--- a/src/libraries/perceptiveModels/include/iCub/perception/sensors.h
+++ b/src/libraries/perceptiveModels/include/iCub/perception/sensors.h
@@ -204,6 +204,7 @@ class SensorEncoderArrays : public Sensor
     int num_arrays;
     int index_array;
     int index_element;
+    mutable yarp::os::Value latch;
 
 public:
     /**
diff --git a/src/libraries/perceptiveModels/src/sensors.cpp b/src/libraries/perceptiveModels/src/sensors.cpp
index 5e81155a86..54888b2525 100644
--- a/src/libraries/perceptiveModels/src/sensors.cpp
+++ b/src/libraries/perceptiveModels/src/sensors.cpp
@@ -101,12 +101,16 @@ bool SensorEncoderArrays::getOutput(Value &in) const
         return false;
 
     if (iencarray->getEncoderArrayStatus(index_array)!=MAS_OK)
+    {
+        in=latch;
         return false;
+    }
 
     double stamp;
     Vector vect(iencarray->getEncoderArraySize(index_array));
     iencarray->getEncoderArrayMeasure(index_array,vect,stamp);
     in=Value(vect[index_element]);
+    latch=in;
 
     return true;
 }
diff --git a/src/modules/actionsRenderingEngine/src/MotorThread.cpp b/src/modules/actionsRenderingEngine/src/MotorThread.cpp
index 982a377779..f873553c13 100644
--- a/src/modules/actionsRenderingEngine/src/MotorThread.cpp
+++ b/src/modules/actionsRenderingEngine/src/MotorThread.cpp
@@ -17,6 +17,7 @@
 */
 
 
+#include <filesystem>
 #include <fstream>
 #include <sstream>
 #include <cmath>
@@ -639,9 +640,9 @@ bool MotorThread::loadKinematicOffsets()
 
 bool MotorThread::saveKinematicOffsets()
 {
-    string fileName=rf.getHomeContextPath();
-    fileName+="/"+kinematics_file;
-    ofstream kin_fout(fileName.c_str());
+    std::filesystem::path fileName(rf.getHomeContextPath()+"/"+kinematics_file);
+    std::filesystem::create_directories(fileName.parent_path());
+    ofstream kin_fout(fileName);
     if (!kin_fout.is_open())
     {
         yError("Unable to open file '%s'!",fileName.c_str());
@@ -2632,7 +2633,9 @@ bool MotorThread::calibFingers(Bottle &options)
             graspModel[arm]->calibrate(prop);
 
             ofstream fout;
-            fout.open((rf.getHomeContextPath()+"/"+graspFile[arm]).c_str());
+            std::filesystem::path fileName(rf.getHomeContextPath()+"/"+graspFile[arm]);
+            std::filesystem::create_directories(fileName.parent_path());
+            fout.open(fileName);
             graspModel[arm]->toStream(fout);
             fout.close();
 
@@ -2988,9 +2991,10 @@ bool MotorThread::suspendLearningModeAction(Bottle &options)
     {
         if (!actions_path.empty())
         {
-            string fileName=rf.getHomeContextPath();
+            std::filesystem::path fileName=rf.getHomeContextPath();
             fileName+="/"+actions_path+"/"+arm_name+"/"+dragger.actionName+".action";
-            ofstream action_fout(fileName.c_str());
+            std::filesystem::create_directories(fileName.parent_path());
+            ofstream action_fout(fileName);
             if(!action_fout.is_open())
             {
                 yError("Unable to open file '%s' for action %s!",(actions_path+"/"+arm_name+"/"+dragger.actionName+".action").c_str(),dragger.actionName.c_str());