Commit 5feea985 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Add unit-test

parent d5f786a7
......@@ -61,13 +61,14 @@ add_required_dependency(roscpp)
add_required_dependency("realtime_tools >= 1.8")
add_required_dependency("dynamic_graph_bridge")
add_required_dependency("controller_interface")
add_required_dependency("controller_manager")
add_required_dependency("pal_common_msgs")
add_required_dependency("dynamic-graph")
# 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
roscpp realtime_tools message_runtime dynamic_graph_bridge pal_hardware_interfaces controller_interface controller_manager
LIBRARIES rcsot_controller)
# Detect the controller interface version to switch code
......@@ -103,7 +104,8 @@ src/log.cpp
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(roscontrol_sot_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# add_dependencies(roscontrol_sot_node ${${PROJECT_NAME}_EXPORTED_TARGETS}
# ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(rcsot_controller
......@@ -111,8 +113,10 @@ target_link_libraries(rcsot_controller
${bullet_LIBRARIES}
)
pkg_config_use_dependency(rcsot_controller urdfdom optional NO_INCLUDE_SYSTEM)
pkg_config_use_dependency(rcsot_controller dynamic-graph optional NO_INCLUDE_SYSTEM)
pkg_config_use_dependency(rcsot_controller urdfdom optional
NO_INCLUDE_SYSTEM)
pkg_config_use_dependency(rcsot_controller dynamic-graph optional
NO_INCLUDE_SYSTEM)
## Mark executables and/or libraries for installation
install(TARGETS rcsot_controller DESTINATION lib )
......@@ -127,4 +131,6 @@ foreach(dir config launch)
)
endforeach()
ADD_SUBDIRECTORY(tests)
SETUP_PROJECT_FINALIZE()
......@@ -42,6 +42,7 @@
<build_depend>sensor_msgs</build_depend>
<build_depend>realtime_tools</build_depend>
<build_depend>controller_interface</build_depend>
<build_depend>controller_manager</build_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>dynamic_graph_bridge</build_depend>
<build_depend>pal_hardware_interfaces</build_depend>
......@@ -54,6 +55,7 @@
<run_depend>pluginlib</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>controller_interface</run_depend>
<run_depend>controller_manager</run_depend>
<run_depend>cmake_modules</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>dynamic_graph_bridge</run_depend>
......
# Copyright (C) 2017-2019 LAAS-CNRS
#
# Author: Olivier Stasse
#
INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIRECTORY}/tests)
ADD_EXECUTABLE(roscontrol_sot_hardware roscontrol_sot_hardware.cpp)
pkg_config_use_dependency(roscontrol_sot_hardware controller_manager
NO_INCLUDE_SYSTEM)
target_link_libraries(roscontrol_sot_hardware
${catkin_LIBRARIES}
${bullet_libraries}
)
/*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
*/
#include <iostream>
#include <sstream>
#include <roscontrol_sot_hardware.hh>
//Threads
void *RTloop(void *argument);
void *ROSloop(void *argument);
pthread_mutex_t mutx = PTHREAD_MUTEX_INITIALIZER; //mutex initialisation
//******************************************************
class TestRobot01Class : public hardware_interface::RobotHW
{
public:
int ReadWrite();
int UpdateImu();
int UpdateCmd();
int UpdateSensor();
TestRobot01Class() //ros::NodeHandle nh);
{
/// 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);
hardware_interface::JointStateHandle
state_handle_Head("Head", &pos_[1], &vel_[1], &eff_[1]);
jnt_state_interface_.registerHandle(state_handle_Head);
hardware_interface::JointStateHandle
state_handle_Neck("Neck", &pos_[2], &vel_[2], &eff_[2]);
jnt_state_interface_.registerHandle(state_handle_Neck);
hardware_interface::JointStateHandle
state_handle_RArm("Rarm", &pos_[3], &vel_[3], &eff_[3]);
jnt_state_interface_.registerHandle(state_handle_RArm);
hardware_interface::JointStateHandle
state_handle_LArm("Larm", &pos_[4], &vel_[4], &eff_[4]);
jnt_state_interface_.registerHandle(state_handle_LArm);
hardware_interface::JointStateHandle
state_handle_RHip("RHip", &pos_[5], &vel_[5], &eff_[5]);
jnt_state_interface_.registerHandle(state_handle_RHip);
hardware_interface::JointStateHandle
state_handle_LHip("LHip", &pos_[6], &vel_[6], &eff_[6]);
jnt_state_interface_.registerHandle(state_handle_LHip);
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_Head
(jnt_state_interface_.getHandle("Head"), &cmd_[1]);
jnt_pos_interface_.registerHandle(pos_handle_Head);
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_RArm
(jnt_state_interface_.getHandle("Rarm"), &cmd_[3]);
jnt_pos_interface_.registerHandle(pos_handle_RArm);
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_RHip
(jnt_state_interface_.getHandle("RHip"), &cmd_[5]);
jnt_pos_interface_.registerHandle(pos_handle_RHip);
hardware_interface::JointHandle pos_handle_LHip
(jnt_state_interface_.getHandle("LHip"), &cmd_[6]);
jnt_pos_interface_.registerHandle(pos_handle_LHip);
registerInterface(&jnt_pos_interface_);
///IMU
hardware_interface::ImuSensorHandle::Data anImuData;
anImuData.name = "name";
anImuData.frame_id = "frame_id";
anImuData.orientation = imu_orientation_;
anImuData.orientation_covariance =
imu_orientation_covariance_;
anImuData.angular_velocity = imu_angular_velocity_;
anImuData.angular_velocity_covariance =
imu_angular_velocity_covariance_;
anImuData.linear_acceleration = imu_linear_acceleration_;
anImuData.linear_acceleration_covariance =
imu_linear_acceleration_covariance_;
hardware_interface::ImuSensorHandle
IMU_handle(anImuData);
imu_state_interface_.registerHandle(IMU_handle);
registerInterface(&imu_state_interface_);
}
double imu_orientation_[6];
double imu_orientation_covariance_[9];
double imu_angular_velocity_[3];
double imu_angular_velocity_covariance_[9];
double imu_linear_acceleration_[3];
double imu_linear_acceleration_covariance_[9];
private:
hardware_interface::JointStateInterface jnt_state_interface_;
hardware_interface::PositionJointInterface jnt_pos_interface_;
hardware_interface::ImuSensorInterface imu_state_interface_;
double cmd_[7];
double pos_[7];
double vel_[7];
double eff_[7];
};
int TestRobot01Class::ReadWrite()
{
}
int TestRobot01Class::UpdateImu()
{
}
/* get position from STM32 */
int TestRobot01Class::UpdateSensor()
{
// Write pos
}
/* get cmd from ROS */
int TestRobot01Class::UpdateCmd()
{
// Read cmd.
}
typedef struct arg_struct
{
controller_manager::ControllerManager * cm;
TestRobot01Class *testrobot01;
} RTloopArgs;
void *RTloop(void *argument)
{
ROS_INFO("IN thread1 OK");
RTloopArgs * aRTloopArgs;
aRTloopArgs = (RTloopArgs *)argument;
ros::Time last_time = ros::Time::now();
while (ros::ok) /// Boucle tant que le master existe (ros::ok())
{
pthread_mutex_lock(&mutx); // On verrouille les variables pour ce thread
// Read phase
aRTloopArgs->testrobot01->ReadWrite();
//Receiving data only one time
aRTloopArgs->testrobot01->UpdateImu(); //Processing incoming data
aRTloopArgs->testrobot01->UpdateSensor(); //Processing incoming data
// Call Controllers
ros::Duration duration = ros::Time::now() - last_time;
aRTloopArgs->cm->update(ros::Time::now(), duration);
last_time = ros::Time::now();
/// Write Phase
aRTloopArgs->testrobot01->UpdateCmd();
pthread_mutex_unlock(&mutx); // On deverrouille les variables
/// Pause
usleep(10000); //100HZ
}
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[])
{
TestRobot01Class testrobot01;
/*---------------- ROS Stuff ------------------ */
/* Initialisation du node : le troisieme argument est son nom */
ros::init(argc, argv, "hw_node");
/* Connexion au master et initialisation du NodeHandle qui permet
d avoir acces aux topics et services */
ros::NodeHandle nh;//testrobot01_nh;
controller_manager::ControllerManager cm(&testrobot01);
RTloopArgs aRTloopArgs;
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
*/
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);
// 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)
{
ROS_ERROR("return code from pthread_create() is %d\n", error_return);
exit(-1);
}
/* Spinners */
ros::MultiThreadedSpinner spinner(4); //unspecified (or set to 0),
// it will use a thread for each CPU core
spinner.spin(); // spin() will not return
// until the node has been shutdown
return 0;
}
#pragma GCC diagnostic push
#pragma GCC system_header
#include <ros/ros.h>
#include <controller_manager/controller_manager.h>
#include <hardware_interface/imu_sensor_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#pragma GCC diagnostic pop
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