Commit eadb9cb9 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'devel' into cmake-export

parents 3b8312b4 a24367df
......@@ -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)
<?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 -->
......
......@@ -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);
......
......@@ -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();
......
......@@ -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;
}
}
}
......
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment