Merge pull request #16 from jmirabel/devel

Improve logging.

src/log.cpp

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

src/log.hh

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

src/roscontrol-sot-controller.cpp

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

src/roscontrol-sot-controller.hh

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