Merge pull request #82 from robotology/feature/saveMvnRecording

Add the optional parameter to save .mvn recording of xsens suit through SDK

CMakeLists.txt

@@ -44,6 +44,8 @@ endif()
 
 if(WIN32)
  option(XSENS_MVN_USE_SDK "Build the driver and the wrapper for the real MVN system using the MVN SDK" ON)
+ #Add compile definition to suppress warning related to header <experimental/filesystem>
+ add_compile_definitions(_SILENCE_EXPERIMENTAL_FILESYSTEM_DEPRECATION_WARNING)
  if(XSENS_MVN_USE_SDK)
  find_package(XsensXME REQUIRED)
  if(NOT ${XsensXME_FOUND})

XSensMVN/include/XSensMVNDriver.h

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

XSensMVN/src/XSensMVNDriverImpl.cpp

@@ -12,6 +12,7 @@
 #include <experimental/filesystem>
 #include <map>
 #include <string>
+#include <ctime>
 
 #include <xme.h>
 
@@ -42,6 +43,16 @@ XSensMVNDriverImpl::XSensMVNDriverImpl(
 
 XSensMVNDriverImpl::~XSensMVNDriverImpl()
 {
+ if(m_driverConfiguration.saveMVNRecording)
+ {
+ // Save .mvn recording file
+ m_connection->stopRecording();
+ while(m_connection->status().isRecording() || m_connection->status().isFlushing()){
+ std::this_thread::sleep_for(std::chrono::microseconds(10));
+ }
+ m_connection->saveAndCloseFile();
+ }
+
  m_connection->removeCallbackHandler(static_cast<XmeCallback*>(this));
 }
 
@@ -647,6 +658,33 @@ bool XSensMVNDriverImpl::calibrate(const std::string calibrationType)
  if (calibrationCompleted) {
  // Record the success of the calibration
  m_driverStatus = DriverStatus::CalibratedAndReadyToRecord;
+
+ if(m_driverConfiguration.saveMVNRecording)
+ {
+ //Start recording .mvn file
+ time_t rawtime;
+ struct tm * timeinfo;
+ char buffer[80];
+
+ time(&rawtime);
+ timeinfo = localtime(&rawtime);
+
+ strftime(buffer, sizeof(buffer), "%d-%m-%Y %H:%M:%S", timeinfo);
+ std::string time_string(buffer);
+
+ // Replace separators with underscore
+ 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";
+
+ m_connection->createMvnFile(mvnFileName);
+ m_connection->startRecording();
+ }
+
  m_calibrator->getLastCalibrationInfo(m_calibrationInfo.type, m_calibrationInfo.quality);
  }
 

app/xml/XsensSuitWearableDevice.xml

@@ -25,6 +25,8 @@
  <!-- 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>
  <!-- Quantities to be extracted from the driver for each time sample -->
  <group name="output-stream-configuration">
  <param name="enable-joint-data">true</param>

devices/XsensSuit/src/XsensSuit.cpp

@@ -762,6 +762,17 @@ bool XsensSuit::open(yarp::os::Searchable& config)
  outputStreamConfig.enableSensorData =
  streamGroup.check("enable-sensor-data", yarp::os::Value(true)).asBool();
 
+ // Check for mvn recording flag
+ bool saveMVNRecording;
+ if (!config.check("saveMVNRecording")) {
+ yWarning() << logPrefix << "OPTIONAL parameter <saveMVNRecording> NOT found, setting it to False.";
+ saveMVNRecording = false;
+ }
+ else {
+ yInfo() << logPrefix << "<saveMVNRecording> parameter set to " << saveMVNRecording;
+ saveMVNRecording = config.find("saveMVNRecording").asBool(); 
+ }
+
  xsensmvn::DriverConfiguration driverConfig{rundepsFolder,
  suitConfiguration,
  acquisitionScenario,
@@ -770,7 +781,8 @@ bool XsensSuit::open(yarp::os::Searchable& config)
  scanTimeout,
  samplingRate,
  subjectBodyDimensions,
- outputStreamConfig};
+ outputStreamConfig,
+ saveMVNRecording};
 
  pImpl->driver.reset(new xsensmvn::XSensMVNDriver(driverConfig));