Commit 76e40c54 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Olivier Stasse

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

parent fdcf4123
......@@ -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}
......
......@@ -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);
}
}
......@@ -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:
......
#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;
}
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