예제 #1
0
파일: main.cpp 프로젝트: xufango/contrib_bk
	/*
	 * Configure function. Receive a previously initialized
	 * resource finder object. Use it to configure your module.
	 * Open port and attach it to message handler.
	 */
	bool configure(yarp::os::ResourceFinder &rf)
	{

		imp.controlled_part = rf.find("part").asString();
		imp.robotname = rf.find("robot").asString();

		if(imp.controlled_part == "right_arm")
		{
			imp.limb = new iCubArm("right");
			imp.gcomp_port = GCOMP_PORT_RIGHT_ARM;
		}
		else if(imp.controlled_part == "left_arm")
		{
			imp.limb = new iCubArm("left");
			imp.gcomp_port = GCOMP_PORT_LEFT_ARM;
		}
		else
		{
			fprintf(stderr, "part not recognised\n");
			return false;
		}

		if (!imp.open())
		{
			fprintf(stderr, "Error opening detector\n");
			return false;
		}
		fflush(stdout);
		return true;
	}
예제 #2
0
qavimator::qavimator(yarp::os::ResourceFinder &config, QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::qavimator)
{
    ui->setupUi(this);
    setupToolBar();
    ui->animationView->init(config);
    nFPS=10;

    width=850;
    height=600;


    readSettings();
    // default size

    if (config.check("name")) {
        GUI_NAME=std::string(config.find("name").asString().c_str());
    }
    if (GUI_NAME[0]!='/') {
        GUI_NAME=std::string("/")+GUI_NAME;
    }
    if (config.check("width")) {
        width=config.find("width").asInt();
    }
    if (config.check("height")) {
        height=config.find("height").asInt();
    }

    //sanity check
    if(width<100) {
        width=100;
    }
    if(height<100) {
        height=100;
    }

    this->resize(width, height);

    int xpos=32,ypos=32;
    if (config.check("xpos")) {
        xpos=config.find("xpos").asInt();
    }
    if (config.check("ypos")) {
        ypos=config.find("ypos").asInt();
    }
    this->move(xpos,ypos);

    setWindowTitle(GUI_NAME.c_str());

    connect(ui->animationView,SIGNAL(backgroundClicked()),this,SLOT(backgroundClicked()));
    connect(this,SIGNAL(resetCamera()),ui->animationView,SLOT(resetCamera()));

    ui->animationView->startTimer(1000/nFPS);


}
예제 #3
0
bool ShowModule::configure(yarp::os::ResourceFinder &rf)
{
    //Ports
    string name=rf.check("name",Value("show3D")).asString().c_str();
    string robot = rf.check("robot",Value("icub")).asString().c_str();
    string cloudpath_file = rf.check("from",Value("cloudsPath.ini")).asString().c_str();
    rf.findFile(cloudpath_file.c_str());

    ResourceFinder cloudsRF;    
    cloudsRF.setDefaultConfigFile(cloudpath_file.c_str());
    cloudsRF.configure(0,NULL);

    // Set the path that contains previously saved pointclouds
    if (rf.check("clouds_path")){
        cloudpath = rf.find("clouds_path").asString().c_str();
    }else{
        string defPathFrom = "/share/ICUBcontrib/contexts/toolIncorporation/sampleClouds/";
        string localModelsPath    = rf.check("local_path")?rf.find("local_path").asString().c_str():defPathFrom;     //cloudsRF.find("clouds_path").asString();
        string icubContribEnvPath = yarp::os::getenv("ICUBcontrib_DIR");
        cloudpath  = icubContribEnvPath + localModelsPath;
    }

    handlerPort.open("/"+name+"/rpc:i");
        attach(handlerPort);

    cloudsInPort.open("/"+name+"/clouds:i");

    // Module rpc parameters
    closing = false;

    // Init variables
    cloudfile = "cloud.ply";

    cloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr (new pcl::PointCloud<pcl::PointXYZRGB>);

    //Threads
    visThrd = new VisThread(50, "Cloud");
    if (!visThrd->start())
    {
        delete visThrd;
        visThrd = 0;
        cout << "\nERROR!!! visThread wasn't instantiated!!\n";
        return false;
    }
    cout << "PCL visualizer Thread istantiated...\n";
    cout << endl << "Configuring done."<<endl;

    printf("Base path: %s \n \n",cloudpath.c_str());

    return true;
}
bool AudioPowerMapModule::configure(yarp::os::ResourceFinder &rf) {

	yInfo("Configuring the module");

	// get the module name which will form the stem of all module port names 
	moduleName = rf.check("name", Value("/AudioPowerMap"), "module name (string)").asString();


	// before continuing, set the module name before getting any other parameters, 
	// specifically the port names which are dependent on the module name
	setName(moduleName.c_str());


	// get the robot name which will form the stem of the robot ports names
	// and append the specific part and device required
	robotName     = rf.check("robot", Value("icub"), "Robot name (string)").asString();
	robotPortName =  "/" + robotName + "/head";
	inputPortName =  rf.check("inputPortName", Value(":i"),	"Input port name (string)").asString();


	// attach a port of the same name as the module (prefixed with a /) to the module
	// so that messages received from the port are redirected to the respond method
	handlerPortName =  "";
	handlerPortName += getName();         

	if (!handlerPort.open(handlerPortName.c_str())) {           
		std::cout << getName() << ": Unable to open port " << handlerPortName << std::endl;  
		return false;
	}

	// attach to port
	attach(handlerPort);                  
	if (rf.check("config")) {
		configFile=rf.findFile(rf.find("config").asString().c_str());
		if (configFile=="") {
			return false;
		}
	}

	else {
		configFile.clear();
	}

	// create the thread and pass pointers to the module parameters
	apr = new AudioPowerMapRatethread(robotName, configFile, rf);
	apr->setName(moduleName);

	// now start the thread to do the work 
	// this calls threadInit() and it if returns true, it then calls run()
	bool ret = apr->start(); 

	// let the RFModule know everything went well
	// so that it will then run the module
	return ret;         
}
예제 #5
0
bool Module::configure(yarp::os::ResourceFinder &rf)
{
    // Parse the configuration file.
    controller_options.robotName = rf.check("robot") ? rf.find("robot").asString().c_str() : "icub";
    controller_options.threadPeriod = rf.check("threadPeriod") ? rf.find("threadPeriod").asInt() : DEFAULT_THREAD_PERIOD;

    dangerPeriodLoopTime = 1.3 * controller_options.threadPeriod;

    controller_options.serverName = rf.check("local") ? rf.find("local").asString().c_str() : "OcraControllerServer";
    controller_options.runInDebugMode = rf.check("debug");
    controller_options.noOutputMode = rf.check("noOutput");
    controller_options.isFloatingBase = rf.check("floatingBase");
    controller_options.useOdometry = rf.check("useOdometry");
    controller_options.idleAnkles = rf.check("idleAnkles");
    if ( controller_options.idleAnkles ) {
        if ( !rf.find("idleAnkles").isNull() ) {
            controller_options.idleAnkleTime = rf.find("idleAnkles").asDouble();
            if ( controller_options.idleAnkleTime < 0.0 ) {
                controller_options.idleAnkleTime = 0.0;
                OCRA_WARNING("Ankle idling time must be > 0. Setting to 0.0s.")
            }
        }
예제 #6
0
void AdaptiveLayer::configureSpeech(yarp::os::ResourceFinder &rf)
{
    //Port for broadcasting recognized keywords to other modules
    //pSpeechRecognizerKeywordOut.open("/"+getName()+"/speechGrammar/keyword:o");

    //Set the tts options
    string ttsOptions = rf.check("ttsOptions",Value("iCub")).asString().c_str();
    iCub->getSpeechClient()->SetOptions(ttsOptions);

    //Populate the speech reco if needed
    bool shouldPopulateGrammar = rf.find("shouldPopulateGrammar").asInt() == 1;
    if (shouldPopulateGrammar)
        populateSpeechRecognizerVocabulary();
}
    bool configure(yarp::os::ResourceFinder &rf)
    {
	path = rf.find("clouds_path").asString();        
	printf("Path: %s",path.c_str());		
	handlerPort.open("/mergeModule");
        attach(handlerPort);

	/* Module rpc parameters */
	visualizing = false;
	saving = true;
        closing = false;

	/*Init variables*/
	initF = true;	
	filesScanned = 0;

	cout<<"Configuring done."<<endl;

        return true;
    }
예제 #8
0
bool   SimToolLoaderModule::configure(yarp::os::ResourceFinder &rf) {

    Bottle table;
    Bottle temp;
    string objectName = "obj";

    /* module name */
    moduleName = rf.check("name", Value("simtoolloader"),
                          "Module name (string)").asString();

    setName(moduleName.c_str());

    /* Hand used */
    hand=rf.find("hand").asString().c_str();
    if ((hand!="left") && (hand!="right"))
        hand="right";

    /* port names */
    simToolLoaderSimOutputPortName  = "/";
    simToolLoaderSimOutputPortName += getName( rf.check("simToolLoaderSimOutputPort",
                                     Value("/sim:rpc"),
                                     "Loader output port(string)")
                                     .asString() );

    simToolLoaderCmdInputPortName  = "/";
    simToolLoaderCmdInputPortName += getName( rf.check("simToolLoaderCmdInputPort",
                                     Value("/cmd:i"),
                                     "Loader input port(string)")
                                     .asString() );

    /* open ports */
    if (!simToolLoaderSimOutputPort.open(
            simToolLoaderSimOutputPortName.c_str())) {

        cout << getName() << ": unable to open port"
        << simToolLoaderSimOutputPortName << endl;
        return false;
    }

    if (!simToolLoaderCmdInputPort.open(
            simToolLoaderCmdInputPortName.c_str())) {

        cout << getName() << ": unable to open port"
        << simToolLoaderCmdInputPortName << endl;
        return false;
    }

    /* Rate thread period */
    threadPeriod = rf.check("threadPeriod", Value(0.5),
        "Control rate thread period key value(double) in seconds ").asDouble();

    /* Read Table Configuration */
    table = rf.findGroup("table");

    /* Read the Objects configurations */
    vector<Bottle> object;

    cout << "Loading objects to buffer" << endl;
    bool noMoreModels = false;
    int n =0;
    while (!noMoreModels){      // read until there are no more objects.
        stringstream s;
        s.str("");
        s << objectName << n;
        string objNameNum = s.str();
        temp = rf.findGroup("objects").findGroup(objNameNum);

        if(!temp.isNull()) {
            cout << "object" << n << ", id: " << objNameNum;
            cout << ", model: " << temp.get(2).asString() << endl;
            object.push_back(temp);
            temp.clear();
            n++;
        }else {
            noMoreModels = true;
        }
    }
    numberObjs = n;

    cout << "Loaded " << object.size() << " objects " << endl;

    /* Create the control rate thread */
    ctrlThread = new CtrlThread(&simToolLoaderSimOutputPort,
                                &simToolLoaderCmdInputPort,
                                threadPeriod,
                                table, object,hand);
    /* Starts the thread */
    if (!ctrlThread->start()) {
        delete ctrlThread;
        return false;
    }

    return true;
}
bool TorqueBalancingReferencesGenerator::configure (yarp::os::ResourceFinder &rf)
{
    using namespace yarp::os;
    using namespace yarp::sig;
    using namespace Eigen;

    
    if (!rf.check("wbi_config_file", "Checking wbi configuration file")) {
        std::cout << "No WBI configuration file found.\n";
        return false;
    }
    
    Property wbiProperties;
    if (!wbiProperties.fromConfigFile(rf.findFile("wbi_config_file"))) {
        std::cout << "Not possible to load WBI properties from file.\n";
        return false;
    }
    wbiProperties.fromString(rf.toString(), false);

    //retrieve the joint list
    std::string wbiList = rf.find("wbi_joint_list").asString();

    wbi::IDList iCubMainJoints;
    if (!yarpWbi::loadIdListFromConfig(wbiList, wbiProperties, iCubMainJoints)) {
        std::cout << "Cannot find joint list\n";
        return false;
    }

    size_t actuatedDOFs = iCubMainJoints.size();

    //create an instance of wbi
    m_robot = new yarpWbi::yarpWholeBodyInterface("torqueBalancingReferencesGenerator", wbiProperties);
    if (!m_robot) {
        std::cout << "Could not create wbi object.\n";
        return false;
    }

    m_robot->addJoints(iCubMainJoints);
    if (!m_robot->init()) {
        std::cout << "Could not initialize wbi object.\n";
        return false;
    }
    
    robotName = rf.check("robot", Value("icubSim"), "Looking for robot name").asString();
//     numberOfPostures = rf.check("numberOfPostures", Value(0), "Looking for numberOfPostures").asInt();

    directionOfOscillation.resize(3,0.0);
    frequencyOfOscillation = 0;
    amplitudeOfOscillation = 0;

    if (!loadReferences(rf,postures,comTimeAndSetPoints,actuatedDOFs,changePostural, changeComWithSetPoints,amplitudeOfOscillation,frequencyOfOscillation, directionOfOscillation  ))
    {
        return false;
    }
    loadReferences(rf,postures,comTimeAndSetPoints,actuatedDOFs,changePostural, changeComWithSetPoints,amplitudeOfOscillation,frequencyOfOscillation, directionOfOscillation);
    for(int i=0; i < postures.size(); i++ )
    {
        std::cerr << "[INFO] time_" << i << " = " << postures[i].time << std::endl;
        std::cerr << "[INFO] posture_" << i << " = " << postures[i].qDes.toString()<< std::endl;
    }
    for(int i=0; i < comTimeAndSetPoints.size(); i++ )
    {
        std::cerr << "[INFO] time_" << i << " = " << comTimeAndSetPoints[i].time << std::endl;
        std::cerr << "[INFO] com_" << i << " = " <<  comTimeAndSetPoints[i].comDes.toString()<< std::endl;
    }
//     
    std::cout << "[INFO] Number of DOFs: " << actuatedDOFs << std::endl;
    
    std::cerr << "[INFO] changePostural: " << changePostural << std::endl;
    
    std::cerr << "[INFO] changeComWithSetPoints: " << changeComWithSetPoints << std::endl;
    
    std::cerr << "[INFO] amplitudeOfOscillation: " << amplitudeOfOscillation << std::endl;

    std::cout << "[INFO] frequencyOfOscillation " << frequencyOfOscillation << std::endl;
    
    std::cerr << "[INFO] directionOfOscillation: " << directionOfOscillation.toString() << std::endl;

    q0.resize(actuatedDOFs, 0.0);
    com0.resize(3, 0.0);
    comDes.resize(3, 0.0);
    DcomDes.resize(3, 0.0);
    DDcomDes.resize(3, 0.0);
    m_robot->getEstimates(wbi::ESTIMATE_JOINT_POS, q0.data());
    
    double world2BaseFrameSerialization[16];
    double rotoTranslationVector[7];
    wbi::Frame world2BaseFrame;
    m_robot->getEstimates(wbi::ESTIMATE_BASE_POS, world2BaseFrameSerialization);
    wbi::frameFromSerialization(world2BaseFrameSerialization, world2BaseFrame);
    m_robot->forwardKinematics(q0.data(), world2BaseFrame, wbi::wholeBodyInterface::COM_LINK_ID, rotoTranslationVector);
    com0[0] = rotoTranslationVector[0];    
    com0[1] = rotoTranslationVector[1];
    com0[2] = rotoTranslationVector[2];
    
    timeoutBeforeStreamingRefs = rf.check("timeoutBeforeStreamingRefs", Value(20), "Looking for robot name").asDouble();

    period = rf.check("period", Value(0.01), "Looking for module period").asDouble();
    
    std::cout << "timeoutBeforeStreamingRefs: " << timeoutBeforeStreamingRefs << "\n";

    std::string group_name = "PORTS_INFO";
    if( !rf.check(group_name) )
    {
        std::cerr << "[ERR] no PORTS_INFO group found" << std::endl;
        return true;
    }
    else
    {
        yarp::os::Bottle group_bot = rf.findGroup(group_name);

        if( !group_bot.check("portNameForStreamingQdes") || 
            !group_bot.check("portNameForStreamingComDes") )
        {
             std::cerr << "[ERR] portNameForStreamingQdes or portNameForStreamingComDes not found in config file" << std::endl;
                    return false;
        }

        //Check coupling configuration is well formed
        std::string portNameForStreamingComDes = group_bot.find("portNameForStreamingComDes").asString();
        std::cout << "portNameForStreamingComDes: " << portNameForStreamingComDes << "\n";
        
        std::string portNameForStreamingQdes = group_bot.find("portNameForStreamingQdes").asString();
        std::cout << "portNameForStreamingQdes: " << portNameForStreamingQdes << "\n";

        portForStreamingComDes.open(portNameForStreamingComDes);
        
        portForStreamingQdes.open(portNameForStreamingQdes);
        
        t0 = yarp::os::Time::now();
    return true;
    }
}
예제 #10
0
bool GBSegmModule::configure (yarp::os::ResourceFinder &rf)
{
    if (rf.check("help","if present, display usage message")) {
        printf("Call with --from configfile.ini\n");
        return false;
    }
   
    if (rf.check("name"))
        setName(rf.find("name").asString().c_str());
    else setName("GBSeg");


    //override defaults if specified - TODO: range checking
    if(rf.check("sigma")) sigma = (float)rf.find("sigma").asDouble();      
    if(rf.check("k")) k = (float)rf.find("k").asDouble();      
    if(rf.check("minRegion")) min_size = rf.find("minRegion").asInt();  

    std::string slash="/";
    _imgPort.open((slash + getName("/rawImg:i").c_str()).c_str());
    _configPort.open((slash + getName("/conf").c_str()).c_str());
    _viewPort.open((slash +getName("/viewImg:o").c_str()).c_str());
    attach(_configPort);

    //read an image to get the dimensions
    ImageOf<PixelRgb> *yrpImgIn;    
    yrpImgIn = _imgPort.read();
    if (yrpImgIn == NULL)   // this is the case if module is requested to quit while waiting for image
        return true;
    input=new image<rgb>(yrpImgIn->width(), yrpImgIn->height(), true);

// 
//     //THIS IS NOT REQUIRED - INPUT IMAGES ARE ALWAYS RGB
//     //IF DIM == 3 THEN THE PROCESSING CLASS CONVERTS TO LUV
//     //IF DIM == 1 THEN THE PROCESSING CLASS ONLY USES THE FIRST CHANNEL
//     //WE USE HUE CHANNEL, SO MUST CONVERT FROM RGB TO HSV
//     //check compatibility of image depth 
//     /*if (yrpImgIn->getPixelSize() != dim_) 
//     {
//             cout << endl << "Incompatible image depth" << endl;
//             return false;
//     }*/ 
// 
//     //override internal image dimension if necessary
//     if( width_ > orig_width_ )
//             width_ = orig_width_;
//     if( height_ > orig_height_ )
//             height_ = orig_height_;
// 
//     //allocate memory for image buffers and get the pointers
// 
//     inputImage.resize(width_, height_); inputImage_ = inputImage.getRawImage();
//     inputHsv.resize(width_, height_); inputHsv_ = inputHsv.getRawImage();
//     inputHue.resize(width_, height_); inputHue_ = inputHue.getRawImage();
//     filtImage.resize(width_, height_);  filtImage_ = filtImage.getRawImage();
//     segmImage.resize(width_, height_);  segmImage_ = segmImage.getRawImage();
//     gradMap.resize(width_, height_);    gradMap_ = (float*)gradMap.getRawImage();
//     confMap.resize(width_, height_);    confMap_ = (float*)confMap.getRawImage();
//     weightMap.resize(width_, height_);  weightMap_ = (float*)weightMap.getRawImage();
//     labelImage.resize(width_, height_);
//     labelView.resize(width_, height_);

    return true;
}
예제 #11
0
bool CamCalibModule::configure(yarp::os::ResourceFinder &rf)
{
    ConstString str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
    verboseExecTime = rf.check("verboseExecTime");

    if      (rf.check("w_align")) align=ALIGN_WIDTH;
    else if (rf.check("h_align")) align=ALIGN_HEIGHT;

    if (rf.check("fps"))
    {
        requested_fps=rf.find("fps").asDouble();
        yInfo() << "Module will run at " << requested_fps;
    }
    else
    {
        yInfo() << "Module will run at max fps";
    }

    setName(str.c_str()); // modulePortName

    Bottle botLeftConfig(rf.toString().c_str());
    Bottle botRightConfig(rf.toString().c_str());

    botLeftConfig.setMonitor(rf.getMonitor());
    botRightConfig.setMonitor(rf.getMonitor());

    string strLeftGroup = "CAMERA_CALIBRATION_LEFT";
    if (botLeftConfig.check(strLeftGroup.c_str()))
    {            
        Bottle &group=botLeftConfig.findGroup(strLeftGroup.c_str(),string("Loading configuration from group " + strLeftGroup).c_str());
        botLeftConfig.fromString(group.toString());
    }
    else
    {
        yError() << "Group " << strLeftGroup << " not found.";
        return false;
    }
    
    string strRightGroup = "CAMERA_CALIBRATION_RIGHT";
    if (botRightConfig.check(strRightGroup.c_str()))
    {            
        Bottle &group=botRightConfig.findGroup(strRightGroup.c_str(),string("Loading configuration from group " + strRightGroup).c_str());
        botRightConfig.fromString(group.toString());
    }
    else
    {
        yError() << "Group " << strRightGroup << " not found.";
        return false;
    }

    string calibToolLeftName  = botLeftConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
    string calibToolRightName = botRightConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();

    calibToolLeft = CalibToolFactories::getPool().get(calibToolLeftName.c_str());
    if (calibToolLeft!=NULL)
    {
        bool ok = calibToolLeft->open(botLeftConfig);
        if (!ok)
        {
            delete calibToolLeft;
            calibToolLeft = NULL;
            return false;
        }
    }
    calibToolRight = CalibToolFactories::getPool().get(calibToolRightName.c_str());
    if (calibToolRight!=NULL)
    {
        bool ok = calibToolRight->open(botRightConfig);
        if (!ok)
        {
            delete calibToolRight;
            calibToolRight = NULL;
            return false;
        }
    }

    if(rf.check("dual"))
    {
        dualImage_mode = true;
        yInfo() << "Dual mode activate!!";
    }

    if(dualImage_mode)
    {
        leftImage  = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;
        rightImage = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;

        // open a single port with name /dual:i
        if (yarp::os::Network::exists(getName("/dual:i")))
        {
            yWarning() << "port " << getName("/dual:i") << " already in use";
        }
        if(! imageInLeft.open(getName("/dual:i")) )
            return false;
        imageInLeft.setStrict(false);
    }
    else
    {
        if (yarp::os::Network::exists(getName("/left:i")))
        {
            yWarning() << "port " << getName("/left:i") << " already in use";
        }
        if (yarp::os::Network::exists(getName("/right:i")))
        {
            yWarning() << "port " << getName("/right:i") << " already in use";
        }
        imageInLeft.open(getName("/left:i"));
        imageInRight.open(getName("/right:i"));
        imageInLeft.setStrict(false);
        imageInRight.setStrict(false);
    }

    if (yarp::os::Network::exists(getName("/out")))
    {
        yWarning() << "port " << getName("/out") << " already in use";
    }
    if (yarp::os::Network::exists(getName("/conf")))
    {
        yWarning() << "port " << getName("/conf") << " already in use";
    }
    imageOut.open(getName("/out:o"));
    configPort.open(getName("/conf"));
    attach(configPort);
    return true;
}
예제 #12
0
bool rd::RobotDevastation::configure(yarp::os::ResourceFinder &rf)
{
    //-- Show help
    //printf("--------------------------------------------------------------\n");
    if (rf.check("help")) {
        printf("RobotDevastation options:\n");
        printf("\t--help (this help)\t--from [file.ini]\t--context [path]\n");
        printf("\t--id integer\n");
        printf("\t--name string\n");
        printf("\t--team integer\n");
        printf("\t--robotName string\n");
        // Do not exit: let last layer exit so we get help from the complete chain.
    }
    printf("RobotDevastation using no additional special options.\n");

    //-- Get player data
    //-----------------------------------------------------------------------------
    if( ! rf.check("id") )
    {
        RD_ERROR("No id!\n");
        return false;
    }
    RD_INFO("id: %d\n",rf.find("id").asInt());

    if( ! rf.check("name") )
    {
        RD_ERROR("No name!\n");
        return false;
    }
    RD_INFO("name: %s\n",rf.find("name").asString().c_str());

    if( ! rf.check("team") )
    {
        RD_ERROR("No team!\n");
        return false;
    }
    RD_INFO("team: %d\n",rf.find("team").asInt());

    if( ! rf.check("robotName") )
    {
        RD_ERROR("No robotName!\n");
        return false;
    }
    RD_INFO("robotName: %s\n",rf.find("robotName").asString().c_str());

    //-- Init mentalMap
    mentalMap = RdMentalMap::getMentalMap();
    mentalMap->configure( rf.find("id").asInt() );

    std::vector< RdPlayer > players;
    players.push_back( RdPlayer(rf.find("id").asInt(),std::string(rf.find("name").asString()),100,100,rf.find("team").asInt(),0) );
    mentalMap->updatePlayers(players);

    mentalMap->addWeapon(RdWeapon("Default gun", 10, 5));

    //-- Init input manager
    RdSDLInputManager::RegisterManager();
    inputManager = RdInputManager::getInputManager("SDL");
    inputManager->addInputEventListener(this);
    if (!inputManager->start() )
    {
        RD_ERROR("Could not init inputManager!\n");
        return false;
    }

    //-- Init sound
    if( ! initSound( rf ) )
        return false;

    if( ! rf.check("noMusic") )
        audioManager->play("bso", -1);

    //-- Init robot
    if( rf.check("mockupRobotManager") ) {
        robotManager = new RdMockupRobotManager(rf.find("robotName").asString());
    } else {
        robotManager = new RdYarpRobotManager(rf.find("robotName").asString());
    }
    if( ! robotManager->connect() ) {
        RD_ERROR("Could not connect to robotName \"%s\"!\n",rf.find("robotName").asString().c_str());
        RD_ERROR("Use syntax: robotDevastation --robotName %s\n",rf.find("robotName").asString().c_str());
        return false;
    }

    //-- Init network manager
    RdYarpNetworkManager::RegisterManager();
    networkManager = RdYarpNetworkManager::getNetworkManager(RdYarpNetworkManager::id);
    networkManager->addNetworkEventListener(mentalMap);
    mentalMap->addMentalMapEventListener((RdYarpNetworkManager *)networkManager);
    networkManager->login(mentalMap->getMyself());

    //-- Init image manager
    if( rf.check("mockupImageManager") ) {
        RdMockupImageManager::RegisterManager();
        imageManager = RdImageManager::getImageManager(RdMockupImageManager::id);
    } else {
        RdYarpImageManager::RegisterManager();
        imageManager = RdImageManager::getImageManager(RdYarpImageManager::id);
    }
    //-- Add the image processing listener to the image manager
    imageManager->addImageEventListener(&processorImageEventListener);
    //-- Configure the camera port
    std::ostringstream remoteCameraPortName;  //-- Default looks like "/0/rd1/img:o"
    remoteCameraPortName << "/";
    remoteCameraPortName << rf.find("robotName").asString();
    remoteCameraPortName << "/img:o";
    imageManager->configure("remote_img_port", remoteCameraPortName.str() );
    std::ostringstream localCameraPortName;  //-- Default should look like "/0/robot/img:i"
    localCameraPortName << "/";
    localCameraPortName << rf.find("id").asInt();
    localCameraPortName << "/robot/img:i";
    imageManager->configure("local_img_port", localCameraPortName.str() ); //-- Name given by me
    if( ! imageManager->start() )
        return false;

    //-- Init output thread
    rateThreadOutput.init(rf);

    return true;
}
예제 #13
0
 /**
  * @brief constructor of the generic thread.
  *
  * @param module_prefix module name.
  * @param thread_period period of the run thread in millisecond.
  * @param rf resource finder.
  **/
 generic_thread( std::string module_prefix, 
                 yarp::os::ResourceFinder rf, 
                 std::shared_ptr<paramHelp::ParamHelperServer> ph  ):    module_prefix( module_prefix ),
                                                                         thread_period( rf.find("thread_period").asInt() ),
                                                                         robot_name( rf.find("robot_name").asString() ),
                                                                         rf( rf ),
                                                                         ph( ph ),
                                                                         RateThread( rf.find("thread_period").asInt() )
 {    
 }
bool ObserverModule::configure(yarp::os::ResourceFinder &rf) {
    /* Process all parameters from both command-line and .ini file */

    /* get the module name which will form the stem of all module port names */
    moduleName            = rf.check("name", 
                           Value(""), 
                           "module name (string)").asString();
    /*
    * before continuing, set the module name before getting any other parameters, 
    * specifically the port names which are dependent on the module name
    */
//    setName(moduleName.c_str());

    /*
    * get the robot name which will form the stem of the robot ports names
    * and append the specific part and device required
    */
    robotName             = rf.check("robot", 
                           Value("icub"), 
                           "Robot name (string)").asString();
    //robotPortName         = "/" + robotName + "/head";

    inputPortName           = rf.check("inputPortName",
			                Value(":i"),
                            "Input port name (string)").asString();
    
    
    /*
    * attach a port of the same name as the module (prefixed with a /) to the module
    * so that messages received from the port are redirected to the respond method
    */
    handlerPortName =  "";
    handlerPortName += getName();         // use getName() rather than a literal 

    if (!handlerPort.open(handlerPortName.c_str())) {           
        cout << getName() << ": Unable to open port " << handlerPortName << endl;  
        return false;
    }

    attach(handlerPort);                  // attach to port
    if (rf.check("config")) {
        configFile=rf.findFile(rf.find("config").asString().c_str());
        if (configFile=="") {
            return false;
        }
    }
    else {
        configFile.clear();
    }

	bool returnval = true;
	//create the thread and configure it using the ResourceFinder
    _effectorThread = new EffectorThread();
	returnval &= _effectorThread->configure(rf); 

	_affordanceAccess = new darwin::observer::AffordanceAccessImpl();
	if (returnval) {
	    returnval &= _affordanceAccess->configure(rf);
	}

	_attentionAccess = new darwin::observer::AttentionAccessImpl();
	if (returnval) {
	    returnval &= _attentionAccess->configure(rf);
	}

	_workspace = new darwin::observer::WorkspaceCalculationsImpl();
	if (returnval) {
		returnval &= _workspace->configure(rf);
	}

    /* create the thread and pass pointers to the module parameters */
    rThread = new ObserverThread(robotName, configFile);
    rThread->setName(getName().c_str());
	rThread->setEffectorAccess(_effectorThread);
	rThread->setAffordanceAccess(_affordanceAccess);
	rThread->setAttentionAccess(_attentionAccess);
	rThread->setWorkspaceAccess(_workspace);
 
    const string pt = rf.findPath("observerConfig.ini");
    unsigned pos = pt.find("observerConfig.ini");
    pathPrefix = pt.substr(0,pos);
    
    printf("observer configuraion file in %s \n", pathPrefix.c_str());
//////////////////////// find file paths
///////////input files
    
    rThread->setPath(pathPrefix);


//    rThread->setColorPath(colormapFile);
//    rThread->setModelPath(modelFile);


	//=======================================================================
//    
    //rThread->setInputPortName(inputPortName.c_str());
    
	if (returnval) {
		returnval &= _effectorThread->start();
	}
    /* now start the thread to do the work */
	if (returnval) {
		returnval &= rThread->start(); // this calls threadInit() and it if returns true, it then calls run()
	}

    return returnval;       // let the RFModule know everything went well
                        // so that it will then run the module
}
예제 #15
0
bool ImageSplitter::configure(yarp::os::ResourceFinder &rf)
{
    // Check input parameters
    if(rf.check("align"))
    {
        yarp::os::ConstString align = rf.find("align").asString();
        if(align == "vertical")
        {
            horizontal = false;
        }
        else if(align == "horizontal")
        {
            horizontal = true;
        }
        else
        {
            yError() << " Incorrect parameter. Supported values for alignment are 'horizontal' or 'vertical'";
            return false;
        }
    }

    yarp::os::ConstString inputPortName    = "/imageSplitter/input:i";
    yarp::os::ConstString outLeftPortName  = "/imageSplitter/left:o";
    yarp::os::ConstString outRightPortName = "/imageSplitter/right:o";

    if(rf.check("nameLeft"))
    {
        outLeftPortName = rf.find("nameLeft").asString();
    }

    if(rf.check("nameRight"))
    {
        outRightPortName = rf.find("nameRight").asString();
    }

    if(!rf.check("remote"))
    {
        yError() << "Missing 'remote' parameter. Please insert the name of the port to connect to.";
        return false;
    }

    yarp::os::ConstString remotePortName = rf.find("remote").asString();

    // opening ports
    bool ret=true;
    ret &= inputPort.open(inputPortName);
    ret &= outLeftPort.open(outLeftPortName);
    ret &= outRightPort.open(outRightPortName);

    if(!ret)
    {
        yError() << " Cannot open ports";
        return false;
    }

    // Connections
    if(! yarp::os::Network::connect(remotePortName, inputPortName))
    {
        yError() << "Cannot connect to remote port " << remotePortName;
        return false;
    }

    // choose filling method
    if(rf.check("m"))
    {
        yarp::os::ConstString align = rf.find("m").asString();
        if(align == "pixel")
        {
            method = 0;
        }
        else if(align == "pixel2")
        {
            method = 1;
        }
        else if(align == "line")
        {
            method = 2;
        }
        else if(align == "whole")
        {
            if(horizontal)
                yError() << "Cannot use 'whole' method for input image horizontally aligned";
            method = 3;
        }
        else
        {
            yError() << "Methods are pixel, line, whole; got " << align;
            return false;
        }
    }

    yInfo() << "using method " << method;
    return true;
}
예제 #16
0
bool EdisonSegmModule::configure (yarp::os::ResourceFinder &rf)
{
    if (rf.check("help","if present, display usage message")) {
        printf("Call with --from configfile.ini\n");
        return false;
    }
   
    if (rf.check("name"))
        setName(rf.find("name").asString().c_str());
    else setName("edisonSeg");

    //defaults for the parameters - as the ones in the EDISON GUI application
    height_ = 240;
    width_ = 320;
    dim_ = 3;
    sigmaS = 7;		
    sigmaR = 6.5;		
    minRegion = 20;  
    gradWindRad = 2; 
    threshold = 0.3; 
    mixture = 0.2;  
    speedup = MED_SPEEDUP; 
    //override defaults if specified - TODO: range checking
    if(rf.check("height")) height_ = rf.find("height").asInt();
    if(rf.check("width")) width_ = rf.find("width").asInt();
    //if(rf.check("dim")) dim_ = rf.find("dim").asInt(); // not required - should always be 3
    if(rf.check("sigmaS")) sigmaS = rf.find("sigmaS").asInt();		
    if(rf.check("sigmaR")) sigmaR = rf.find("sigmaR").asDouble();		
    if(rf.check("minRegion")) minRegion = rf.find("minRegion").asInt();  
    if(rf.check("gradWinRad")) gradWindRad = rf.find("gradWinRad").asDouble(); 
    if(rf.check("threshold")) threshold = rf.find("threshold").asDouble(); 
    if(rf.check("mixture")) mixture = rf.find("mixture").asDouble();  
    if(rf.check("speedup")) 
    {
            int spdp = rf.find("speedup").asInt(); 
            setSpeedUpValue(spdp);

            
    }
    // name of the camera calibration file
    // ConstString strCamConfigPath=rf.findFile("camera");

    std::string slash="/";
    _imgPort.open((slash + getName("/rawImg:i").c_str()).c_str());
    _configPort.open((slash + getName("/conf").c_str()).c_str());
    _filtPort.open((slash + getName("/filtImg:o").c_str()).c_str());
    _labelPort.open((slash +getName("/labeledImg:o").c_str()).c_str());
    _viewPort.open((slash +getName("/viewImg:o").c_str()).c_str());
    _rawPort.open((slash +getName("/rawImg:o").c_str()).c_str());
    _labelViewPort.open((slash +getName("/debugImg:o").c_str()).c_str());
    //attach(_configPort, true);
    attach(_configPort);

    //read an image to get the dimensions
    ImageOf<PixelRgb> *yrpImgIn;	
    yrpImgIn = _imgPort.read();
    if (yrpImgIn == NULL)   // this is the case if module is requested to quit while waiting for image
        return true;
    orig_height_ = yrpImgIn->height();
    orig_width_ = yrpImgIn->width();

    //THIS IS NOT REQUIRED - INPUT IMAGES ARE ALWAYS RGB
    //IF DIM == 3 THEN THE PROCESSING CLASS CONVERTS TO LUV
    //IF DIM == 1 THEN THE PROCESSING CLASS ONLY USES THE FIRST CHANNEL
    //WE USE HUE CHANNEL, SO MUST CONVERT FROM RGB TO HSV
    //check compatibility of image depth 
    /*if (yrpImgIn->getPixelSize() != dim_) 
    {
            cout << endl << "Incompatible image depth" << endl;
            return false;
    }*/ 

    //override internal image dimension if necessary
    if( width_ > orig_width_ )
            width_ = orig_width_;
    if( height_ > orig_height_ )
            height_ = orig_height_;

    //allocate memory for image buffers and get the pointers

    inputImage.resize(width_, height_); inputImage_ = inputImage.getRawImage();
    inputHsv.resize(width_, height_); inputHsv_ = inputHsv.getRawImage();
    inputHue.resize(width_, height_); inputHue_ = inputHue.getRawImage();
    filtImage.resize(width_, height_);  filtImage_ = filtImage.getRawImage();
    segmImage.resize(width_, height_);  segmImage_ = segmImage.getRawImage();
    gradMap.resize(width_, height_);    gradMap_ = (float*)gradMap.getRawImage();
    confMap.resize(width_, height_);    confMap_ = (float*)confMap.getRawImage();
    weightMap.resize(width_, height_);  weightMap_ = (float*)weightMap.getRawImage();
    labelImage.resize(width_, height_);
    labelView.resize(width_, height_);

    return true;
}
예제 #17
0
SimoxRobotViewer::SimoxRobotViewer(const std::string &title,yarp::os::ResourceFinder &rf)
    :QMainWindow(NULL)
{
    pingValue = 0;
    visualisationTypeRobot = VirtualRobot::SceneObject::Full;
    visualizationRobotEnabled = true;
    viewer = NULL;
    sceneSep = new SoSeparator();
    sceneSep->ref();
    robotSep = new SoSeparator();
    robotSep->ref();
    reachSep = new SoSeparator();
    reachSep->ref();
    isClosed = false;
    enableViewer = true;

    setupUI(title);

    if (rf.check("SimoxDataPath"))
    {
        ConstString dataPath=rf.find("SimoxDataPath").asString();
        VR_INFO << "Adding rf.SimoxDataPath: " << dataPath.c_str() << endl;
        VirtualRobot::RuntimeEnvironment::addDataPath(dataPath.c_str());
    }
    if (rf.check("RobotFile"))
    {

        ConstString robotFile=rf.find("RobotFile").asString();
        std::string robFileComplete = robotFile.c_str();
        if (!VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFileComplete))
        {
            VR_ERROR << " Could not find file: " << robFileComplete << endl;
        } else
        {
            std::cout << "Using robot file: " << robFileComplete << endl;
            loadRobot(robFileComplete);
        }
    }
    if (!robot)
        VR_INFO << "No robot..." << endl;

    if (robot && rf.check("EndEffector"))
    {
        std::string eef = rf.find("EndEffector").asString().c_str();
        VR_INFO << "Selecting rf.EndEffector: " << eef << endl;
        selectEEF(eef);
    }

    if (robot && !currentEEF)
    {
        // select first eef

        std::vector<EndEffectorPtr> eefs;
        robot->getEndEffectors(eefs);
        if (eefs.size()>0)
        {
            VR_INFO << "Selecting first EEF: " << eefs[0]->getName() << endl;
            selectEEF(eefs[0]->getName());
        }
    }
    if (!currentEEF)
        VR_INFO << "Skipping EEF definition..." << endl;

    if (robot && currentEEF && rf.check("ReachabilityFile"))
    {
        std::string reachFile = rf.find("ReachabilityFile").asString().c_str();
        std::string reachFileComplete = reachFile.c_str();
        if (!VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(reachFileComplete))
        {
            VR_ERROR << " Could not find file: " << reachFileComplete << endl;
        } else
            loadReachability(reachFileComplete);
    }
    if (!reachSpace)
        VR_INFO << "Skipping reachability information..." << endl;
    if (viewer)
        viewer->viewAll();
    SoQt::show(this);
}
예제 #18
0
bool PasarModule::configure(yarp::os::ResourceFinder &rf) {
    std::string opcName;
    std::string gazePortName;
    std::string handlerPortName;
    std::string saliencyPortName;

    string moduleName = rf.check("name", Value("pasar")).asString().c_str();
    setName(moduleName.c_str());


    //    moduleName = rf.check("name",
    //        Value("pasar")).asString();
    //    setName(moduleName.c_str());

    //Parameters
    pTopDownAppearanceBurst = rf.check("parameterTopDownAppearanceBurst",
        Value(0.5)).asDouble();
    pTopDownDisappearanceBurst = rf.check("parameterTopDownDisappearanceBurst",
        Value(0.5)).asDouble();
    pTopDownAccelerationCoef = rf.check("parameterTopDownAccelerationCoef",
        Value(0.1)).asDouble();
    //pLeakyIntegrationA		=  rf.check("parameterLeakyIntegrationA", 
    //    Value(0.9)).asDouble(); 
    pTopDownInhibitionReturn = rf.check("parameterInhibitionReturn",
        Value(0.05)).asDouble();
    pExponentialDecrease = rf.check("ExponentialDecrease",
        Value(0.9)).asDouble();
    pTopDownWaving = rf.check("pTopDownWaving",
        Value(0.2)).asDouble();
    dBurstOfPointing = rf.check("pBurstOfPointing",
        Value(0.2)).asDouble();

    //check for decrease
    if (pExponentialDecrease >= 1 || pExponentialDecrease <= 0.0)   pExponentialDecrease = 0.95;

    presentRightHand.first = false;
    presentRightHand.second = false;
    presentLeftHand.first = false;
    presentLeftHand.first = false;

    rightHandt1 = Vector(3, 0.0);
    rightHandt2 = Vector(3, 0.0);
    leftHandt1 = Vector(3, 0.0);
    leftHandt2 = Vector(3, 0.0);

    thresholdMovementAccel = rf.check("thresholdMovementAccel",
        Value(0.02)).asDouble();
    thresholdWaving = rf.check("thresholdWaving",
        Value(0.02)).asDouble();
    thresholdSaliency = rf.check("thresholdSaliency",
        Value(0.005)).asDouble();

    isControllingMotors = rf.check("motorControl",
        Value(0)).asInt() == 1;
    //Ports

    opcName=rf.check("opc",Value("OPC")).asString().c_str();
    opc = new OPCClient(moduleName);
    while (!opc->connect(opcName))
    {
        cout<<"Waiting connection to OPC..."<<endl;
        Time::delay(1.0);
    }

    opc->checkout();

    icub = opc->addOrRetrieveEntity<Agent>("icub");



    if (!saliencyInput.open(("/" + moduleName + "/saliency:i").c_str())) {
        cout << getName() << ": Unable to open port saliency:i" << endl;
        return false;
    }


    if (!Network::connect("/agentDetector/skeleton:o", ("/" + moduleName + "/skeleton:i").c_str()))
    {
        isSkeletonIn = false;
    }
    else
    {
        yInfo() << " is connected to skeleton";
        isSkeletonIn = true;
    }

    isPointing = rf.find("isPointing").asInt() == 1;
    isWaving = rf.find("isWaving").asInt() == 1;

    yInfo() << " pointing: "  << isPointing;
    yInfo() << " waving: " << isWaving;


    if (!saliencyOutput.open(("/" + moduleName + "/saliency:o").c_str())) {
        cout << getName() << ": Unable to open port saliency:o" << endl;
        return false;
    }

    if (!handlerPort.open(("/" + moduleName + "/rpc").c_str())) {
        cout << getName() << ": Unable to open port rpc" << endl;
        return false;
    }

    attach(handlerPort);                  // attach to port
    trackedObject = "";
    presentObjectsLastStep.clear();
    return true;
}
예제 #19
0
bool eventDrivenModule::configure(yarp::os::ResourceFinder &rf) {
    /* Process all parameters from both command-line and .ini file */

    /* get the module name which will form the stem of all module port names */
    moduleName            = rf.check("name", 
                           Value("/eventDriven"), 
                           "module name (string)").asString();
    /*
    * before continuing, set the module name before getting any other parameters, 
    * specifically the port names which are dependent on the module name
    */
    setName(moduleName.c_str());

    /*
    * get the robot name which will form the stem of the robot ports names
    * and append the specific part and device required
    */
    robotName             = rf.check("robot", 
                           Value("icub"), 
                           "Robot name (string)").asString();
    robotPortName         = "/" + robotName + "/head";

    inputPortName           = rf.check("inputPortName",
			                Value("/AER:i"),
                            "Input port name (string)").asString();
    
    
    /*
    * attach a port of the same name as the module (prefixed with a /) to the module
    * so that messages received from the port are redirected to the respond method
    */
    handlerPortName =  "";
    handlerPortName += getName();         // use getName() rather than a literal 

    if (!handlerPort.open(handlerPortName.c_str())) {           
        cout << getName() << ": Unable to open port " << handlerPortName << endl;  
        return false;
    }

    attach(handlerPort);                  // attach to port
    if (rf.check("config")) {
        configFile=rf.findFile(rf.find("config").asString().c_str());
        if (configFile=="") {
            return false;
        }
    }
    else {
        configFile.clear();
    }


    /* create the thread and pass pointers to the module parameters */
    edThread = new eventDrivenThread(robotName, configFile);
    edThread->setName(getName().c_str());
    edThread->setInputPortName(inputPortName.c_str());
    
    /* now start the thread to do the work */
    edThread->start(); // this calls threadInit() and it if returns true, it then calls run()

    return true ;       // let the RFModule know everything went well
                        // so that it will then run the module
}
bool quaternionEKFModule::configure ( yarp::os::ResourceFinder& rf )
{
    if( rf.check("robot") ) {
        robotName = rf.find("robot").asString();
    } else {
        yError("[quaternionEKFModule::configure] Configuration failed! No robot param was found");
        return false;
    }
    
    if( rf.check("rate") && rf.find("rate").asDouble() )
    {
        period = rf.find("rate").asDouble();
    } else {
        yError("[quaternionEKFModule::configure] Configuration failed. No rate was specified. Remember it should be a double.");
        return false;
    }
    
    if( rf.check("local") ) {
        local =  rf.find("local").asString();
    } else {
        yError("[quaternionEKFModule::configure] Configuration failed. No local name was foundd.");
        return false;
    }
    
    if( rf.check("autoconnect") ) {
        autoconnect = rf.find("autoconnect").asBool();
    } else {
        yError("[quaternionEKFModule::configure] Configuration failed. No value for autoconnect was found.");
        return false;
    }
    
    if( rf.check("mode") ) {
        mode = rf.find("mode").asString();
    } else {
        yError("[quaternionEKFModule::configure] Configuration failed. No value for mode was found.");
        return false;
    }
    
    if ( rf.check("usingXSens") ) {
        usingxsens = rf.find("usingXSens").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for usingXSens was found.");
        return false;
    }
    
    if ( rf.check("verbose") ) {
        verbose = rf.find("verbose").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for verbose was found.");
        return false;
    }
    
    if ( rf.check("sensorPortName") )  {
        sensorPortName = rf.find("sensorPortName").asString();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for sensorPortName was found");
        return false;
    }
    
    if ( rf.check("usingEKF") ) {
        usingEKF = rf.find("usingEKF").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for usingEKF was found.");
        return false;
    }
    
    if (rf.check("usingSkin")) {
        usingSkin = rf.find("usingSkin").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for usingSkin was found.");
        return false;
    }
    
    if (rf.check("inWorldRefFrame")) {
        inWorldRefFrame = rf.find("inWorldRefFrame").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for inWorldRefFrame was found.");
        return false;
    }
    
    if (rf.check("gravityVec")) {
        gravityVec = rf.find("gravityVec").asDouble();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for gravityVec was found.");
        return false;
    }
    
    if (rf.check("debugGyro")) {
        debugGyro = rf.find("debugGyro").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for debugGyro was found. ");
        return false;
    }
    
    if (rf.check("debugAcc")) {
        debugAcc = rf.find("debugAcc").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for debugAcc was found. ");
        return false;
    }
    
    if (rf.check("using2acc")) {
        using2acc = rf.find("using2acc").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for using2acc was found.");
        return false;
    }
    
    if ( rf.check("calib") ) {
        calib = rf.find("calib").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for calib was found.");
        return false;
    }
    
    // ------------ IMU PORT ---------------------------------------
    /*TODO This should be configurable! The number of input ports
     depending on the amount of sensor readings.*/
    std::string tmpOffline = "offline";
    std::string tmpOnline  = "online";
    //  If the estimate is done online
    if (!calib) {
        if (!tmpOnline.compare(mode)) {
            yInfo(" [quaternionEKFModule::configure] Online estimation will be performed");
            std::string gyroMeasPortName = "/";
            gyroMeasPortName += local;
            gyroMeasPortName += "/imu:i";
            if (!gyroMeasPort.open(gyroMeasPortName.c_str())) {
                yError("[quaternionEKFModule::configure] Could not open gyroMeasPort");
                return false;
            }
            // If using two accelerometers, open another port for the second reading
            if (using2acc) {
                std::string gyroMeasPortName2 = "/"; gyroMeasPortName2 += local; gyroMeasPortName2 += "/imu2:i";
                if (!gyroMeasPort2.open(gyroMeasPortName2.c_str())) {
                    yError("[quaternionEKFModule::configure] Coult not open gyroMeasPort2");
                    return false;
                }
            }

            // Obtaining filter parameters from configuration file
            yarp::os::Property filterParams;

            // If using Direct method with atan2
            if(!usingEKF)
            {
                if( !rf.check(DIRECT_GROUP_PARAMS_NAME) )  {
                    yError("[quaternionEKFModule::configure] Could not load DIRECT-FILTER-PARAMS group from config file");
                    return false;
                } else   {
                    filterParams.fromString(rf.findGroup(DIRECT_GROUP_PARAMS_NAME).tail().toString());
                    yInfo(" [quaternionEKFModule::configure] Filter parameters are: %s ", filterParams.toString().c_str());
                }
            }
            else
            {
                if( !rf.check(FILTER_GROUP_PARAMS_NAME) )  {
                    yError("[quaternionEKFModule::configure] Could not load EKF-PARAMS group from config file");
                    return false;
                } else   {
                    filterParams.fromString(rf.findGroup(FILTER_GROUP_PARAMS_NAME).tail().toString());
                    yInfo(" [quaternionEKFModule::configure] Filter parameters are: %s ", filterParams.toString().c_str());
                }
            }
            // ----------- THREAD INSTANTIATION AND CALLING -----------------
            quatEKFThread = new quaternionEKFThread(period, local, robotName, autoconnect, usingxsens, usingEKF, inWorldRefFrame, gravityVec, usingSkin, sensorPortName, debugGyro, debugAcc, verbose, filterParams, &gyroMeasPort, &gyroMeasPort2);
            if (!quatEKFThread->start()) {
                yError("Error starting quaternionEKFThread!");
                return false;
            }
            
            yInfo(" [quaternionEKFModule::configure] quaternionEKFThread started");
            
        } else {
            // If the estimate is done offline, read from file with a datadumper format and don't create the thread.
            if(!tmpOffline.compare(mode)) {
                yInfo(" [quaternionEKFModule::configure] Offline batch estimation will be performed");
                
                // **** Initialization
                // Create dataDumper parser
                m_parser = new dataDumperParser(DATAFILE);
                m_parser->parseFile();
                m_parser->countLines();
                
                // Change period of the module thread
                
            } else {
                yError("[quaternionEKFModule::configure] An invalid option was passed to 'mode'. Available options are 'offline' or 'online'.");
                return false;
            }
        }
    } else { 
        std::cout << "Calibrating only... Done bby updateModule()" << std::endl;
    }
  return true;
}