Write log in binary format and add executable to parse it.

CMakeLists.txt

@@ -126,6 +126,9 @@ pkg_config_use_dependency(rcsot_controller urdfdom)
 ## Mark executables and/or libraries for installation
 install(TARGETS rcsot_controller DESTINATION lib )
 
+ADD_EXECUTABLE(roscontrol_sot_parse_log
+  src/roscontrol_sot_parse_log.cc)
+install(TARGETS roscontrol_sot_parse_log DESTINATION bin )
 
 foreach(dir config launch)
   install(DIRECTORY ${dir}

src/log.cpp

@@ -10,6 +10,8 @@
 #include <iostream>
 #include <iomanip>
 
+#include<ros/console.h>
+
 using namespace std;
 using namespace rc_sot_system;
 
@@ -159,35 +161,50 @@ void Log::save(std::string &fileName)
 
 }
 
+inline void writeHeaderToBinaryBuffer (ofstream& of,
+    const unsigned int& nVector,
+    const unsigned int& vectorSize)
+{
+  of.write ((char*)&nVector   , sizeof(unsigned int));
+  of.write ((char*)&vectorSize, sizeof(unsigned int));
+}
+
+inline void writeToBinaryFile (ofstream& of,
+    const double& t, const double& dt,
+    const std::vector<double>& data, const std::size_t& idx, const std::size_t& size)
+{
+  of.write ((char*)&t          ,       sizeof(double));
+  of.write ((char*)&dt         ,       sizeof(double));
+  of.write ((char*)(&data[idx]), size*(sizeof(double)));
+}
+
 void Log::saveVector(std::string &fileName,std::string &suffix,
-		     std::vector<double> &avector,
+		     const std::vector<double> &avector,
 		     unsigned int size)
 {
   ostringstream oss;
   oss << fileName;
   oss << suffix.c_str();
   std::string actualFileName= oss.str();
-  ofstream aof(actualFileName.c_str());
-  aof << std::setprecision(12) << std::setw(12) << std::setfill('0');
+
+  ofstream aof(actualFileName.c_str(), std::ios::binary | std::ios::trunc);
+
+  std::size_t idx = 0;
+  double dt;
   if (aof.is_open())
     {
+      writeHeaderToBinaryBuffer (aof, length_, size+2);
       for(unsigned long int i=0;i<length_;i++)
 	{
-	  // Save timestamp
-	  aof << StoredData_.timestamp[i] << " " ;
-
 	  // Compute and save dt
 	  if (i==0)
-	    aof << StoredData_.timestamp[i] - StoredData_.timestamp[length_-1] << " ";
+            dt = StoredData_.timestamp[i] - StoredData_.timestamp[length_-1];
 	  else
-	    aof << StoredData_.timestamp[i] - StoredData_.timestamp[i-1] << " ";
-
-	  // Save all data
-	  for(unsigned long int datumID=0;datumID<size;datumID++)
-	    aof << avector[i*size+datumID] << " " ;
-
-	  aof << std::endl;
+            dt = StoredData_.timestamp[i] - StoredData_.timestamp[i-1];
+          writeToBinaryFile (aof, StoredData_.timestamp[i], dt, avector, idx, size);
+          idx += size;
 	}
       aof.close();
+      ROS_INFO_STREAM("Wrote log file " << actualFileName);
     }
 }

src/log.hh

@@ -70,7 +70,7 @@ namespace rc_sot_system {
     // Save one vector of information.
     void saveVector(std::string &filename, 
 		    std::string &suffix,
-		    std::vector<double> &avector,
+		    const std::vector<double> &avector,
 		    unsigned int);
 
   public:

src/roscontrol_sot_parse_log.cc

+#include <fstream>
+#include <iostream>
+#include <iomanip>
+
+int main (int argc, char* argv[])
+{
+  if (argc != 2) {
+    std::cerr << "Usage: " << argv[0] << " binary_file_name\n";
+    return 1;
+  }
+
+  std::ifstream in (argv[1], std::ios::binary);
+  if (!in.is_open() || !in.good()) {
+    std::cerr << "Couldn't open file " << argv[1] << '\n';
+    return 2;
+  }
+
+  // Read headers
+  unsigned int nVector = 0, vectorSize = 0;
+  in.read ((char*)&nVector   , sizeof(unsigned int));
+  in.read ((char*)&vectorSize, sizeof(unsigned int));
+  if (!in.good()) {
+    std::cerr << "Couldn't parse file: " << argv[1] << '\n';
+    return 3;
+  }
+
+  // Read datas
+  double v;
+  std::cout << std::setprecision(12) << std::setw(12) << std::setfill('0');
+
+  for (std::size_t i=0; i < nVector; ++i) {
+    for (std::size_t j=0; j < vectorSize; ++j) {
+      in.read ((char*)&v, sizeof(double));
+      if (!in.good()) {
+        std::cerr << "Stopped to parse at (" << i << ',' << j
+          << ") of file: " << argv[1] << '\n';
+        return 4;
+      }
+      std::cout << v << ' ';
+    }
+    std::cout << '\n';
+  }
+  return 0;
+}