Merge pull request #39 from florent-lamiraux/devel

Make package more robust.

Author: florent-lamiraux

src/log.cpp

@@ -90,28 +90,35 @@ void Log::record(DataToLog &aDataToLog) {
       StoredData_.controls[JointID + lref_] = aDataToLog.controls[JointID];
   }
   for (unsigned int axis = 0; axis < 3; axis++) {
-    StoredData_.accelerometer[lrefts_ * 3 + axis] =
-        aDataToLog.accelerometer[axis];
-    StoredData_.gyrometer[lrefts_ * 3 + axis] = aDataToLog.gyrometer[axis];
+    if (StoredData_.accelerometer.size() >= lrefts_ * 3 + 3)
+      StoredData_.accelerometer[lrefts_ * 3 + axis] =
+          aDataToLog.accelerometer[axis];
+  }
+  for (unsigned int axis = 0; axis < 3; axis++) {
+    if (StoredData_.gyrometer.size() >= lrefts_ * 3 + 3)
+      StoredData_.gyrometer[lrefts_ * 3 + axis] = aDataToLog.gyrometer[axis];
   }
   std::size_t width_pad = 6 * profileLog_.nbForceSensors;
 
   for (unsigned int fsID = 0; fsID < profileLog_.nbForceSensors; fsID++) {
     for (unsigned int axis = 0; axis < 6; axis++) {
-      StoredData_.force_sensors[lrefts_ * width_pad + fsID * 6 + axis] =
-          aDataToLog.force_sensors[fsID * 6 + axis];
+      if (StoredData_.force_sensors.size() >
+          lrefts_ * width_pad + (profileLog_.nbForceSensors - 1) * 6 + 5)
+        StoredData_.force_sensors[lrefts_ * width_pad + fsID * 6 + axis] =
+            aDataToLog.force_sensors[fsID * 6 + axis];
     }
   }
 
   struct timeval current;
   gettimeofday(&current, 0);
 
-  StoredData_.timestamp[lrefts_] =
-      ((double)current.tv_sec + 0.000001 * (double)current.tv_usec) -
-      timeorigin_;
-
-  StoredData_.duration[lrefts_] = time_stop_it_ - time_start_it_;
+  if (StoredData_.timestamp.size() > lrefts_) {
+    StoredData_.timestamp[lrefts_] =
+        ((double)current.tv_sec + 0.000001 * (double)current.tv_usec) -
+        timeorigin_;
 
+    StoredData_.duration[lrefts_] = time_stop_it_ - time_start_it_;
+  }
   lref_ += profileLog_.nbDofs;
   lrefts_++;
   if (lref_ >= profileLog_.nbDofs * profileLog_.length) {

src/roscontrol-sot-controller.cpp

@@ -556,8 +556,11 @@ bool RCSotController::readParamsJointNames(ros::NodeHandle &robot_nh) {
         return false;
       }
     }
-  } else
+  } else {
+    ROS_ERROR(
+        "ROS parameter \"/sot_controller/joint_names\" shoud be defined.");
     return false;
+  }
 
   /// Deduce from this the degree of freedom number.
   nbDofs_ = joints_name_.size();
@@ -938,7 +941,8 @@ void RCSotController::readControl(
     std::string &lmapRC2Sot = it_mapRC2Sot->second;
     command_ = controlValues[lmapRC2Sot].getValues();
     ODEBUG4("angleControl_.size() = " << command_.size());
-    for (unsigned int i = 0; i < command_.size(); ++i) {
+    for (unsigned int i = 0; i < std::min(command_.size(), joints_.size());
+         ++i) {
       joints_[joints_name_[i]].joint.setCommand(command_[i]);
       DataOneIter_.controls[i] = command_[i];
     }