Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
roscontrol_sot
Commits
eadb9cb9
Commit
eadb9cb9
authored
Mar 12, 2020
by
Guilhem Saurel
Browse files
Merge branch 'devel' into cmake-export
parents
3b8312b4
a24367df
Changes
6
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
eadb9cb9
...
...
@@ -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
View file @
eadb9cb9
<?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
View file @
eadb9cb9
...
...
@@ -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
View file @
eadb9cb9
...
...
@@ -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
View file @
eadb9cb9
...
...
@@ -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
()
<<
"
\n
Use 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
\n
Use 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
View file @
eadb9cb9
...
...
@@ -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
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment