Add Xsens saveCurrentCalibration option

CHANGELOG.md

@@ -9,7 +9,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Added
 - Added `iwear_logger` wrapper device to log wearable sensors data using `yarp-telemetry`. (https://github.com/robotology/wearables/pull/113)
 - Added `AddInstallRPATHSupport` cmake module for linking shared objects of private dependencies. (https://github.com/robotology/wearables/pull/113)
-- Updated `Paexo` wearable device with 6D force torque sensors implementation using `iFeelDriver`. (https://github.com/robotology/wearables/pull/117) 
+- Updated `Paexo` wearable device with 6D force torque sensors implementation using `iFeelDriver`. (https://github.com/robotology/wearables/pull/117)
+- Updated `Xsens` wearable device with `saveCurrentCalibration` option. (https://github.com/robotology/wearables/pull/120)
 
 ## [1.2.1] - 2021-04-12
 

XSensMVN/include/XSensMVNDriver.h

@@ -144,6 +144,7 @@ namespace xsensmvn {
         const bodyDimensions bodyDimensions;
         const DriverDataStreamConfig dataStreamConfiguration;
         const bool saveMVNRecording;
+        const bool saveCurrentCalibration;
     };
 
     enum class DriverStatus

XSensMVN/src/XSensMVNDriverImpl.cpp

@@ -659,7 +659,8 @@ bool XSensMVNDriverImpl::calibrate(const std::string calibrationType)
         // Record the success of the calibration
         m_driverStatus = DriverStatus::CalibratedAndReadyToRecord;
 
-        if(m_driverConfiguration.saveMVNRecording)
+        // Save calibration and .mvn record
+        if(m_driverConfiguration.saveMVNRecording || m_driverConfiguration.saveCurrentCalibration)
         {
             //Start recording .mvn file
             time_t rawtime;
@@ -676,13 +677,25 @@ bool XSensMVNDriverImpl::calibrate(const std::string calibrationType)
             std::replace(time_string.begin(), time_string.end(), ' ', '_'); //Replace space with underscore
             std::replace(time_string.begin(), time_string.end(), '-', '_'); //Replace - with underscore
             std::replace(time_string.begin(), time_string.end(), ':', '_'); //Replace : with underscore
-
-            const XsString mvnFileName("recording_" + time_string);
 
-            xsInfo << "Recording to " << mvnFileName.toStdString() << ".mvn file";
+            if(m_driverConfiguration.saveMVNRecording)
+            {
+                const XsString mvnFileName("recording_" + time_string);
 
-            m_connection->createMvnFile(mvnFileName);
-            m_connection->startRecording();
+                xsInfo << "Recording to " << mvnFileName.toStdString() << ".mvn file";
+
+                m_connection->createMvnFile(mvnFileName);
+                m_connection->startRecording();
+            }
+
+            if (m_driverConfiguration.saveCurrentCalibration)
+            {
+                const XsString mvnFileName("recording_" + time_string + "_calibration.mvn");
+
+                xsInfo << "Calibration saved to " << mvnFileName.toStdString() << " file";
+
+                m_connection->saveCurrentCalibration(mvnFileName);
+            }
         }
 
         m_calibrator->getLastCalibrationInfo(m_calibrationInfo.type, m_calibrationInfo.quality);

app/xml/XsensSuitWearableDevice.xml

@@ -18,15 +18,17 @@
         <!-- NposeWalk - TposeWalk - Npose - Tpose-->
         <param name="default-calibration-type">Npose</param>
         <!-- Minimum calibration quality to be considered good enought to be applied and allow the acquisition to start. Available values are: -->
-        <!-- Poor - Acceptable -- Good -->
-        <param name="minimum-calibration-quality-required">Acceptable</param>
+        <!-- Poor - Acceptable - Good -->
+        <param name="minimum-calibration-quality-required">Poor</param>
         <!-- Maximum Time [s] to scan for the suit. Set to -1 enables endless scan-->
         <param name="scan-timeout">60</param>
         <!-- Sampling rate [Hz]. Available values are:-->
         <!-- 240 - 120 - 80 - 60 -->
         <param name="sampling-rate">120</param>
         <!-- Flag to save mvn recording -->
         <param name="saveMVNRecording">false</param>
+	<!-- Flag to save .mvn calibration -->
+        <param name="saveCurrentCalibration">true</param>
         <!-- Quantities to be extracted from the driver for each time sample -->
         <group name="output-stream-configuration">
             <param name="enable-joint-data">true</param>
@@ -46,7 +48,7 @@
             <param name="shoeSoleHeight">0.02</param>
         </group>
     </device>
-    
+
     <device type="iwear_wrapper" name="XSensSuitDeviceWrapper">
         <param name="period">0.01</param>
         <param name="dataPortName">/XSensSuit/WearableData/data:o</param>
@@ -58,7 +60,7 @@
         </action>
         <action phase="shutdown" level="5" type="detach"/>
     </device>
-    
+
     <device type="ixsensmvncontrol_wrapper" name="XSensSuitControl">
         <param name="rpcPortName">/XSensSuit/Control/rpc:i</param>
         <action phase="startup" level="5" type="attach">
@@ -68,4 +70,4 @@
         </action>
         <action phase="shutdown" level="5" type="detach"/>
     </device>
-</robot> 
+</robot>

devices/XsensSuit/src/XsensSuit.cpp

@@ -765,12 +765,23 @@ bool XsensSuit::open(yarp::os::Searchable& config)
     // Check for mvn recording flag
     bool saveMVNRecording;
     if (!config.check("saveMVNRecording")) {
-        yWarning() << logPrefix << "OPTIONAL parameter <saveMVNRecording> NOT found, setting it to False.";
+        yWarning() << logPrefix << "OPTIONAL parameter <saveMVNRecording> NOT found, setting it to false.";
         saveMVNRecording = false;
     }
     else {
+        saveMVNRecording = config.find("saveMVNRecording").asBool();
         yInfo() << logPrefix << "<saveMVNRecording> parameter set to " << saveMVNRecording;
-        saveMVNRecording = config.find("saveMVNRecording").asBool();        
+    }
+
+    // Check for saving current calibration flag
+    bool saveCurrentCalibration;
+    if (!config.check("saveCurrentCalibration")) {
+        yWarning() << logPrefix << "OPTIONAL parameter <saveCurrentCalibration> NOT found, setting it to false.";
+        saveCurrentCalibration = false;
+    }
+    else {
+        saveCurrentCalibration = config.find("saveCurrentCalibration").asBool();
+        yInfo() << logPrefix << "<saveCurrentCalibration> parameter set to " << saveCurrentCalibration;
     }
 
     xsensmvn::DriverConfiguration driverConfig{rundepsFolder,
@@ -782,7 +793,8 @@ bool XsensSuit::open(yarp::os::Searchable& config)
                                                samplingRate,
                                                subjectBodyDimensions,
                                                outputStreamConfig,
-                                               saveMVNRecording};
+                                               saveMVNRecording,
+                                               saveCurrentCalibration};
 
     pImpl->driver.reset(new xsensmvn::XSensMVNDriver(driverConfig));