[skinManager] made it compliant with the new skinDynLib configuration…

… files. It can now work with both files.

src/libraries/skinDynLib/CMakeLists.txt

@@ -25,7 +25,7 @@ set(folder_header   include/iCub/skinDynLib/skinContact.h
                     include/iCub/skinDynLib/dynContact.h
                     include/iCub/skinDynLib/dynContactList.h
                     include/iCub/skinDynLib/common.h
-	               include/iCub/skinDynLib/rpcSkinManager.h 
+                    include/iCub/skinDynLib/rpcSkinManager.h 
                     include/iCub/skinDynLib/utils.h
                     include/iCub/skinDynLib/Taxel.h
                     include/iCub/skinDynLib/skinPart.h
@@ -47,4 +47,4 @@ icub_export_library(${PROJECTNAME} INTERNAL_INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/i
                                    DESTINATION include/iCub/skinDynLib
                                    FILES ${folder_header})
 
-								   
+

src/modules/skinManager/include/iCub/skinManager/compensator.h

@@ -40,6 +40,7 @@
 #include "iCub/skinDynLib/skinContact.h"
 #include "iCub/skinDynLib/skinContactList.h"
 #include "iCub/skinDynLib/rpcSkinManager.h"
+#include "iCub/skinDynLib/utils.h"
 
 using namespace std;
 using namespace yarp::os; 
@@ -153,6 +154,7 @@ class Compensator
     bool setContactCompensationGain(double gain);
     bool setMaxNeighborDistance(double d);
     bool setTaxelPosesFromFile(const char *filePath);
+    bool setTaxelPosesFromFileOld(const char *filePath);
     bool setTaxelPoses(const vector<Vector> &poses);
     bool setTaxelPose(unsigned int taxelId, const Vector &pose);
     bool setTaxelPositions(const Vector &positions);

src/modules/skinManager/src/compensationThread.cpp

@@ -137,7 +137,7 @@ bool CompensationThread::threadInit()
     skinEventsOn = false;
 	Bottle &skinEventsConf = rf->findGroup("SKIN_EVENTS");
 	if(!skinEventsConf.isNull()){
-        cout<< "SKIN_EVENTS section found\n";
+        yDebug("SKIN_EVENTS section found");
         string eventPortName = "/" + moduleName + "/skin_events:o";  // output skin events
 	    if(!skinEventsPort.open(eventPortName.c_str()))
             sendErrorMsg("Unable to open port "+eventPortName);
@@ -154,7 +154,7 @@ bool CompensationThread::threadInit()
             }else{
                 FOR_ALL_PORTS(i){
                     // cout<< "Skin part "<< SkinPart_s[skinPartList->get(i).asInt()]<< endl;
-                    yInfo("Skin part %s",SkinPart_s[skinPartList->get(i).asInt()].c_str());
+                    yDebug("Skin part %s",SkinPart_s[skinPartList->get(i).asInt()].c_str());
                     compensators[i]->setSkinPart((SkinPart)skinPartList->get(i).asInt());
                 }
             }
@@ -173,7 +173,7 @@ bool CompensationThread::threadInit()
             else{
                 maxNeighDist = skinEventsConf.check("maxNeighborDist", Value(MAX_NEIGHBOR_DISTANCE)).asDouble();
 
-                printf("Max neighbor distance: %f m\n", maxNeighDist);
+                yInfo("Max neighbor distance: %f m\n", maxNeighDist);
                 FOR_ALL_PORTS(i){
                     if(compWorking[i]){
 	                    string taxelPosFile = taxelPosFiles->get(i).asString().c_str();

src/modules/skinManager/src/compensator.cpp

@@ -419,8 +419,8 @@ void Compensator::updateBaseline(){
 		baselines[j]    += change;
         mean_change     += change;
 
-//        if(j<5)
-//            printf("%d touch detected %s, change %.6f, gain: %.3f\n", j, touchDetected[j]?"yes":"no", change, touchDetected[j]?contactCompensationGain:compensationGain);
+    //        if(j<5)
+    //        printf("%d touch detected %s, change %.6f, gain: %.3f\n", j, touchDetected[j]?"yes":"no", change, touchDetected[j]?contactCompensationGain:compensationGain);
 
         if(baselines[j]<0){
             char* temp = new char[300];
@@ -707,6 +707,46 @@ bool Compensator::setMaxNeighborDistance(double d){
 }
 
 bool Compensator::setTaxelPosesFromFile(const char *filePath){
+    yarp::os::ResourceFinder rf;
+    rf.setVerbose(false);
+    rf.setDefaultContext("skinGui");   //overridden by --context parameter
+    rf.setDefaultConfigFile(filePath); //overridden by --from parameter
+    rf.configure(0,NULL);
+    rf.setVerbose(true);
+
+    yarp::os::Bottle &calibration = rf.findGroup("calibration");
+    if (calibration.isNull())
+    {
+        return setTaxelPosesFromFileOld(filePath);
+    }
+    else
+    {
+        poseSem.wait();
+        int size=calibration.size()-1;
+        skinDim=size;
+        taxelPos.resize(skinDim, zeros(3));
+        taxelOri.resize(skinDim, zeros(3));
+        taxelPoseConfidence.resize(skinDim, 0.0);
+        for (unsigned int i = 0; i < size; ++i)
+        {
+            if (i<taxelPos.size())
+            {
+                Vector taxelPosNrm(6,0.0);
+                taxelPosNrm = vectorFromBottle(*(calibration.get(i+1).asList()),0,6);
+                taxelPos[i] = taxelPosNrm.subVector(0,2);
+                taxelOri[i] = taxelPosNrm.subVector(3,5);
+            }
+            if(norm(taxelPos[i])>0.0)
+                taxelPoseConfidence[i] = 1.0;
+        }
+        computeNeighbors();
+        poseSem.post();
+    }
+
+    return true;
+}
+
+bool Compensator::setTaxelPosesFromFileOld(const char *filePath){
 	ifstream posFile;
 	posFile.open(filePath);	
 	if (!posFile.is_open())