Unverified Commit 317d0adb authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #16 from jmirabel/devel

Improve logging.
parents 63c8229e 3b3f239f
......@@ -102,6 +102,7 @@ void Log::record(DataToLog &aDataToLog) {
lref_ = 0;
lrefts_ = 0;
}
assert(lref_ == lrefts_*profileLog_.nbDofs);
}
void Log::start_it() {
......@@ -123,35 +124,39 @@ void Log::stop_it() {
}
void Log::save(std::string &fileName) {
assert(lref_ == lrefts_*profileLog_.nbDofs);
std::string suffix("-mastate.log");
saveVector(fileName, suffix, StoredData_.motor_angle, profileLog_.nbDofs);
saveVector(fileName, suffix, StoredData_.motor_angle, profileLog_.nbDofs, lref_);
suffix = "-jastate.log";
saveVector(fileName, suffix, StoredData_.joint_angle, profileLog_.nbDofs);
saveVector(fileName, suffix, StoredData_.joint_angle, profileLog_.nbDofs, lref_);
suffix = "-vstate.log";
saveVector(fileName, suffix, StoredData_.velocities, profileLog_.nbDofs);
saveVector(fileName, suffix, StoredData_.velocities, profileLog_.nbDofs, lref_);
suffix = "-torques.log";
saveVector(fileName, suffix, StoredData_.torques, profileLog_.nbDofs);
saveVector(fileName, suffix, StoredData_.torques, profileLog_.nbDofs, lref_);
suffix = "-motor-currents.log";
saveVector(fileName, suffix, StoredData_.motor_currents, profileLog_.nbDofs);
saveVector(fileName, suffix, StoredData_.motor_currents, profileLog_.nbDofs, lref_);
suffix = "-accelero.log";
saveVector(fileName, suffix, StoredData_.accelerometer, 3);
saveVector(fileName, suffix, StoredData_.accelerometer, 3, 3*lrefts_);
suffix = "-gyro.log";
saveVector(fileName, suffix, StoredData_.gyrometer, 3);
saveVector(fileName, suffix, StoredData_.gyrometer, 3, 3*lrefts_);
ostringstream oss;
oss << "-forceSensors.log";
suffix = oss.str();
saveVector(fileName, suffix, StoredData_.force_sensors,
6 * profileLog_.nbForceSensors);
6 * profileLog_.nbForceSensors,
6 * profileLog_.nbForceSensors * lrefts_);
suffix = "-temperatures.log";
saveVector(fileName, suffix, StoredData_.temperatures, profileLog_.nbDofs);
saveVector(fileName, suffix, StoredData_.temperatures, profileLog_.nbDofs,
lref_);
suffix = "-controls.log";
saveVector(fileName, suffix, StoredData_.controls, profileLog_.nbDofs);
saveVector(fileName, suffix, StoredData_.controls, profileLog_.nbDofs, lref_);
suffix = "-duration.log";
saveVector(fileName, suffix, StoredData_.duration, 1);
saveVector(fileName, suffix, StoredData_.duration, 1, lrefts_);
}
inline void writeHeaderToBinaryBuffer(ofstream &of, const std::size_t &nVector,
......@@ -169,7 +174,8 @@ 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) {
const std::vector<double> &avector, std::size_t size,
std::size_t start) {
ostringstream oss;
oss << fileName;
oss << suffix.c_str();
......@@ -180,16 +186,22 @@ void Log::saveVector(std::string &fileName, std::string &suffix,
std::size_t idx = 0;
double dt;
if (aof.is_open()) {
std::size_t N = size * profileLog_.length;
writeHeaderToBinaryBuffer(aof, profileLog_.length, size + 2);
for (unsigned long int i = 0; i < profileLog_.length; i++) {
std::size_t k = (start + i) % profileLog_.length;
// Compute and save dt
if (i == 0)
dt = StoredData_.timestamp[i] -
if (i == 0) {
dt = 0;
} else if (k == 0) {
dt = StoredData_.timestamp[0] -
StoredData_.timestamp[profileLog_.length - 1];
else
dt = StoredData_.timestamp[i] - StoredData_.timestamp[i - 1];
} else
dt = StoredData_.timestamp[k] - StoredData_.timestamp[k - 1];
writeToBinaryFile(aof, StoredData_.timestamp[i], dt, avector, idx, size);
idx += size;
idx = (idx + size) % N;
}
aof.close();
ROS_INFO_STREAM("Wrote log file " << actualFileName);
......
......@@ -75,8 +75,14 @@ private:
double time_stop_it_;
// 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);
const std::vector<double> &avector,
std::size_t size,
std::size_t start);
public:
Log();
......
......@@ -75,7 +75,6 @@ RCSotController::RCSotController()
type_name_("RCSotController"), simulation_mode_(false),
accumulated_time_(0.0), jitter_(0.0), verbosity_level_(0) {
RESETDEBUG4();
profileLog_.length = 300000;
}
void RCSotController::displayClaimedResources(
......@@ -105,17 +104,27 @@ void RCSotController::displayClaimedResources(
#endif
}
void RCSotController::initLogs() {
void RCSotController::initLogs(ros::NodeHandle &robot_nh) {
ROS_INFO_STREAM("Initialize log data structure");
int length = 300000;
if (robot_nh.hasParam("/sot_controller/number_logged_iterations")) {
robot_nh.getParam("/sot_controller/number_logged_iterations", length);
int minutes = static_cast<int>(floor(length * dt_ / 60));
int seconds = static_cast<int>(floor(length * dt_)) - minutes * 60;
ROS_INFO_STREAM("Number of iterations that will be logged " << length
<< ", i.e. " << minutes << "m" << seconds << "s");
}
/// Initialize the size of the data to store.
/// Set temporary profileLog to one
/// because DataOneIter is just for one iteration.
size_t tmp_length = profileLog_.length;
profileLog_.length = 1;
DataOneIter_.init(profileLog_);
/// Set profile Log to real good value for the stored data.
profileLog_.length = tmp_length;
profileLog_.length = length;
/// Initialize the data logger for 300s.
RcSotLog_.init(profileLog_);
}
......@@ -135,7 +144,7 @@ bool RCSotController::initRequest(lhi::RobotHW *robot_hw,
return false;
ROS_WARN("initRequest 3");
/// Create all the internal data structures for logging.
initLogs();
initLogs(robot_nh);
ROS_WARN("initRequest 4");
/// Create SoT
SotLoaderBasic::Initialization();
......
......@@ -196,7 +196,7 @@ protected:
/// Initialize internal structure for the logs based on nbDofs
/// number of force sensors and size of the buffer.
void initLogs();
void initLogs(ros::NodeHandle &robot_nh);
///@{ \name Read the parameter server
/// \brief Entry point
......
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