Fix bug on initializing the log data structure.

src/log.cpp

@@ -87,6 +87,7 @@ void Log::record(DataToLog &aDataToLog)
       StoredData_.gyrometer[lrefts_*3+axis] = aDataToLog.gyrometer[axis];
     }
   unsigned width_pad= 6 * profileLog_.nbForceSensors;
+  
   for(unsigned int fsID=0;fsID<profileLog_.nbForceSensors;fsID++)
     {
       for(unsigned int axis=0;axis<6;axis++)
@@ -95,6 +96,7 @@ void Log::record(DataToLog &aDataToLog)
 	    aDataToLog.force_sensors[fsID*6+axis];
 	}
     }
+  
   struct timeval current;
   gettimeofday(&current,0);
 

src/roscontrol-sot-controller.cpp

@@ -82,6 +82,7 @@ namespace sot_controller
     verbosity_level_(0)
   {
     RESETDEBUG4();
+    profileLog_.length=300000;
   }
   
   void RCSotController::
@@ -120,6 +121,18 @@ namespace sot_controller
 #endif
   }
 
+  void RCSotController::initLogs()
+  {
+    ROS_INFO_STREAM("Initialize log data structure");
+    
+    /// Initialize the size of the data to store. 
+    DataOneIter_.init(profileLog_);
+    
+    /// Initialize the data logger for 300s.
+    RcSotLog.init(profileLog_);
+
+  }
+  
   bool RCSotController::
   initRequest (lhi::RobotHW * robot_hw,
 	       ros::NodeHandle &robot_nh,
@@ -135,6 +148,9 @@ namespace sot_controller
     if (!initInterfaces(robot_hw,robot_nh,controller_nh,claimed_resources))
       return false;
 
+    /// Create all the internal data structures for logging.
+    initLogs();
+    
     /// Create SoT
     SotLoaderBasic::Initialization();
 
@@ -308,6 +324,14 @@ namespace sot_controller
 	robot_nh.getParam("/sot_controller/verbosity_level",verbosity_level_);
 	ROS_INFO_STREAM("Verbosity_level " << verbosity_level_);
       }
+    if (robot_nh.hasParam("/sot_controller/log/size"))
+      {
+	int llength;
+	robot_nh.getParam("/sot_controller/log/size",llength);
+	profileLog_.length=(unsigned int)llength;
+	ROS_INFO_STREAM("Size of the log " << profileLog_.length);
+      }
+
   }
   
   bool RCSotController::
@@ -459,11 +483,7 @@ namespace sot_controller
 
     /// Deduce from this the degree of freedom number.
     nbDofs_ = joints_name_.size();
-    /// Initialize the size of the data to store. 
-    DataOneIter_.init(profileLog_);
-    
-    /// Initialize the data logger for 300s.
-    RcSotLog.init(profileLog_);
+    profileLog_.nbDofs = nbDofs_;
 	
     return true;
   }
@@ -678,6 +698,7 @@ namespace sot_controller
       // sensor handle on torque forces
       ft_sensors_.push_back(ft_iface_->getHandle(ft_iface_names[i]));
     }
+    profileLog_.nbForceSensors = ft_iface_names.size();
     return true;
   }
 

src/roscontrol-sot-controller.hh

@@ -227,7 +227,11 @@ namespace sot_controller
     bool initForceSensors();
     /// Initialize the hardware interface accessing the temperature sensors.
     bool initTemperatureSensors();
-
+    
+    /// Initialize internal structure for the logs based on nbDofs
+    /// number of force sensors and size of the buffer.
+    void initLogs();
+    
     ///@{ \name Read the parameter server
     /// \brief Entry point
     bool readParams(ros::NodeHandle &robot_nh);