Skip to content

Commit 4c2df2e

Browse files
nim65snim65s
authored
Merge pull request #28 from florent-lamiraux/noetic-devel
Fix logs
2 parents 2c3addc + a137892 commit 4c2df2e

File tree

5 files changed

+111
-22
lines changed

5 files changed

+111
-22
lines changed

‎src/log.cpp

Lines changed: 32 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -24,14 +24,28 @@ void DataToLog::init(ProfileLog &aProfileLog) {
2424
orientation.resize(4 * aProfileLog.length);
2525
accelerometer.resize(3 * aProfileLog.length);
2626
gyrometer.resize(3 * aProfileLog.length);
27-
force_sensors.resize(aProfileLog.nbForceSensors * 6 * aProfileLog.length);
27+
force_sensors.resize(aProfileLog.nbForceSensors*6*aProfileLog.length);
2828
temperatures.resize(aProfileLog.nbDofs * aProfileLog.length);
2929
controls.resize(aProfileLog.nbDofs * aProfileLog.length);
3030
timestamp.resize(aProfileLog.length);
3131
duration.resize(aProfileLog.length);
3232

3333
for (unsigned int i = 0; i < aProfileLog.nbDofs * aProfileLog.length; i++) {
34-
motor_angle[i] = joint_angle[i] = velocities[i] = 0.0;
34+
motor_angle[i] = joint_angle[i] = velocities[i] = torques[i] =
35+
motor_currents[i] = temperatures[i] = controls[i] = 0.0;
36+
}
37+
for (unsigned int i = 0; i < 4 * aProfileLog.length; i++) {
38+
orientation[i] = 0;
39+
}
40+
for (unsigned int i = 0; i < 3 * aProfileLog.length; i++) {
41+
accelerometer[i] = gyrometer[i] = 0;
42+
}
43+
for (unsigned int i = 0; i < aProfileLog.nbForceSensors*6*aProfileLog.length;
44+
i++) {
45+
force_sensors[i] = 0;
46+
}
47+
for (unsigned int i = 0; i < aProfileLog.length; i++) {
48+
timestamp[i] = duration[i] = 0;
3549
}
3650
}
3751

@@ -129,36 +143,36 @@ void Log::save(std::string &fileName) {
129143
assert(lref_ == lrefts_*profileLog_.nbDofs);
130144

131145
std::string suffix("-mastate.log");
132-
saveVector(fileName, suffix, StoredData_.motor_angle, profileLog_.nbDofs, lref_);
146+
saveVector(fileName, suffix, StoredData_.motor_angle, profileLog_.nbDofs);
133147
suffix = "-jastate.log";
134-
saveVector(fileName, suffix, StoredData_.joint_angle, profileLog_.nbDofs, lref_);
148+
saveVector(fileName, suffix, StoredData_.joint_angle, profileLog_.nbDofs);
135149
suffix = "-vstate.log";
136-
saveVector(fileName, suffix, StoredData_.velocities, profileLog_.nbDofs, lref_);
150+
saveVector(fileName, suffix, StoredData_.velocities, profileLog_.nbDofs);
137151
suffix = "-torques.log";
138-
saveVector(fileName, suffix, StoredData_.torques, profileLog_.nbDofs, lref_);
152+
saveVector(fileName, suffix, StoredData_.torques, profileLog_.nbDofs);
139153
suffix = "-motor-currents.log";
140-
saveVector(fileName, suffix, StoredData_.motor_currents, profileLog_.nbDofs, lref_);
154+
saveVector(fileName, suffix, StoredData_.motor_currents, profileLog_.nbDofs);
141155
suffix = "-accelero.log";
142-
saveVector(fileName, suffix, StoredData_.accelerometer, 3, 3*lrefts_);
156+
saveVector(fileName, suffix, StoredData_.accelerometer, 3);
143157
suffix = "-gyro.log";
144-
saveVector(fileName, suffix, StoredData_.gyrometer, 3, 3*lrefts_);
158+
saveVector(fileName, suffix, StoredData_.gyrometer, 3);
145159

146160
ostringstream oss;
147161
oss << "-forceSensors.log";
148162
suffix = oss.str();
149-
saveVector(fileName, suffix, StoredData_.force_sensors,
150-
6 * profileLog_.nbForceSensors,
151-
6 * profileLog_.nbForceSensors * lrefts_);
163+
if (profileLog_.nbForceSensors > 0) {
164+
saveVector(fileName, suffix, StoredData_.force_sensors,
165+
6 * profileLog_.nbForceSensors);
166+
}
152167

153168
suffix = "-temperatures.log";
154-
saveVector(fileName, suffix, StoredData_.temperatures, profileLog_.nbDofs,
155-
lref_);
169+
saveVector(fileName, suffix, StoredData_.temperatures, profileLog_.nbDofs);
156170

157171
suffix = "-controls.log";
158-
saveVector(fileName, suffix, StoredData_.controls, profileLog_.nbDofs, lref_);
172+
saveVector(fileName, suffix, StoredData_.controls, profileLog_.nbDofs);
159173

160174
suffix = "-duration.log";
161-
saveVector(fileName, suffix, StoredData_.duration, 1, lrefts_);
175+
saveVector(fileName, suffix, StoredData_.duration, 1);
162176
}
163177

