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 <ros/ros.h>
      11             : #include <ros_end_effector/Utils.h>
      12             : 
      13             : 
      14             : /** Utils funcion to create process to run roscore,
      15             :  * gently copied from https://github.com/ADVRHumanoids/CartesianInterface/blob/refactor2020/tests/testutils.h
      16             :  */
      17             : 
      18             : namespace ROSEE {
      19             :     
      20             : namespace TestUtils {
      21             : 
      22             : class Process
      23             : {
      24             : 
      25             : public:
      26             : 
      27             :     Process(std::vector<std::string> args);
      28             : 
      29             :     int wait();
      30             : 
      31             :     void kill(int signal = SIGTERM);
      32             : 
      33             :     ~Process();
      34             : 
      35             : private:
      36             : 
      37             :     std::string _name;
      38             :     pid_t _pid;
      39             : 
      40             : };
      41             : 
      42          69 : Process::Process(std::vector<std::string>  args):
      43          69 :     _name(args[0])
      44             : {
      45         138 :     std::vector<const char *> args_cstr;
      46          69 :     for(auto& a : args) args_cstr.push_back(a.c_str());
      47          69 :     args_cstr.push_back(nullptr);
      48             : 
      49          69 :     char ** argv = (char**)args_cstr.data();
      50             : 
      51          69 :     _pid = ::fork();
      52             : 
      53          69 :     if(_pid == -1)
      54             :     {
      55           0 :         perror("fork");
      56           0 :         throw std::runtime_error("Unable to fork()");
      57             :     }
      58             : 
      59          69 :     if(_pid == 0)
      60             :     {
      61           0 :         ::execvp(argv[0], argv);
      62           0 :         perror("execvp");
      63           0 :         throw std::runtime_error("Unknown command");
      64             :     }
      65             : 
      66          69 : }
      67             : 
      68          69 : int Process::wait()
      69             : {
      70             :     int status;
      71          69 :     while(::waitpid(_pid, &status, 0) != _pid);
      72          69 :     printf("Child process '%s' exited with status %d\n", _name.c_str(), status);
      73          69 :     return status;
      74             : 
      75             : }
      76             : 
      77          69 : void Process::kill(int signal)
      78             : {
      79          69 :     ::kill(_pid, signal);
      80          69 :     printf("Killed process '%s' with signal %d\n", _name.c_str(), signal);
      81          69 : }
      82             : 
      83         138 : Process::~Process()
      84             : {
      85          69 :     kill(SIGINT);
      86          69 :     wait();
      87          69 : }
      88             : 
      89             : /**
      90             :  * @brief Function to be called in the main of each test, it runs roscore and fill
      91             :  * parameter server with robot models
      92             :  * 
      93             :  * @return a not 0 if some error happens
      94             :  */
      95          24 : int prepareROSForTests ( int argc, char **argv, std::string testName ) {
      96             :     
      97             : 
      98             :     
      99          24 :     ros::init ( argc, argv, testName );
     100             :         
     101             :     /////////////////////////// I cant manage to make this working, to wait the roscore
     102             :     //ros::Time::init();
     103             :     //while (!ros::master::check()) //wait for roscore to be ready
     104             :     //{
     105             :     //    std::cout << "waiting for roscore..." << std::endl;
     106             :     //    ros::Duration(0.2).sleep();
     107             :     //}
     108             :     ////////////////////////////////////////////////////////////////////////////////
     109             :     
     110             :     //fill ros param with file models, needed by moveit parserMoveIt
     111          48 :     std::string modelPathURDF = ROSEE::Utils::getPackagePath() + "configs/urdf/" + argv[1];
     112          48 :     std::string modelPathSRDF = ROSEE::Utils::getPackagePath() + "configs/srdf/" + argv[1];
     113             : 
     114             :     //Is there a better way to parse?
     115          48 :     std::ifstream urdf(modelPathURDF + ".urdf");
     116          48 :     std::ifstream srdf(modelPathSRDF + ".srdf");
     117          48 :     std::stringstream sUrdf, sSrdf;
     118          24 :     sUrdf << urdf.rdbuf();
     119          24 :     sSrdf << srdf.rdbuf();
     120             : 
     121          24 :     ros::param::set("robot_description" , sUrdf.str());
     122          24 :     ros::param::set("robot_description_semantic" , sSrdf.str());
     123          24 :     ros::param::set("robot_name", argv[1]);
     124             :     
     125          48 :     return 0;
     126             : }
     127             : 
     128             : 
     129             : } //namespace TestUtils
     130             : 
     131             : } //namespace ROSEE
     132             : 
     133             : #endif // TESTUTILS_H