Fix logs and add test

  The duration of each iteration was not logged in the line corresponding to
  the other data. The bug has been fixed and a test has been added to check
  the good functioning of the circular buffer.

src/log.cpp

@@ -143,36 +143,36 @@ void Log::save(std::string &fileName) {
   assert(lref_ == lrefts_*profileLog_.nbDofs);
 
   std::string suffix("-mastate.log");
-  saveVector(fileName, suffix, StoredData_.motor_angle, profileLog_.nbDofs, lref_);
+  saveVector(fileName, suffix, StoredData_.motor_angle, profileLog_.nbDofs);
   suffix = "-jastate.log";
-  saveVector(fileName, suffix, StoredData_.joint_angle, profileLog_.nbDofs, lref_);
+  saveVector(fileName, suffix, StoredData_.joint_angle, profileLog_.nbDofs);
   suffix = "-vstate.log";
-  saveVector(fileName, suffix, StoredData_.velocities, profileLog_.nbDofs, lref_);
+  saveVector(fileName, suffix, StoredData_.velocities, profileLog_.nbDofs);
   suffix = "-torques.log";
-  saveVector(fileName, suffix, StoredData_.torques, profileLog_.nbDofs, lref_);
+  saveVector(fileName, suffix, StoredData_.torques, profileLog_.nbDofs);
   suffix = "-motor-currents.log";
-  saveVector(fileName, suffix, StoredData_.motor_currents, profileLog_.nbDofs, lref_);
+  saveVector(fileName, suffix, StoredData_.motor_currents, profileLog_.nbDofs);
   suffix = "-accelero.log";
-  saveVector(fileName, suffix, StoredData_.accelerometer, 3, 3*lrefts_);
+  saveVector(fileName, suffix, StoredData_.accelerometer, 3);
   suffix = "-gyro.log";
-  saveVector(fileName, suffix, StoredData_.gyrometer, 3, 3*lrefts_);
+  saveVector(fileName, suffix, StoredData_.gyrometer, 3);
 
   ostringstream oss;
   oss << "-forceSensors.log";
   suffix = oss.str();
-  saveVector(fileName, suffix, StoredData_.force_sensors,
-             6 * profileLog_.nbForceSensors,
-             6 * profileLog_.nbForceSensors * lrefts_);
+  if (profileLog_.nbForceSensors > 0) {
+    saveVector(fileName, suffix, StoredData_.force_sensors,
+	       6 * profileLog_.nbForceSensors);
+  }
 
   suffix = "-temperatures.log";
-  saveVector(fileName, suffix, StoredData_.temperatures, profileLog_.nbDofs,
-      lref_);
+  saveVector(fileName, suffix, StoredData_.temperatures, profileLog_.nbDofs);
 
   suffix = "-controls.log";
-  saveVector(fileName, suffix, StoredData_.controls, profileLog_.nbDofs, lref_);
+  saveVector(fileName, suffix, StoredData_.controls, profileLog_.nbDofs);
 
   suffix = "-duration.log";
-  saveVector(fileName, suffix, StoredData_.duration, 1, lrefts_);
+  saveVector(fileName, suffix, StoredData_.duration, 1);
 }
 
 inline void writeHeaderToBinaryBuffer(ofstream &of, const std::size_t &nVector,
@@ -190,8 +190,7 @@ inline void writeToBinaryFile(ofstream &of, const double &t, const double &dt,
 }
 
 void Log::saveVector(std::string &fileName, std::string &suffix,
-                     const std::vector<double> &avector, std::size_t size,
-                     std::size_t start) {
+                     const std::vector<double> &avector, std::size_t size) {
   ostringstream oss;
   oss << fileName;
   oss << suffix.c_str();
@@ -206,7 +205,7 @@ void Log::saveVector(std::string &fileName, std::string &suffix,
 
     writeHeaderToBinaryBuffer(aof, profileLog_.length, size + 2);
     for (unsigned long int i = 0; i < profileLog_.length; i++) {
-      std::size_t k = (start + i) % profileLog_.length;
+      std::size_t k = i % profileLog_.length;
 
       // Compute and save dt
       if (i == 0) {

src/log.hh

@@ -76,13 +76,11 @@ private:
 
   // Save one vector of information.
   // \param size number of contiguous values of avector that forms one line.
-  // \param start index in the time vector at which saving should start.
   // \note avector is a circular buffer. Data will be written from
   //       start to N, and then from 0 to start.
   void saveVector(std::string &filename, std::string &suffix,
                   const std::vector<double> &avector,
-                  std::size_t size,
-                  std::size_t start);
+                  std::size_t size);
 
 public:
   Log();

tests/CMakeLists.txt

@@ -31,3 +31,9 @@ TARGET_INCLUDE_DIRECTORIES(${CONTROLLER_NAME} SYSTEM PUBLIC
   ${catkin_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS})
 TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} ${DEVICE_NAME})
 INSTALL(TARGETS ${CONTROLLER_NAME} DESTINATION lib)
+
+ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
+ADD_EXECUTABLE(sot-test-log sot-test-log.cc)
+TARGET_LINK_LIBRARIES(sot-test-log rcsot_controller)
+
+ADD_PYTHON_UNIT_TEST("py-test-log" "tests/test_log.py" "python")

tests/sot-test-log.cc

+#include <cmath>
+#include "../src/log.hh"
+
+#define BOOST_TEST_MODULE hpp_constraints
+#include <boost/test/included/unit_test.hpp>
+
+BOOST_AUTO_TEST_CASE (logger) {
+  rc_sot_system::DataToLog DataOneIter;
+  rc_sot_system::ProfileLog profileLog;
+  rc_sot_system::Log RcSotLog;
+  std::size_t nbDofs(6);
+
+  profileLog.nbDofs = nbDofs;
+  profileLog.length = 1;
+  profileLog.nbForceSensors = 0;
+  DataOneIter.init(profileLog);
+  // Length of the buffer
+  profileLog.length = 100;
+  RcSotLog.init(profileLog);
+
+  for (std::size_t i=0; i<250; ++i){
+    for (std::size_t j=0; j<nbDofs; ++j){
+      DataOneIter.motor_angle[j] = DataOneIter.joint_angle[j] =
+	DataOneIter.velocities[j] = DataOneIter.torques[j] =
+	DataOneIter.motor_currents[j] = DataOneIter.temperatures[j] =
+	DataOneIter.controls[j] = (double)(i+j);
+    }
+    RcSotLog.record(DataOneIter);
+  }
+  std::string filename("./test.log");
+  RcSotLog.save(filename);
+
+
+}

tests/test_log.py

+import unittest
+import subprocess
+import csv
+
+# This file tests the output of executable sot-test-log compiled from
+# tests/sot-test-log.cc.
+#
+# Logs are saved in binary format in file "build/tests/test.log-vstate.log",
+# then converted in ascii in file "test.log-vstate-ascii.log" and then
+# compared to the expected values.
+class TestLog(unittest.TestCase):
+    def test_log(self):
+        subprocess.run("./sot-test-log")
+        res = subprocess.run(["../roscontrol-sot-parse-log",
+                              "test.log-vstate.log"], capture_output=True,
+                             text = True)
+        with open('./test.log-vstate-ascii.log', 'w') as f:
+            f.write(res.stdout)
+        with open('./test.log-vstate-ascii.log', 'r') as f:
+            r = csv.reader(f, delimiter=' ')
+            for i, line in enumerate(r):
+                self.assertEqual(len(line), 9)
+                data = list(map(float, line[:8]))
+                # latest data between 200 and 250
+                if i < 50:
+                    self.assertEqual(data[2], i+200)
+                    self.assertEqual(data[3], i+201)
+                    self.assertEqual(data[4], i+202)
+                    self.assertEqual(data[5], i+203)
+                    self.assertEqual(data[6], i+204)
+                    self.assertEqual(data[7], i+205)
+                # data between 150 and 200
+                else:
+                    self.assertEqual(data[2], i+100)
+                    self.assertEqual(data[3], i+101)
+                    self.assertEqual(data[4], i+102)
+                    self.assertEqual(data[5], i+103)
+                    self.assertEqual(data[6], i+104)
+                    self.assertEqual(data[7], i+105)
+
+if __name__ == "__main__":
+    unittest.main()