164178
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,
176190
}
177191

178192
void Log::saveVector(std::string &fileName, std::string &suffix,
179-
const std::vector<double> &avector, std::size_t size,
180-
std::size_t start) {
193+
const std::vector<double> &avector, std::size_t size) {
181194
ostringstream oss;
182195
oss << fileName;
183196
oss << suffix.c_str();
@@ -192,7 +205,7 @@ void Log::saveVector(std::string &fileName, std::string &suffix,
192205

193206
writeHeaderToBinaryBuffer(aof, profileLog_.length, size + 2);
194207
for (unsigned long int i = 0; i < profileLog_.length; i++) {
195-
std::size_t k = (start + i) % profileLog_.length;
208+
std::size_t k = i % profileLog_.length;
196209

197210
// Compute and save dt
198211
if (i == 0) {

‎src/log.hh

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -76,13 +76,11 @@ private:
7676

7777
// Save one vector of information.
7878
// \param size number of contiguous values of avector that forms one line.
79-
// \param start index in the time vector at which saving should start.
8079
// \note avector is a circular buffer. Data will be written from
8180
// start to N, and then from 0 to start.
8281
void saveVector(std::string &filename, std::string &suffix,
8382
const std::vector<double> &avector,
84-
std::size_t size,
85-
std::size_t start);
83+
std::size_t size);
8684

8785
public:
8886
Log();

‎tests/CMakeLists.txt

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,3 +31,10 @@ TARGET_INCLUDE_DIRECTORIES(${CONTROLLER_NAME} SYSTEM PUBLIC
3131
${catkin_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS})
3232
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} ${DEVICE_NAME})
3333
INSTALL(TARGETS ${CONTROLLER_NAME} DESTINATION lib)
34+
35+
IF(${PYTHON_VERSION_MAJOR} GREATER 2)
36+
ADD_EXECUTABLE(sot-test-log sot-test-log.cc)
37+
TARGET_LINK_LIBRARIES(sot-test-log rcsot_controller)
38+
39+
ADD_PYTHON_UNIT_TEST("py-test-log" "tests/test_log.py" "python")
40+
ENDIF()

‎tests/sot-test-log.cc

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
#include <cmath>
2+
#include "../src/log.hh"
3+
4+
int main() {
5+
rc_sot_system::DataToLog DataOneIter;
6+
rc_sot_system::ProfileLog profileLog;
7+
rc_sot_system::Log RcSotLog;
8+
std::size_t nbDofs(6);
9+
10+
profileLog.nbDofs = nbDofs;
11+
profileLog.length = 1;
12+
profileLog.nbForceSensors = 0;
13+
DataOneIter.init(profileLog);
14+
// Length of the buffer
15+
profileLog.length = 100;
16+
RcSotLog.init(profileLog);
17+
18+
for (std::size_t i=0; i<250; ++i){
19+
for (std::size_t j=0; j<nbDofs; ++j){
20+
DataOneIter.motor_angle[j] = DataOneIter.joint_angle[j] =
21+
DataOneIter.velocities[j] = DataOneIter.torques[j] =
22+
DataOneIter.motor_currents[j] = DataOneIter.temperatures[j] =
23+
DataOneIter.controls[j] = (double)(i+j);
24+
}
25+
RcSotLog.record(DataOneIter);
26+
}
27+
std::string filename("./test.log");
28+
RcSotLog.save(filename);
29+
}

‎tests/test_log.py

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
import unittest
2+
import subprocess
3+
import csv
4+
5+
# This file tests the output of executable sot-test-log compiled from
6+
# tests/sot-test-log.cc.
7+
#
8+
# Logs are saved in binary format in file "build/tests/test.log-vstate.log",
9+
# then converted in ascii in file "test.log-vstate-ascii.log" and then
10+
# compared to the expected values.
11+
class TestLog(unittest.TestCase):
12+
def test_log(self):
13+
subprocess.run("./sot-test-log")
14+
res = subprocess.run(["../roscontrol-sot-parse-log",
15+
"test.log-vstate.log"], capture_output=True,
16+
text = True)
17+
with open('./test.log-vstate-ascii.log', 'w') as f:
18+
f.write(res.stdout)
19+
with open('./test.log-vstate-ascii.log', 'r') as f:
20+
r = csv.reader(f, delimiter=' ')
21+
for i, line in enumerate(r):
22+
self.assertEqual(len(line), 9)
23+
data = list(map(float, line[:8]))
24+
# latest data between 200 and 250
25+
if i < 50:
26+
self.assertEqual(data[2], i+200)
27+
self.assertEqual(data[3], i+201)
28+
self.assertEqual(data[4], i+202)
29+
self.assertEqual(data[5], i+203)
30+
self.assertEqual(data[6], i+204)
31+
self.assertEqual(data[7], i+205)
32+
# data between 150 and 200
33+
else:
34+
self.assertEqual(data[2], i+100)
35+
self.assertEqual(data[3], i+101)
36+
self.assertEqual(data[4], i+102)
37+
self.assertEqual(data[5], i+103)
38+
self.assertEqual(data[6], i+104)
39+
self.assertEqual(data[7], i+105)
40+
41+
if __name__ == "__main__":
42+
unittest.main()

0 commit comments

Comments
 (0)