Skip to content
GitLab
Commit af2e0f25 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[tests] Add unit tests.

parent 8fae02ae
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.
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("Rarm"), &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("Larm"), &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))