Commit d5f27702 authored by Olivier Stasse's avatar Olivier Stasse

Fix several bugs in logging with the new logging architecture.

parent 0bd99a03
......@@ -62,8 +62,8 @@ void Log::init(ProfileLog &aProfileLog)
void Log::record(DataToLog &aDataToLog)
{
if ( (aDataToLog.motor_angle.size()!=aDataToLog.nbDofs()) ||
(aDataToLog.velocities.size()!=aDataToLog.nbDofs()))
if ( (aDataToLog.motor_angle.size()!=profileLog_.nbDofs) ||
(aDataToLog.velocities.size()!=profileLog_.nbDofs))
return;
for(unsigned int JointID=0;JointID<aDataToLog.nbDofs();JointID++)
......@@ -105,9 +105,9 @@ void Log::record(DataToLog &aDataToLog)
StoredData_.duration[lrefts_] = time_stop_it_ - time_start_it_;
lref_ += aDataToLog.nbDofs();
lref_ += profileLog_.nbDofs;
lrefts_ ++;
if (lref_>=aDataToLog.nbDofs()*aDataToLog.length())
if (lref_>=profileLog_.nbDofs*profileLog_.length)
{
lref_=0;
lrefts_=0;
......@@ -153,7 +153,7 @@ void Log::save(std::string &fileName)
ostringstream oss;
oss << "-forceSensors.log";
suffix = oss.str();
saveVector(fileName,suffix,StoredData_.force_sensors, 6);
saveVector(fileName,suffix,StoredData_.force_sensors, 6 * profileLog_.nbForceSensors);
suffix = "-temperatures.log";
saveVector(fileName,suffix,StoredData_.temperatures, profileLog_.nbDofs);
......
......@@ -124,12 +124,17 @@ namespace sot_controller
void RCSotController::initLogs()
{
ROS_INFO_STREAM("Initialize log data structure");
/// Initialize the size of the data to store.
/// Initialize the size of the data to store.
/// Set temporary profileLog to one
/// because DataOneIter is just for one iteration.
unsigned 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;
/// Initialize the data logger for 300s.
RcSotLog.init(profileLog_);
RcSotLog_.init(profileLog_);
}
......@@ -909,7 +914,7 @@ namespace sot_controller
void RCSotController::one_iteration()
{
// Chrono start
RcSotLog.start_it();
RcSotLog_.start_it();
/// Update the sensors.
fillSensors();
......@@ -926,10 +931,10 @@ namespace sot_controller
readControl(controlValues_);
// Chrono stop.
RcSotLog.stop_it();
RcSotLog_.stop_it();
/// Store everything in Log.
RcSotLog.record(DataOneIter_);
RcSotLog_.record(DataOneIter_);
}
void RCSotController::
......@@ -1038,7 +1043,7 @@ namespace sot_controller
stopping(const ros::Time &)
{
std::string afilename("/tmp/sot.log");
RcSotLog.save(afilename);
RcSotLog_.save(afilename);
SotLoaderBasic::CleanUp();
......
......@@ -140,7 +140,7 @@ namespace sot_controller
/// @}
/// \brief Log
rc_sot_system::Log RcSotLog;
rc_sot_system::Log RcSotLog_;
/// @}
const std::string type_name_;
......
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