Fix several bugs in logging with the new logging architecture.

Author: olivier-stasse

src/log.cpp

@@ -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);

src/roscontrol-sot-controller.cpp

@@ -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();
 

src/roscontrol-sot-controller.hh

@@ -140,7 +140,7 @@ namespace sot_controller
     /// @}
 
     /// \brief Log
-    rc_sot_system::Log RcSotLog;
+    rc_sot_system::Log RcSotLog_;
     /// @}
 
     const std::string type_name_;