Unverified Commit 4c2df2ec authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #28 from florent-lamiraux/noetic-devel

Fix logs
parents 2c3addc1 a137892b
Pipeline #17081 passed with stage
in 3 minutes and 47 seconds
......@@ -24,14 +24,28 @@ void DataToLog::init(ProfileLog &aProfileLog) {
orientation.resize(4 * aProfileLog.length);
accelerometer.resize(3 * aProfileLog.length);
gyrometer.resize(3 * aProfileLog.length);
force_sensors.resize(aProfileLog.nbForceSensors * 6 * aProfileLog.length);
force_sensors.resize(aProfileLog.nbForceSensors*6*aProfileLog.length);
temperatures.resize(aProfileLog.nbDofs * aProfileLog.length);
controls.resize(aProfileLog.nbDofs * aProfileLog.length);
timestamp.resize(aProfileLog.length);
duration.resize(aProfileLog.length);
for (unsigned int i = 0; i < aProfileLog.nbDofs * aProfileLog.length; i++) {
motor_angle[i] = joint_angle[i] = velocities[i] = 0.0;
motor_angle[i] = joint_angle[i] = velocities[i] = torques[i] =
motor_currents[i] = temperatures[i] = controls[i] = 0.0;
}
for (unsigned int i = 0; i < 4 * aProfileLog.length; i++) {
orientation[i] = 0;
}
for (unsigned int i = 0; i < 3 * aProfileLog.length; i++) {
accelerometer[i] = gyrometer[i] = 0;
}
for (unsigned int i = 0; i < aProfileLog.nbForceSensors*6*aProfileLog.length;
i++) {
force_sensors[i] = 0;
}
for (unsigned int i = 0; i < aProfileLog.length; i++) {
timestamp[i] = duration[i] = 0;
}
}
......@@ -129,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,
......@@ -176,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();
......@@ -192,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,10 @@ 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)
IF(${PYTHON_VERSION_MAJOR} GREATER 2)
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")
ENDIF()
#include <cmath>
#include "../src/log.hh"
int main() {
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