Merge branch 'devel' into cmake-export

CMakeLists.txt

@@ -4,43 +4,29 @@
 #
 cmake_minimum_required(VERSION 2.8.3)
 
-set(CXX_DISABLE_WERROR True)
-
-set(WARNING_CXX_FLAGS "${WARNING_CXX_FLAGS} -Werror=format-security")
-
 SET(PROJECT_ORG stack-of-tasks)
 set(PROJECT_DESCRIPTION "roscontrol_sot")
 set(PROJECT_NAME roscontrol_sot)
-set(PROJECT_URL "https://github.com/stack-of-tasks/roscontrol_sot")
+set(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
 
-# Export CMake Target
-SET(PROJECT_USE_CMAKE_EXPORT TRUE)
 
-# Disable failing compilation when a compilation error appears
+set(CXX_DISABLE_WERROR True)
+SET(CMAKE_CXX_STANDARD 11)
+SET(PROJECT_USE_CMAKE_EXPORT TRUE)
 set(CXX_DISABLE_WERROR False)
 
 include(cmake/base.cmake)
-
-# Specify the project.
-cmake_policy(SET CMP0048 NEW)
-PROJECT(${PROJECT_NAME}
-  LANGUAGES
-  CXX
-  VERSION
-  ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}.${PROJECT_VERSION_PATCH}
-  )
-
 include(cmake/ros.cmake)
-include(cmake/GNUInstallDirs.cmake)
 include(cmake/python.cmake)
 
-find_package(PkgConfig REQUIRED)
+CMAKE_POLICY(SET CMP0048 OLD)
+project(roscontrol_sot CXX)
 
 add_required_dependency(bullet)
 add_required_dependency("urdfdom")
 
 SET(CATKIN_REQUIRED_COMPONENTS
-  temperature_sensor_controller  
+  temperature_sensor_controller
   pal_hardware_interfaces
   controller_interface
   controller_manager
@@ -64,22 +50,10 @@ find_package(catkin REQUIRED COMPONENTS
   ${CATKIN_REQUIRED_COMPONENTS}
 )
 
-## LAAS cmake submodule part
-set(PROJECT_DESCRIPTION "Integration of the Stack of Tasks in roscontrol")
-set(PROJECT_NAME roscontrol_sot)
-set(PROJECT_URL "https://github.com/stack-of-tasks/roscontrol_sot")
-
-
 include_directories(include tests ${catkin_INCLUDE_DIRS} ${bullet_INCLUDE_DIRS})
 
 link_directories(${bullet_LIBRARY_DIRS})
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin)
-set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib)
-set(CMAKE_INSTALL_RPATH "${LIBRARY_OUTPUT_PATH}")
-set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${LIBRARY_OUTPUT_PATH}")
 
 # Add dependency through jrl-cmakemodules to compile
 # this code without catkin_make
@@ -94,7 +68,7 @@ add_required_dependency("pinocchio" REQUIRED)
 catkin_package(CATKIN_DEPENDS
   temperature_sensor_controller
   pal_hardware_interfaces
-  controller_interface controller_manager  
+  controller_interface controller_manager
   roscpp
   realtime_tools
   message_runtime
@@ -118,7 +92,7 @@ LIST(APPEND LOGGING_WATCHED_VARIABLES
   TEMPERATURE_SENSOR_CONTROLLER_FOUND
   CONTROLLER_INTERFACE_FOUND
   CONTROLLER_INTERFACE_VERSION )
-  
+
 
 ###########
 ## Build ##
@@ -155,13 +129,13 @@ target_link_libraries(rcsot_controller dynamic-graph::dynamic-graph)
 
 ## Mark executables and/or libraries for installation
 install(TARGETS rcsot_controller
-  EXPORT ${TARGETS_EXPORT_NAME}    
+  EXPORT ${TARGETS_EXPORT_NAME}
   DESTINATION lib )
 
 ADD_EXECUTABLE(roscontrol-sot-parse-log
   src/roscontrol-sot-parse-log.cc)
 install(TARGETS roscontrol-sot-parse-log
-  EXPORT ${TARGETS_EXPORT_NAME}    
+  EXPORT ${TARGETS_EXPORT_NAME}
   DESTINATION bin )
 
 foreach(dir config launch)
@@ -171,20 +145,4 @@ foreach(dir config launch)
 endforeach()
 
 
-#ADD_SUBDIRECTORY(tests)
-
-get_cmake_property(_variableNames VARIABLES)
-list (SORT _variableNames)
-foreach (_variableName ${_variableNames})
-  LIST(APPEND LOGGING_WATCHED_VARIABLES
-    ${_variableName}
-    )
-endforeach()
-
-SETUP_PROJECT_FINALIZE()
-
-get_cmake_property(_variableNames VARIABLES)
-list (SORT _variableNames)
-foreach (_variableName ${_variableNames})
-    message(STATUS "${_variableName}=${${_variableName}}")
-endforeach()
+ADD_SUBDIRECTORY(tests)

package.xml

 <?xml version="1.0"?>
 <package>
   <name>roscontrol_sot</name>
-  <version>0.1.0</version>
+  <version>0.2.0</version>
   <description>Wrapping the Stack-of-tasks framework in ros-control</description>
 
   <!-- Maintainer email -->

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();
@@ -1061,18 +1070,15 @@ void RCSotController::update(const ros::Time &, const ros::Duration &period) {
       } else
         accumulated_time_ += periodInSec;
     } catch (std::exception const &exc) {
-      std::cerr << "Failure happened during one_iteration evaluation: "
-                << "std_exception" << std::endl;
-      std::cerr << "Use gdb on this line together with gdb to "
-                << "investiguate the problem: " << std::endl;
-      std::cerr << __FILE__ << " " << __LINE__ << std::endl;
-      throw exc;
+      ROS_ERROR_STREAM("Failure happened during one_iteration evaluation: "
+                << exc.what() << "\nUse gdb to investiguate the problem\n"
+                << __FILE__ << ":" << __LINE__);
+      throw;
     } catch (...) {
-      std::cerr << "Failure happened during one_iteration evaluation: "
-                << "unknown exception" << std::endl;
-      std::cerr << "Use gdb on this line together with gdb to "
-                << "investiguate the problem: " << std::endl;
-      std::cerr << __FILE__ << " " << __LINE__ << std::endl;
+      ROS_ERROR_STREAM("Failure happened during one_iteration evaluation: "
+                "unknown exception\nUse gdb to investiguate the problem\n"
+                << __FILE__ << ":" << __LINE__);
+      throw;
     }
   } else {
     /// Update the sensors.
@@ -1080,7 +1086,8 @@ void RCSotController::update(const ros::Time &, const ros::Duration &period) {
     try {
       sotController_->setupSetSensors(sensorsIn_);
     } catch (std::exception &e) {
-      throw e;
+      ROS_ERROR_STREAM("RCSotController::update: " << e.what());
+      throw;
     }
   }
 }

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