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
af2e0f25
Commit
af2e0f25
authored
Aug 21, 2019
by
Olivier Stasse
Browse files
[tests] Add unit tests.
parent
8fae02ae
Changes
12
Expand all
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
af2e0f25
...
...
@@ -20,20 +20,26 @@ find_package(PkgConfig REQUIRED)
add_required_dependency
(
bullet
)
add_required_dependency
(
"urdfdom"
)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package
(
catkin REQUIRED COMPONENTS
SET
(
CATKIN_REQUIRED_COMPONENTS
pal_hardware_interfaces
controller_interface
roscpp
rospy
std_msgs
dynamic_graph_bridge
control_msgs
sensor_msgs
realtime_tools
realtime_tools
)
SET
(
CATKIN_DEPENDS_LIBRARIES ros_bridge sot_loader rcsot_controller
)
SET
(
CATKIN_REQUIRED_COMPONENTS
${
CATKIN_REQUIRED_COMPONENTS
}
rospy
)
SET
(
CATKIN_DEPENDS_LIBRARIES
${
CATKIN_DEPENDS_LIBRARIES
}
ros_interpreter
)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package
(
catkin REQUIRED COMPONENTS
${
CATKIN_REQUIRED_COMPONENTS
}
)
## LAAS cmake submodule part
...
...
@@ -42,7 +48,7 @@ set(PROJECT_NAME roscontrol_sot)
set
(
PROJECT_URL
"https://github.com/stack-of-tasks/roscontrol_sot"
)
include_directories
(
include
${
bullet_INCLUDE_DIRS
}
${
catkin_INCLUDE_DIRS
}
)
include_directories
(
include
tests
${
bullet_INCLUDE_DIRS
}
${
catkin_INCLUDE_DIRS
}
)
link_directories
(
${
bullet_LIBRARY_DIRS
}
)
...
...
@@ -64,12 +70,16 @@ add_required_dependency("controller_interface")
add_required_dependency
(
"controller_manager"
)
add_required_dependency
(
"pal_common_msgs"
)
add_required_dependency
(
"dynamic-graph"
)
add_required_dependency
(
"dynamic-graph-python"
)
add_required_dependency
(
"sot-core"
)
add_required_dependency
(
"pinocchio"
)
# This is necessary so that the pc file generated by catking is similar
to the on
# done directly by jrl-cmake-modules
# This is necessary so that the pc file generated by catking is similar
#
to the on
done directly by jrl-cmake-modules
catkin_package
(
CATKIN_DEPENDS
roscpp realtime_tools message_runtime dynamic_graph_bridge pal_hardware_interfaces controller_interface controller_manager
LIBRARIES rcsot_controller
)
roscpp realtime_tools message_runtime dynamic_graph_bridge
pal_hardware_interfaces controller_interface controller_manager
LIBRARIES
${
CATKIN_DEPENDS_LIBRARIES
}
)
# Detect the controller interface version to switch code
if
(
CONTROLLER_INTERFACE_FOUND
)
...
...
@@ -131,6 +141,7 @@ foreach(dir config launch)
)
endforeach
()
ADD_SUBDIRECTORY
(
tests
)
SETUP_PROJECT_FINALIZE
()
config/sot_test_params.yaml
0 → 100644
View file @
af2e0f25
gains
:
arm_1_joint
:
{
p
:
1
,
d
:
0.0
,
i
:
0
,
i_clamp
:
1.5
}
arm_2_joint
:
{
p
:
1
,
d
:
0.0
,
i
:
0
,
i_clamp
:
1.5
}
arm_3_joint
:
{
p
:
1
,
d
:
0.0
,
i
:
0
,
i_clamp
:
1.5
}
arm_4_joint
:
{
p
:
1
,
d
:
0.0
,
i
:
0
,
i_clamp
:
1.5
}
arm_5_joint
:
{
p
:
1
,
d
:
0.0
,
i
:
0
,
i_clamp
:
1.5
}
arm_6_joint
:
{
p
:
1
,
d
:
0.0
,
i
:
0
,
i_clamp
:
1.5
}
arm_7_joint
:
{
p
:
1
,
d
:
0.0
,
i
:
0
,
i_clamp
:
1.5
}
joint_state_controller
:
type
:
joint_state_controller/JointStateController
publish_rate
:
50
sot_controller
:
type
:
sot_controller/RCSotController
libname
:
libsot-test-controller.so
joint_names
:
[
arm_1_joint
,
arm_2_joint
,
arm_3_joint
,
arm_4_joint
,
arm_5_joint
,
arm_6_joint
,
arm_7_joint
]
map_rc_to_sot_device
:
{
motor-angles
:
motor-angles
,
joint-angles
:
joint-angles
,
velocities
:
velocities
,
forces
:
forces
,
currents
:
currents
,
torques
:
torques
,
cmd-joints
:
control
,
cmd-effort
:
control
,
accelerometer_0
:
accelerometer_0
,
gyrometer_0
:
gyrometer_0
}
control_mode
:
arm_1_joint
:
ros_control_mode
:
POSITION
arm_2_joint
:
ros_control_mode
:
POSITION
arm_3_joint
:
ros_control_mode
:
POSITION
arm_4_joint
:
ros_control_mode
:
POSITION
arm_5_joint
:
ros_control_mode
:
POSITION
arm_6_joint
:
ros_control_mode
:
POSITION
arm_7_joint
:
ros_control_mode
:
POSITION
dt
:
0.001
jitter
:
0.0004
verbosity_level
:
25
launch/roscontrol_sot_hardware.launch
View file @
af2e0f25
<launch>
<!-- Sot Controller configuration -->
<!-- <rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_your_robot_params_gazebo.yaml"/> -->
<!-- <rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_your_robot_controller.yaml" /> -->
<!-- Load URDF file -->
<param name="robot_description" command="$(find xacro)/xacro.py $(find roscontrol_sot)/urdf/test.urdf" />
<!-- Spawn walking controller -->
<!-- Spawn simulated hardware -->
<node name="roscontrol_sot_hardware_spawner"
pkg="roscontrol_sot" type="roscontrol_sot_hardware" output="screen"
launch-prefix="sudo -EA gdb --args">
<env name="SUDO_ASKPASS" value="/usr/bin/ssh-askpass"/>
</node>
<node name="bringup_controllers_hardware_spawner"
pkg="controller_manager" type="spawner" output="screen"
respawn="false"
args="joint_state_controller"/>
<rosparam command="load" file="$(find roscontrol_sot)/config/sot_test_params.yaml"/>
<!-- Spawn roscontrol sot -->
<node name="sot_controller_spawner"
pkg="controller_manager" type="spawner" output="screen"
args="sot_controller" />
</launch>
src/roscontrol-sot-controller.cpp
View file @
af2e0f25
This diff is collapsed.
Click to expand it.
tests/CMakeLists.txt
View file @
af2e0f25
...
...
@@ -3,12 +3,101 @@
# Author: Olivier Stasse
#
INCLUDE_DIRECTORIES
(
${
CMAKE_SOURCE_DIRECTORY
}
/tests
)
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
}
"
)
set
(
PKG_CONFIG_ADDITIONAL_VARIABLES
${
PKG_CONFIG_ADDITIONAL_VARIABLES
}
plugindirname
plugindir
)
ADD_EXECUTABLE
(
roscontrol_sot_hardware roscontrol_sot_hardware.cpp
)
# Create directory for local wrap.so
file
(
MAKE_DIRECTORY
${
LIBRARY_OUTPUT_PATH
}
/dynamic_graph/sot/test/sot_test_device
)
pkg_config_use_dependency
(
roscontrol_sot_hardware controller_manager
NO_INCLUDE_SYSTEM
)
target_link_libraries
(
roscontrol_sot_hardware
${
catkin_LIBRARIES
}
${
bullet_libraries
}
)
# Add the library to wrap the test device.
MACRO
(
build_test_device
)
SET
(
DEVICE_NAME sot-test-device
)
ADD_LIBRARY
(
${
DEVICE_NAME
}
SHARED
sot-test-device.cpp
)
set_target_properties
(
${
DEVICE_NAME
}
PROPERTIES BUILD_WITH_INSTALL_RPATH True
)
#set_target_properties(${DEVICE_NAME} PROPERTIES PREFIX "")
# Link the dynamic library containing the SoT with its dependencies.
PKG_CONFIG_USE_DEPENDENCY
(
${
DEVICE_NAME
}
"dynamic-graph"
)
PKG_CONFIG_USE_DEPENDENCY
(
${
DEVICE_NAME
}
"dynamic-graph-python"
)
PKG_CONFIG_USE_DEPENDENCY
(
${
DEVICE_NAME
}
"sot-core"
)
PKG_CONFIG_USE_DEPENDENCY
(
${
DEVICE_NAME
}
"pinocchio"
)
PKG_CONFIG_USE_DEPENDENCY
(
${
DEVICE_NAME
}
"dynamic_graph_bridge"
)
IF
(
UNIX AND NOT APPLE
)
TARGET_LINK_LIBRARIES
(
${
DEVICE_NAME
}
${
Boost_LIBRARIES
}
)
ENDIF
(
UNIX AND NOT APPLE
)
INSTALL
(
TARGETS
${
DEVICE_NAME
}
DESTINATION
${
CMAKE_INSTALL_LIBDIR
}
)
# build python submodule
STRING
(
REPLACE - _ PYTHON_LIBRARY_NAME
${
DEVICE_NAME
}
)
SET
(
NEW_ENTITY_CLASS
${
ENTITIES
}
)
DYNAMIC_GRAPH_PYTHON_MODULE
(
"sot/test/
${
PYTHON_LIBRARY_NAME
}
"
${
DEVICE_NAME
}
sot/test/
${
PYTHON_LIBRARY_NAME
}
/wrap
)
ENDMACRO
()
build_test_device
()
# Add the library to wrap the controller of Test.
MACRO
(
build_test_controller
)
SET
(
CONTROLLER_NAME sot-test-controller
)
ADD_LIBRARY
(
${
CONTROLLER_NAME
}
SHARED
sot-test-controller.cpp
)
#
set_target_properties
(
${
CONTROLLER_NAME
}
PROPERTIES BUILD_WITH_INSTALL_RPATH True
)
#set_target_properties(${CONTROLLER_NAME} PROPERTIES PREFIX "")
# Link the dynamic library containing the SoT with its dependencies.
PKG_CONFIG_USE_DEPENDENCY
(
${
CONTROLLER_NAME
}
"dynamic-graph"
)
PKG_CONFIG_USE_DEPENDENCY
(
${
CONTROLLER_NAME
}
"dynamic-graph-python"
)
PKG_CONFIG_USE_DEPENDENCY
(
${
CONTROLLER_NAME
}
"sot-core"
)
PKG_CONFIG_USE_DEPENDENCY
(
${
CONTROLLER_NAME
}
"dynamic_graph_bridge"
)
ADD_DEPENDENCIES
(
${
CONTROLLER_NAME
}
sot-test-device
)
TARGET_LINK_LIBRARIES
(
${
CONTROLLER_NAME
}
"ros_bridge"
)
TARGET_LINK_LIBRARIES
(
${
CONTROLLER_NAME
}
"ros_interpreter"
)
TARGET_LINK_LIBRARIES
(
${
CONTROLLER_NAME
}
"sot-test-device"
)
IF
(
UNIX AND NOT APPLE
)
TARGET_LINK_LIBRARIES
(
${
CONTROLLER_NAME
}
${
Boost_LIBRARIES
}
)
ENDIF
(
UNIX AND NOT APPLE
)
INSTALL
(
TARGETS
${
CONTROLLER_NAME
}
DESTINATION
${
CMAKE_INSTALL_LIBDIR
}
)
ENDMACRO
()
build_test_controller
()
tests/roscontrol_sot_hardware.cpp
View file @
af2e0f25
/*Sources :
* https://github.com/jhu-lcsr-attic/barrett_control/blob/libbarrett/
barrett_hw/src/wam_server.cpp
* http://www.ensta-bretagne.fr/lemezo/files/teaching/ROS/TP1.pdf
* http://homepages.laas.fr/ostasse/Teaching/ROS/rosintro.pdf
* https://github.com/ros-controls/ros_control/wiki/hardware_interface
# To compile :g++ ros_hardware.cpp -o ros_harware -I/opt/ros/kinetic/include
-L/opt/ros/kinetic/lib
*/
/*
* Copyright: LAAS CNRS, 2019
* Author: O. Stasse
* LICENSE: See LICENSE.txt
*
*/
#include
<iostream>
#include
<sstream>
...
...
@@ -15,7 +12,7 @@
//Threads
void
*
RTloop
(
void
*
argument
);
void
*
ROSloop
(
void
*
argument
);
pthread_mutex_t
mutx
=
PTHREAD_MUTEX_INITIALIZER
;
//mutex initialisation
//******************************************************
...
...
@@ -34,63 +31,104 @@ public:
{
/// connect and register the joint state interface
hardware_interface
::
JointStateHandle
state_handle_base
(
"base"
,
&
pos_
[
0
],
&
vel_
[
0
],
&
eff_
[
0
]);
jnt_state_interface_
.
registerHandle
(
state_handle_base
);
#ifndef TEMPERATURE_SENSOR_CONTROLLER
state_handle_arm_1
(
"arm_1_joint"
,
&
pos_
[
0
],
&
vel_
[
0
],
&
eff_
[
0
]);
#else
state_handle_arm_1
(
"arm_1_joint"
,
&
pos_
[
0
],
&
vel_
[
0
],
&
eff_
[
0
],
&
abs_pos_
[
0
],
&
abs_torque_
[
0
]);
#endif
jnt_state_interface_
.
registerHandle
(
state_handle_arm_1
);
hardware_interface
::
JointStateHandle
state_handle_Head
(
"Head"
,
&
pos_
[
1
],
&
vel_
[
1
],
&
eff_
[
1
]);
jnt_state_interface_
.
registerHandle
(
state_handle_Head
);
#ifndef TEMPERATURE_SENSOR_CONTROLLER
state_handle_arm_2
(
"arm_2_joint"
,
&
pos_
[
1
],
&
vel_
[
1
],
&
eff_
[
1
]);
#else
state_handle_arm_2
(
"arm_2_joint"
,
&
pos_
[
1
],
&
vel_
[
1
],
&
eff_
[
1
],
&
abs_pos_
[
1
],
&
abs_torque_
[
1
]);
#endif
jnt_state_interface_
.
registerHandle
(
state_handle_arm_2
);
hardware_interface
::
JointStateHandle
state_handle_Neck
(
"Neck"
,
&
pos_
[
2
],
&
vel_
[
2
],
&
eff_
[
2
]);
jnt_state_interface_
.
registerHandle
(
state_handle_Neck
);
#ifndef TEMPERATURE_SENSOR_CONTROLLER
state_handle_arm_3
(
"arm_3_joint"
,
&
pos_
[
2
],
&
vel_
[
2
],
&
eff_
[
2
]);
#else
state_handle_arm_3
(
"arm_3_joint"
,
&
pos_
[
2
],
&
vel_
[
2
],
&
eff_
[
2
],
&
abs_pos_
[
2
],
&
abs_torque_
[
2
]);
#endif
jnt_state_interface_
.
registerHandle
(
state_handle_arm_3
);
hardware_interface
::
JointStateHandle
state_handle_RArm
(
"Rarm"
,
&
pos_
[
3
],
&
vel_
[
3
],
&
eff_
[
3
]);
jnt_state_interface_
.
registerHandle
(
state_handle_RArm
);
#ifndef TEMPERATURE_SENSOR_CONTROLLER
state_handle_arm_4
(
"arm_4_joint"
,
&
pos_
[
3
],
&
vel_
[
3
],
&
eff_
[
3
]);
#else
state_handle_arm_4
(
"arm_4_joint"
,
&
pos_
[
3
],
&
vel_
[
3
],
&
eff_
[
3
],
&
abs_pos_
[
3
],
&
abs_torque_
[
3
]);
#endif
jnt_state_interface_
.
registerHandle
(
state_handle_arm_4
);
hardware_interface
::
JointStateHandle
state_handle_LArm
(
"Larm"
,
&
pos_
[
4
],
&
vel_
[
4
],
&
eff_
[
4
]);
jnt_state_interface_
.
registerHandle
(
state_handle_LArm
);
#ifndef TEMPERATURE_SENSOR_CONTROLLER
state_handle_arm_5
(
"arm_5_joint"
,
&
pos_
[
4
],
&
vel_
[
4
],
&
eff_
[
4
]);
#else
state_handle_arm_5
(
"arm_5_joint"
,
&
pos_
[
4
],
&
vel_
[
4
],
&
eff_
[
4
],
&
abs_pos_
[
4
],
&
abs_torque_
[
4
]);
#endif
jnt_state_interface_
.
registerHandle
(
state_handle_arm_5
);
hardware_interface
::
JointStateHandle
state_handle_RHip
(
"RHip"
,
&
pos_
[
5
],
&
vel_
[
5
],
&
eff_
[
5
]);
jnt_state_interface_
.
registerHandle
(
state_handle_RHip
);
#ifndef TEMPERATURE_SENSOR_CONTROLLER
state_handle_arm_6
(
"arm_6_joint"
,
&
pos_
[
5
],
&
vel_
[
5
],
&
eff_
[
5
]);
#else
state_handle_arm_6
(
"arm_6_joint"
,
&
pos_
[
5
],
&
vel_
[
5
],
&
eff_
[
5
],
&
abs_pos_
[
5
],
&
abs_torque_
[
5
]);
#endif
jnt_state_interface_
.
registerHandle
(
state_handle_arm_6
);
hardware_interface
::
JointStateHandle
state_handle_LHip
(
"LHip"
,
&
pos_
[
6
],
&
vel_
[
6
],
&
eff_
[
6
]);
jnt_state_interface_
.
registerHandle
(
state_handle_LHip
);
#ifndef TEMPERATURE_SENSOR_CONTROLLER
state_handle_arm_7
(
"arm_7_joint"
,
&
pos_
[
6
],
&
vel_
[
6
],
&
eff_
[
6
]);
#else
state_handle_arm_7
(
"arm_7_joint"
,
&
pos_
[
6
],
&
vel_
[
6
],
&
eff_
[
6
],
&
abs_pos_
[
6
],
&
abs_torque_
[
6
]);
#endif
jnt_state_interface_
.
registerHandle
(
state_handle_arm_7
);
registerInterface
(
&
jnt_state_interface_
);
/// connect and register the joint position interface
hardware_interface
::
JointHandle
pos_handle_
base
(
jnt_state_interface_
.
getHandle
(
"
base
"
),
&
cmd_
[
0
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
base
);
hardware_interface
::
JointHandle
pos_handle_
arm_1
(
jnt_state_interface_
.
getHandle
(
"
arm_1_joint
"
),
&
cmd_
[
0
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
arm_1
);
hardware_interface
::
JointHandle
pos_handle_
Head
(
jnt_state_interface_
.
getHandle
(
"
Head
"
),
&
cmd_
[
1
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
Head
);
hardware_interface
::
JointHandle
pos_handle_
arm_2
(
jnt_state_interface_
.
getHandle
(
"
arm_2_joint
"
),
&
cmd_
[
1
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
arm_2
);
hardware_interface
::
JointHandle
pos_handle_
Neck
(
jnt_state_interface_
.
getHandle
(
"
Neck
"
),
&
cmd_
[
2
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
Neck
);
hardware_interface
::
JointHandle
pos_handle_
arm_3
(
jnt_state_interface_
.
getHandle
(
"
arm_3_joint
"
),
&
cmd_
[
2
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
arm_3
);
hardware_interface
::
JointHandle
pos_handle_
RArm
(
jnt_state_interface_
.
getHandle
(
"
R
arm"
),
&
cmd_
[
3
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
RArm
);
hardware_interface
::
JointHandle
pos_handle_
arm_4
(
jnt_state_interface_
.
getHandle
(
"arm
_4_joint
"
),
&
cmd_
[
3
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
arm_4
);
hardware_interface
::
JointHandle
pos_handle_
LArm
(
jnt_state_interface_
.
getHandle
(
"
L
arm"
),
&
cmd_
[
4
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
LArm
);
hardware_interface
::
JointHandle
pos_handle_
arm_5
(
jnt_state_interface_
.
getHandle
(
"arm
_5_joint
"
),
&
cmd_
[
4
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
arm_5
);
hardware_interface
::
JointHandle
pos_handle_
RHip
(
jnt_state_interface_
.
getHandle
(
"
RHip
"
),
&
cmd_
[
5
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
RHip
);
hardware_interface
::
JointHandle
pos_handle_
arm_6
(
jnt_state_interface_
.
getHandle
(
"
arm_6_joint
"
),
&
cmd_
[
5
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
arm_6
);
hardware_interface
::
JointHandle
pos_handle_
LHip
(
jnt_state_interface_
.
getHandle
(
"
LHip
"
),
&
cmd_
[
6
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
LHip
);
hardware_interface
::
JointHandle
pos_handle_
arm_7
(
jnt_state_interface_
.
getHandle
(
"
arm_7_joint
"
),
&
cmd_
[
6
]);
jnt_pos_interface_
.
registerHandle
(
pos_handle_
arm_7
);
registerInterface
(
&
jnt_pos_interface_
);
...
...
@@ -130,26 +168,32 @@ private:
double
pos_
[
7
];
double
vel_
[
7
];
double
eff_
[
7
];
double
abs_pos_
[
7
];
double
abs_torque_
[
7
];
};
int
TestRobot01Class
::
ReadWrite
()
{
return
0
;
}
int
TestRobot01Class
::
UpdateImu
()
{
return
0
;
}
/* get position from STM32 */
int
TestRobot01Class
::
UpdateSensor
()
{
// Write pos
return
0
;
}
/* get cmd from ROS */
int
TestRobot01Class
::
UpdateCmd
()
{
return
0
;
// Read cmd.
}
...
...
@@ -167,7 +211,21 @@ void *RTloop(void *argument)
ros
::
Time
last_time
=
ros
::
Time
::
now
();
while
(
ros
::
ok
)
/// Boucle tant que le master existe (ros::ok())
pthread_t
thread1
;
struct
sched_param
params1
;
params1
.
sched_priority
=
90
;
// 1(low) to 99(high)
thread1
=
pthread_self
();
// int error_return =
pthread_setschedparam
(
thread1
,
SCHED_RR
,
&
params1
);
unsigned
int
nb_it
=
0
;
while
(
ros
::
ok
()
//&& (nb_it < 100)
)
/// Boucle tant que le master existe (ros::ok())
/// and nb_it < 100
{
pthread_mutex_lock
(
&
mutx
);
// On verrouille les variables pour ce thread
...
...
@@ -188,21 +246,13 @@ void *RTloop(void *argument)
pthread_mutex_unlock
(
&
mutx
);
// On deverrouille les variables
/// Pause
usleep
(
10000
);
//100HZ
nb_it
++
;
// std::cout << "nb_it: " << nb_it << "\n";
}
ros
::
shutdown
();
pthread_exit
(
EXIT_SUCCESS
);
}
void
*
ROSloop
(
void
*
argument
)
{
ROS_INFO
(
"IN thread2 OK"
);
ros
::
Rate
loop_rate
(
10
);
/// Frequence boucle en Hz
while
(
ros
::
ok
)
// NOT real time loop
{
loop_rate
.
sleep
();
}
pthread_exit
(
EXIT_SUCCESS
);
}
int
main
(
int
argc
,
char
*
argv
[])
{
...
...
@@ -220,41 +270,19 @@ int main(int argc, char *argv[])
aRTloopArgs
.
cm
=
&
cm
;
aRTloopArgs
.
testrobot01
=
&
testrobot01
;
/*---------------- Initialisation moteurs ------------------ */
ROS_INFO
(
"Starting in 5s"
);
ROS_INFO
(
"-------------------------------- 5"
);
sleep
(
1
);
ROS_INFO
(
"------------------------- 4"
);
sleep
(
1
);
ROS_INFO
(
"----------------- 3"
);
sleep
(
1
);
ROS_INFO
(
"--------- 2"
);
sleep
(
1
);
ROS_INFO
(
"--- 1"
);
sleep
(
1
);
/*---------------- Gestion des threads ------------------ */
/*
Thread "number 1" : Main
Thread "number 2" : Rosloop
Thread "number 3" : RTloop
Thread "number 4" : spinner
Thread "number 1" : Main - spinner
Thread "number 2" : RTloop
*/
pthread_t
thread1
;
pthread_t
thread2
;
int
error_return
;
struct
sched_param
params1
;
params1
.
sched_priority
=
90
;
// 1(low) to 99(high)
error_return
=
pthread_setschedparam
(
thread1
,
SCHED_RR
,
&
params1
);
pthread_t
thread1
;
// function sets the scheduling policy and parameters of the thread
error_return
=
pthread_create
(
&
thread1
,
NULL
,
RTloop
,
(
void
*
)
&
aRTloopArgs
);
// create a new thread
ROS_INFO
(
"thread1 created OK"
);
error_return
=
pthread_create
(
&
thread2
,
NULL
,
ROSloop
,(
void
*
)
&
aRTloopArgs
);
// create a new thread
ROS_INFO
(
"thread2 created OK"
);
if
(
error_return
)
{
...
...
tests/roscontrol_sot_hardware.hh
View file @
af2e0f25
/*
* Copyright: LAAS CNRS, 2019
* Author: O. Stasse
* LICENSE: See LICENSE.txt
*
*/
#pragma GCC diagnostic push
#pragma GCC system_header
#include
<ros/ros.h>
...
...
tests/sot-test-controller.cpp
0 → 100644
View file @
af2e0f25
/*
* Copyright 2016,
*
* Olivier Stasse
*
* LAAS, CNRS
*
*
*/
#include
<sot/core/debug.hh>
#include
<sot/core/exception-abstract.hh>
#include
<dynamic_graph_bridge/ros_init.hh>
#include
<dynamic_graph_bridge/ros_interpreter.hh>
#include
"sot-test-controller.hh"
#include
<boost/thread/thread.hpp>
#include
<boost/thread/condition.hpp>
#include
<ros/console.h>
#define ENABLE_RT_LOG
#include
<dynamic-graph/real-time-logger.h>
const
std
::
string
SoTTestController
::
LOG_PYTHON
=
"/tmp/TestController_python.out"
;
using
namespace
std
;
boost
::
condition_variable
cond
;
boost
::
mutex
mut
;
bool
data_ready
;
void
workThread
(
SoTTestController
*
aSoTTest
)
{
dynamicgraph
::
Interpreter
aLocalInterpreter
(
dynamicgraph
::
rosInit
(
false
,
true
));
aSoTTest
->
interpreter_
=
boost
::
make_shared
<
dynamicgraph
::
Interpreter
>
(
aLocalInterpreter
);
std
::
cout
<<
"Going through the thread."
<<
std
::
endl
;
{
boost
::
lock_guard
<
boost
::
mutex
>
lock
(
mut
);
data_ready
=
true
;
}
cond
.
notify_all
();
ros
::
waitForShutdown
();
}
SoTTestController
::
SoTTestController
(
std
::
string
RobotName
)
:
device_
(
new
SoTTestDevice
(
RobotName
))
{
init
();
}
SoTTestController
::
SoTTestController
(
const
char
robotName
[])
:
device_
(
new
SoTTestDevice
(
robotName
))