Fix fingers calibration in ARE (#986)

robotology · Sep 25, 2024 · fb23d1d · fb23d1d
1 parent 75f2c94
commit fb23d1d
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 6 deletions.
diff --git 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
@@ -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
@@ -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());