Add unit-test

CMakeLists.txt

@@ -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()

package.xml

@@ -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>

tests/CMakeLists.txt

+# 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}
+  )

tests/roscontrol_sot_hardware.cpp

+/*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;
+}

tests/roscontrol_sot_hardware.hh

+#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