Commit 60593c46 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

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.
parent 294e5331
Pipeline #17056 failed
......@@ -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) {
......
......@@ -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();
......
......@@ -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")
#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);
}
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()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment