testUtils.h

Go to the documentation of this file.

#ifndef TESTUTILS_H
#define TESTUTILS_H

#include <unistd.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/wait.h>

//for the function prepareROSForTests
#include <ros/ros.h>
#include <end_effector/Utils.h>


namespace ROSEE {
    
namespace TestUtils {

class Process
{

public:

    Process(std::vector<std::string> args);

    int wait();

    void kill(int signal = SIGTERM);

    ~Process();

private:

    std::string _name;
    pid_t _pid;

};

Process::Process(std::vector<std::string>  args):
    _name(args[0])
{
    std::vector<const char *> args_cstr;
    for(auto& a : args) args_cstr.push_back(a.c_str());
    args_cstr.push_back(nullptr);

    char ** argv = (char**)args_cstr.data();

    _pid = ::fork();

    if(_pid == -1)
    {
        perror("fork");
        throw std::runtime_error("Unable to fork()");
    }

    if(_pid == 0)
    {
        ::execvp(argv[0], argv);
        perror("execvp");
        throw std::runtime_error("Unknown command");
    }

}

int Process::wait()
{
    int status;
    while(::waitpid(_pid, &status, 0) != _pid);
    printf("Child process '%s' exited with status %d\n", _name.c_str(), status);
    return status;

}

void Process::kill(int signal)
{
    ::kill(_pid, signal);
    printf("Killed process '%s' with signal %d\n", _name.c_str(), signal);
}

Process::~Process()
{
    kill(SIGINT);
    wait();
}

int prepareROSForTests ( int argc, char **argv, std::string testName ) {
    

    
    ros::init ( argc, argv, testName );
        
    //ros::Time::init();
    //while (!ros::master::check()) //wait for roscore to be ready
    //{
    //    std::cout << "waiting for roscore..." << std::endl;
    //    ros::Duration(0.2).sleep();
    //}
    
    //fill ros param with file models, needed by moveit parserMoveIt
    std::string modelPathURDF = ROSEE::Utils::getPackagePath() + "configs/urdf/" + argv[1];
    std::string modelPathSRDF = ROSEE::Utils::getPackagePath() + "configs/srdf/" + argv[1];

    //Is there a better way to parse?
    std::ifstream urdf(modelPathURDF + ".urdf");
    std::ifstream srdf(modelPathSRDF + ".srdf");
    std::stringstream sUrdf, sSrdf;
    sUrdf << urdf.rdbuf();
    sSrdf << srdf.rdbuf();

    ros::param::set("robot_description" , sUrdf.str());
    ros::param::set("robot_description_semantic" , sSrdf.str());
    ros::param::set("robot_name", argv[1]);
    
    return 0;
}


} //namespace TestUtils

} //namespace ROSEE

#endif // TESTUTILS_H