Line data    Source code

       1             : #ifndef TESTUTILS_H
       2             : #define TESTUTILS_H
       3             : 
       4             : #include <unistd.h>
       5             : #include <stdio.h>
       6             : #include <sys/types.h>
       7             : #include <sys/wait.h>
       8             : 
       9             : //for the function prepareROSForTests
      10             : #include <rclcpp/rclcpp.hpp>
      11             : #include <end_effector/Utils.h>
      12             : 
      13             : #include <ament_index_cpp/get_package_share_directory.hpp>
      14             : #include <ament_index_cpp/get_package_prefix.hpp>
      15             : 
      16             : 
      17             : /** Utils funcion to create process to run roscore,
      18             :  * gently copied from https://github.com/ADVRHumanoids/CartesianInterface/blob/refactor2020/tests/testutils.h
      19             :  */
      20             : 
      21             : namespace ROSEE {
      22             :     
      23             : namespace TestUtils {
      24             : 
      25             : class Process
      26             : {
      27             : 
      28             : public:
      29             : 
      30             :     Process(std::vector<std::string> args);
      31             : 
      32             :     int wait();
      33             : 
      34             :     void kill(int signal = SIGTERM);
      35             : 
      36             :     ~Process();
      37             : 
      38             : private:
      39             : 
      40             :     std::string _name;
      41             :     pid_t _pid;
      42             : 
      43             : };
      44             : 
      45          41 : Process::Process(std::vector<std::string>  args):
      46          41 :     _name(args[0])
      47             : {
      48          82 :     std::vector<const char *> args_cstr;
      49         205 :     for(auto& a : args) args_cstr.push_back(a.c_str());
      50          41 :     args_cstr.push_back(nullptr);
      51             : 
      52          41 :     char ** argv = (char**)args_cstr.data();
      53             : 
      54          41 :     _pid = ::fork();
      55             : 
      56          82 :     if(_pid == -1)
      57             :     {
      58           0 :         perror("fork");
      59           0 :         throw std::runtime_error("Unable to fork()");
      60             :     }
      61             : 
      62          82 :     if(_pid == 0)
      63             :     {
      64         164 :         std::vector<std::string> source_args{ament_index_cpp::get_package_prefix("end_effector") + "/../setup.bash"};
      65          41 :         char** source_args_char = (char**)source_args.data();
      66             :         
      67          41 :         ::execvp("source", source_args_char);
      68          41 :         ::execvp(argv[0], argv);
      69          41 :         perror("execvp");
      70           0 :         throw std::runtime_error("Unknown command");
      71             :     }
      72             : 
      73          41 : }
      74             : 
      75          41 : int Process::wait()
      76             : {
      77             :     int status;
      78          41 :     while(::waitpid(_pid, &status, 0) != _pid);
      79          41 :     printf("Child process '%s' exited with status %d\n", _name.c_str(), status);
      80          41 :     return status;
      81             : 
      82             : }
      83             : 
      84          41 : void Process::kill(int signal)
      85             : {
      86          41 :     ::kill(_pid, signal);
      87          41 :     printf("Killed process '%s' with signal %d\n", _name.c_str(), signal);
      88          41 : }
      89             : 
      90          41 : Process::~Process()
      91             : {
      92          41 :     kill(SIGINT);
      93          41 :     wait();
      94          41 : }
      95             : 
      96             : /**
      97             :  * @brief Function to be called in the main of each test, it runs roscore and fill
      98             :  * parameter server with robot models
      99             :  * 
     100             :  * @return a not 0 if some error happens
     101             :  */
     102         172 : rclcpp::Node::SharedPtr prepareROSForTests ( int argc, char **argv, std::string testName ) {
     103             :     
     104         172 :     auto node = rclcpp::Node::make_shared(testName);
     105             :     
     106             :     //fill ros param with file models, needed by moveit parserMoveIt
     107         516 :     std::string modelPathURDF = ament_index_cpp::get_package_share_directory("end_effector") + "/configs/urdf/" + argv[1];
     108         516 :     std::string modelPathSRDF = ament_index_cpp::get_package_share_directory("end_effector") + "/configs/srdf/" + argv[1];
     109             : 
     110             :     //Is there a better way to parse?
     111         344 :     std::ifstream urdf(modelPathURDF + ".urdf");
     112         344 :     std::ifstream srdf(modelPathSRDF + ".srdf");
     113         344 :     std::stringstream sUrdf, sSrdf;
     114         172 :     sUrdf << urdf.rdbuf();
     115         172 :     sSrdf << srdf.rdbuf();
     116             : 
     117         172 :     node->declare_parameter("robot_description", sUrdf.str()); 
     118         172 :     node->declare_parameter("robot_description_semantic", sSrdf.str()); 
     119             : //     node->declare_parameter("robot_name", argv[1]); 
     120             :     
     121         172 :     node->set_parameter(rclcpp::Parameter("robot_description", sUrdf.str()));
     122         172 :     node->set_parameter(rclcpp::Parameter("robot_description_semantic", sSrdf.str()));
     123             : //     node->set_parameter(rclcpp::Parameter("robot_name", argv[1]));
     124             : 
     125         344 :     return node;
     126             : }
     127             : 
     128             : 
     129             : } //namespace TestUtils
     130             : 
     131             : } //namespace ROSEE
     132             : 
     133             : 
     134             : namespace {
     135             :     
     136             : int argc_g;    
     137             : char** argv_g;
     138             : 
     139             : class MyTestEnvironment : public testing::Environment {
     140             :     
     141             : public:
     142             :     
     143          28 :     explicit MyTestEnvironment(int argc, char **argv ) {
     144          28 :         argc_g = argc;
     145          28 :         argv_g = argv;
     146          28 :     }
     147             : };
     148             :     
     149             : }
     150             : 
     151             : #endif // TESTUTILS_H