Esempio n. 1
0
void ReactiveLayer::configureTactile(yarp::os::ResourceFinder &rf)
{
    //Initialise the tactile response
    Bottle grpTactile = rf.findGroup("TACTILE");
    Bottle *tactileStimulus = grpTactile.find("stimuli").asList();

    if (tactileStimulus)
    {
        for (int d = 0; d<tactileStimulus->size(); d++)
        {
            string tactileStimulusName = tactileStimulus->get(d).asString().c_str();
            StimulusEmotionalResponse response;
            Bottle * bSentences = grpTactile.find((tactileStimulusName + "-sentence").c_str()).asList();
            for (int s = 0; s<bSentences->size(); s++)
            {
                response.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            //choregraphies
            Bottle *bChore = grpTactile.find((tactileStimulusName + "-chore").c_str()).asList();
            for (int sC = 0; bChore && sC<bChore->size(); sC++)
            {
                response.m_choregraphies.push_back(bChore->get(sC).asString().c_str());
            }
            std::string sGroupTemp = tactileStimulusName;
            sGroupTemp += "-effect";
            Bottle *bEffects = grpTactile.find(sGroupTemp.c_str()).asList();
            for (int i = 0; bEffects && i<bEffects->size(); i += 2)
            {
                response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble();
            }
            tactileEffects[tactileStimulusName] = response;
        }
    }
}
Esempio n. 2
0
void AdaptiveLayer::configureGestures(yarp::os::ResourceFinder &rf)
{
    //Initialise the gestures response
    Bottle grpGesture = rf.findGroup("GESTURES");
    Bottle *gestureStimulus = grpGesture.find("stimuli").asList();

    if (gestureStimulus)
    {
        for(int d=0; d<gestureStimulus->size(); d++)
        {
            string gestureStimulusName = gestureStimulus->get(d).asString().c_str();
            StimulusEmotionalResponse response;
            Bottle * bSentences = grpGesture.find((gestureStimulusName + "-sentence").c_str()).asList();
            for(int s=0;s<bSentences->size();s++)
            {
                response.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            std::string sGroupTemp = gestureStimulusName;
            sGroupTemp += "-effect";
            Bottle *bEffects = grpGesture.find( sGroupTemp.c_str()).asList();
            for(int i=0; bEffects && i<bEffects->size(); i += 2)
            {
                response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i+1).asDouble();
            }
            gestureEffects[gestureStimulusName] = response;
        }
    }
}
Esempio n. 3
0
void ReactiveLayer::configureSalutation(yarp::os::ResourceFinder &rf)
{
    ;
    //Initialise the gestures response
    Bottle grpSocial = rf.findGroup("SOCIAL");
    salutationLifetime = grpSocial.check("salutationLifetime", Value(15.0)).asDouble();
    Bottle *socialStimulus = grpSocial.find("stimuli").asList();

    if (socialStimulus)
    {
        for (int d = 0; d<socialStimulus->size(); d++)
        {
            string socialStimulusName = socialStimulus->get(d).asString().c_str();
            StimulusEmotionalResponse response;
            Bottle * bSentences = grpSocial.find((socialStimulusName + "-sentence").c_str()).asList();
            for (int s = 0; s<bSentences->size(); s++)
            {
                response.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            std::string sGroupTemp = socialStimulusName;
            sGroupTemp += "-effect";
            Bottle *bEffects = grpSocial.find(sGroupTemp.c_str()).asList();
            for (int i = 0; bEffects && i<bEffects->size(); i += 2)
            {
                response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble();
            }
            salutationEffects[socialStimulusName] = response;
        }
    }

    //Add the relevant Entities for handling salutation
    iCub->opc->addOrRetrieveAction("is");
    iCub->opc->addOrRetrieveAdjective("saluted");
}
Esempio n. 4
0
bool visualFilterModule::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("/visualFeaX"), 
                           "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";

    /*
    * 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

    /* create the thread and pass pointers to the module parameters */
    printf("trying to start the main thread \n");
    vfThread = new visualFilterThread();
    vfThread->setName(getName().c_str());
    
    /* now start the thread to do the work */
    vfThread->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
}
Esempio n. 5
0
bool ReactiveLayer::configure(yarp::os::ResourceFinder &rf)
{
    string moduleName = rf.check("name",Value("ReactiveLayer")).asString().c_str();
    setName(moduleName.c_str());

    cout<<moduleName<<": finding configuration files..."<<endl;
    period = rf.check("period",Value(0.1)).asDouble();

    //Create an iCub Client and check that all dependencies are here before starting
    bool isRFVerbose = false;
    iCub = new ICubClient(moduleName,"reactiveLayer","client.ini",isRFVerbose);
    iCub->opc->isVerbose = false;
    char rep = 'n';
    while (rep!='y'&&!iCub->connect())
    {
        cout<<"iCubClient : Some dependencies are not running..."<<endl;
        //cout<<"Continue? y,n"<<endl;
        //cin>>rep;
        break; //to debug
        Time::delay(1.0);
    }

    //Set the voice
    std::string ttsOptions = rf.check("ttsOptions", yarp::os::Value("iCubina 85.0")).asString();
    if (iCub->getSpeechClient())
        iCub->getSpeechClient()->SetOptions(ttsOptions);

    //Configure the various components
    configureOPC(rf);
    configureAllostatic(rf);
    configureTactile(rf);
    configureSalutation(rf);

    cout<<"Configuration done."<<endl;

    rpc.open ( ("/"+moduleName+"/rpc").c_str());
    attach(rpc);

    //Initialise timers
    lastFaceUpdate = Time::now();
    physicalInteraction = false;
    someonePresent = false;

    //iCub->getReactableClient()->SendOSC(yarp::os::Bottle("/event reactable pong start"));

    return true;
}
Esempio n. 6
0
bool GuiUpdaterModule::configure(yarp::os::ResourceFinder &rf)
{
    iCub = NULL;

    string moduleName      = rf.check("name", 
        Value("guiUpdater"), 
        "module name (string)").asString().c_str();

    string opcName      = rf.check("OPCname", 
        Value("OPC"), 
        "OPC name (string)").asString().c_str();

    setName(moduleName.c_str());

    displaySkeleton = rf.check("displaySkeletons");
    cout<<"Display skeleton status: "<<displaySkeleton<<endl;

    w = new OPCClient(getName().c_str());
    w->connect(opcName);
    w->isVerbose = false;

    //GUI Port
    string guiPortName = "/";
    guiPortName +=  getName() + "/gui:o";
    toGui.open(guiPortName.c_str());
    resetGUI();

    //GUI Base Port
    string guiBasePortName = "/";
    guiBasePortName +=  getName() + "/guiBase:o";
    toGuiBase.open(guiBasePortName.c_str());

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

    return true ;
}
Esempio n. 7
0
bool lumaChroma::configure(yarp::os::ResourceFinder &rf)
{    
    /* Process all parameters from both command-line and .ini file */

    moduleName          = rf.check("name", 
                        Value("lumaChroma"), 
                        "module name (string)").asString();

    setName(moduleName.c_str());

    imageType           = rf.check("image", 
                        Value("yuv"), 
                        "image type (string)").asString();

    whichPort           = rf.check("out",
                        Value(""),
                        "default port (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

    /* create the thread and pass pointers to the module parameters */
    procThread = new PROCThread(moduleName, imageType, whichPort);

    /* now start the thread to do the work */
    procThread->open();

    return true ;      // let the RFModule know everything went well
}
Esempio n. 8
0
bool actionClass::parseTorqueBalancingSequences(std::string              filenamePrefix,
        std::string              filenameSuffix,
        int                      partID,
        std::string              format,
        yarp::os::ResourceFinder &rf)
{
    bool ret = false;
    actionStructForTorqueBalancing tmp_action;
    string filename  = filenamePrefix + "_" + filenameSuffix + ".txt";
    filename  = rf.findFile(filename);
    fprintf(stderr, "[!!!] File found for %s: %s\n", filenameSuffix.c_str(), filename.c_str());
    // Open file
    ifstream data_file( filename.c_str() );
    string line;
    std::deque<double> tmp_com;
    std::deque<double> tmp_postural;
    std::deque<double> tmp_constraints;

    while( std::getline(data_file, line) )
    {
        int col = 0;
        std::istringstream iss( line );
        std::string result;
        tmp_com.clear();
        tmp_postural.clear();
        tmp_constraints.clear();
        while( std::getline( iss, result , ' ') )
        {
            if ( strcmp(result.c_str(),"") != 0 )
            {
                std::stringstream convertor;
                convertor.clear();
                convertor.str(result);
                if (partID == COM_ID)
                    tmp_com.push_back(atof(result.c_str()));
                if (partID == POSTURAL_ID)
                    tmp_postural.push_back(atof(result.c_str()));
                if (partID == CONSTRAINTS_ID)
                    tmp_constraints.push_back(atoi(result.c_str()));
                col++;
            }
        }
        if (partID == COM_ID)
            action_vector_torqueBalancing.com_traj.push_back(tmp_com);
        if (partID == POSTURAL_ID)
            action_vector_torqueBalancing.postural_traj.push_back(tmp_postural);
        if (partID == CONSTRAINTS_ID)
            action_vector_torqueBalancing.constraints.push_back(tmp_constraints);
    }

    data_file.close();
    ret = true;
    return ret;
}
Esempio n. 9
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;
}
Esempio n. 10
0
void printInstalledFolders(yarp::os::ResourceFinder &rf, folderType ftype)
{
    ResourceFinderOptions opts;
    opts.searchLocations=ResourceFinderOptions::Installed;
    yarp::os::Bottle contextPaths=rf.findPaths(getFolderStringName(ftype), opts);
    printf("**INSTALLED DATA:\n");
    for (int curPathId=0; curPathId<contextPaths.size(); ++curPathId)
    {
        printf("* Directory : %s\n", contextPaths.get(curPathId).asString().c_str());
        printContentDirs(contextPaths.get(curPathId).asString());
    }
}
Esempio n. 11
0
    /* 
    * 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)
    {
        timeout=rf.check("timeout",Value(60.0)).asDouble();
        vergencePort.open("/radHelper/vergence:i");
        handlerPort.open("/radHelper/attentionStatus:io");
        suspendPort.open("/radHelper/suspend:o");
        attach(handlerPort);

        //Network::connect("/gazeArbiter/icub/left_cam/status:o","/radHelper/vergence:i","udp");
        //Network::connect("/radHelper/suspend:o","/gazeArbiter/icub/left_cam");
        return true;
    }
bool InterpersonalDistanceRegulator::configure(yarp::os::ResourceFinder &rf)
{
	isPaused = false;
	isQuitting = false;

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

    cout<<moduleName<<": finding configuration files..."<<endl;
    period = rf.check("period",Value(0.1)).asDouble();
	preferedDistanceToPeople = rf.check("preferedDistanceToPeople",Value(0.9)).asDouble();
	fwdSpeed = rf.check("forwardSpeed", Value(0.075)).asDouble();
	backwdSpeed = rf.check("backwardSpeed", Value(0.075)).asDouble();
	turningSpeed = rf.check("angularSpeed", Value(0.075)).asDouble();

	ikart = new SubSystem_iKart(moduleName);
	while (!ikart->Connect())
	{
		cout << "Connecting IDR to iKart..." << endl;
		Time::delay(1.0);
	}

	opc = new OPCClient(moduleName);
	while (!opc->connect("OPC"))
	{
		cout << "Connecting InterpersonalThread to OPC..." << endl;
		Time::delay(1.0);
	}
    cout<<"Configuration done."<<endl;

    rpc.open ( ("/"+moduleName+"/rpc").c_str());
    attach(rpc);
    return true;
}
Esempio n. 13
0
void prepareHomeFolder(yarp::os::ResourceFinder &rf, folderType ftype)
{
    ACE_DIR* dir= YARP_opendir((rf.getDataHome()).c_str());
    if (dir!=NULL)
        YARP_closedir(dir);
    else
    {
        yarp::os::mkdir((rf.getDataHome()).c_str());
    }

    dir= YARP_opendir((rf.getDataHome() + PATH_SEPARATOR + getFolderStringName(ftype)).c_str());
    if (dir!=NULL)
        YARP_closedir(dir);
    else
    {
        yarp::os::mkdir((rf.getDataHome() + PATH_SEPARATOR + getFolderStringName(ftype)).c_str());
    }
    dir= YARP_opendir((rf.getDataHome() + PATH_SEPARATOR + getFolderStringNameHidden(ftype)).c_str());
    if (dir!=NULL)
        YARP_closedir(dir);
    else
    {
        ConstString hiddenPath=(rf.getDataHome() + PATH_SEPARATOR + getFolderStringNameHidden(ftype));
        yarp::os::mkdir(hiddenPath.c_str());
#ifdef WIN32
        SetFileAttributes(hiddenPath.c_str(), FILE_ATTRIBUTE_HIDDEN);
#endif
    }
}
Esempio n. 14
0
bool QuaternionEKF::readEstimatorParams(yarp::os::ResourceFinder &rf, quaternionEKFParams &estimatorParams)
{
    yarp::os::Bottle botParams;
    botParams = rf.findGroup("QuaternionEKF");
    yInfo("QuaternionEKF params are: %s ", botParams.toString().c_str());
    if ( botParams.isNull() )
    {
        yError("[QuaternionEKF::readEstimatorParams] No parameters were read from QuaternionEKF group");
        return false;
    } else {
        estimatorParams.stateSize = botParams.find("state_size").asInt();
        estimatorParams.inputSize = botParams.find("input_size").asInt();
        estimatorParams.measurementSize = botParams.find("measurement_size").asInt();
        estimatorParams.muSystemNoise = botParams.find("mu_system_noise").asDouble();
        estimatorParams.sigmaSystemNoise = botParams.find("sigma_system_noise").asDouble();
        estimatorParams.sigmaMeasurementNoise = botParams.find("sigma_measurement_noise").asDouble();
        estimatorParams.sigmaGyro = botParams.find("sigma_gyro_noise").asDouble();
        estimatorParams.piorMu = botParams.find("prior_mu_state").asDouble();
        estimatorParams.priorCovariance = botParams.find("prior_cov_state").asDouble();
        estimatorParams.muGyroNoise = botParams.find("mu_gyro_noise").asDouble();
        estimatorParams.floatingBaseAttitude = botParams.find("floating_base_attitude").asBool();
        estimatorParams.rot_from_ft_to_acc_bottle = new yarp::os::Bottle(*botParams.find("rot_from_ft_to_acc").asList());
    }

    botParams.clear();
    botParams = rf.findGroup("module_parameters");
    if ( botParams.isNull() )
    {
        yError("[QuaternionEKF::readEstimatorParams] No parameters were read from 'module_params' group. period is needed!");
        return false;
    } else {
        estimatorParams.period = botParams.find("period").asInt();
        estimatorParams.robotPrefix = botParams.find("robot").asString();
        estimatorParams.streamMeasurements = botParams.find("stream_measurements").asBool();
    }
    return true;
}
Esempio n. 15
0
bool ears::configure(yarp::os::ResourceFinder &rf)
{
    string moduleName = rf.check("name", Value("ears")).asString().c_str();
    setName(moduleName.c_str());

    yInfo() << moduleName << " : finding configuration files...";
    period = rf.check("period", Value(0.1)).asDouble();

    //Create an iCub Client and check that all dependencies are here before starting
    bool isRFVerbose = false;
    iCub = new ICubClient(moduleName, "ears", "client.ini", isRFVerbose);
    iCub->opc->isVerbose = false;
    if (!iCub->connect())
    {
        yInfo() << " iCubClient : Some dependencies are not running...";
        Time::delay(1.0);
    }
    rpc.open(("/" + moduleName + "/rpc").c_str());
    attach(rpc);

    portToBehavior.open("/" + moduleName + "/behavior:o");
    while (!Network::connect(portToBehavior.getName(),"/BehaviorManager/trigger:i ")) {
        yWarning() << " Behavior is not reachable";
        yarp::os::Time::delay(0.5);
    }

    portTarget.open("/" + moduleName + "/target:o");

    MainGrammar = rf.findFileByName(rf.check("MainGrammar", Value("MainGrammar.xml")).toString());

    bShouldListen = true;

    yInfo() << "\n \n" << "----------------------------------------------" << "\n \n" << moduleName << " ready ! \n \n ";
    
    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;         
}
Esempio n. 17
0
    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        yarp::os::Time::turboBoost();
        
        mNavThread=new Navigator(&rf);

        if (!mNavThread->start())
        {
            delete mNavThread;
            return false;
        }

        std::string local=rf.check("local",yarp::os::Value("/ikartnav")).asString().c_str();
        mHandlerPort.open(local.c_str());
        attach(mHandlerPort);
        //attachTerminal();

        return true;
    }
Esempio n. 18
0
 bool configure( yarp::os::ResourceFinder &rf )
   {
       std::string moduleName = rf.check("name", 
               yarp::os::Value("demoServerModule"), 
               "module name (string)").asString().c_str();
       setName(moduleName.c_str());
       
       std::string slash="/";
       
       attach(cmdPort);
       
       std::string cmdPortName= "/";
       cmdPortName+= getName();
       cmdPortName += "/cmd";
       if (!cmdPort.open(cmdPortName.c_str())) {           
           std::cout << getName() << ": Unable to open port " << cmdPortName << std::endl;  
           return false;
       }
       return true;
   }   
    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;
    }
Esempio n. 20
0
    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        yarp::os::Time::turboBoost();

        Property p;
        ConstString configFile = rf.findFile("from");
        if (configFile!="") p.fromConfigFile(configFile.c_str());

        gotoThread = new GotoThread(10,rf,p);

        if (!gotoThread->start())
        {
            delete gotoThread;
            return false;
        }

        rpcPort.open("/ikartGoto/rpc:i");
        attach(rpcPort);
        //attachTerminal();

        return true;
    }
Esempio n. 21
0
bool Recognition::initPorts(yarp::os::ResourceFinder &rf){
	moduleName            = rf.check("name", 
                           Value("Recognition"), 
                           "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());

    // Open cam left
	camLeftName = "/";
    camLeftName += getName() + "/in/left";

    if (!camLeft.open(camLeftName.c_str())) {           
        cout << getName() << ": Unable to open port " << camLeftName << endl;  
        return false;
    }
   
	portAckName = "/";
    portAckName += getName() + "/out";

    if (!portAck.open(portAckName.c_str())) {           
        cout << getName() << ": Unable to open port " << portAckName << endl;  
        return false;
    }
   
	portInName = "/";
    portInName += getName() + "/in";

    if (!portIn.open(portInName.c_str())) {           
        cout << getName() << ": Unable to open port " << portInName << endl;  
        return false;
    }
    attach(portIn);
    
    return true;
}
Esempio n. 22
0
bool DispBlobberModule::configure(yarp::os::ResourceFinder &rf)
{
    moduleName = rf.check("name", Value("dispBlobber"), "module name (string)").asString();

    setName(moduleName.c_str());

    handlerPortName =  "/";
    handlerPortName += getName();
    handlerPortName +=  "/rpc:i";

    if (!handlerPort.open(handlerPortName.c_str()))    {
        fprintf(stdout, "%s : Unable to open RPC port %s\n", getName().c_str(), handlerPortName.c_str());
        return false;
    }
    attach(handlerPort);

    blobPort = new DispBlobberPort( moduleName, rf );

    blobPort->open();

    closing = false;

    return true ;
}
Esempio n. 23
0
bool babbler::configure(yarp::os::ResourceFinder &rf)
{

    elapsedCycles = 0;
    f = 0;
    string moduleName = rf.check("name", Value("tuneBabbler")).asString().c_str();
    setName(moduleName.c_str());

    string moduleOutput = rf.check("output", Value("/microphone")).asString().c_str();

    int seed = rf.check("seed", Value(1)).asInt();

    samples = rf.check("samples", Value(512)).asInt();
    rate = rf.check("rate", Value(8000)).asInt();
    channels = rf.check("channels", Value(1)).asInt();

    bool    bEveryThingisGood = true;

    // create an output port
    port2output = moduleOutput;

    if (!portOutput.open(port2output.c_str())) {
        cout << getName() << ": Unable to open port " << port2output << endl;
        cout << "The microphone might be turned on" << endl;
        bEveryThingisGood = false;
    }

    cout << moduleName << ": finding configuration files..." << endl;

    rpc.open(("/" + moduleName + "/rpc").c_str());
    attach(rpc);

    srand(seed);

    return bEveryThingisGood;
}
Esempio n. 24
0
    /**
    * Performs module configuration, parsing user options stored in the resource finder.
    * Available options are described in main module documentation.
    * @return true if the module was successfully configured and opened, false otherwise.
    */
    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        yarp::os::Time::turboBoost();

        m_rpcPort.open("/"+m_module_name+"/rpc");
        attach(m_rpcPort);
        //attachTerminal();
        m_rf = rf;
        
        //configuration file checking
        Bottle general_group = m_rf.findGroup("GENERAL");
        if (general_group.isNull())
        {
            yError() << "Missing GENERAL group!";
            return false;
        }

        Bottle initial_group = m_rf.findGroup("INITIAL_POS");
        if (initial_group.isNull())
        {
            yError() << "Missing INITIAL_POS group!";
            return false;
        }

        Bottle localization_group = m_rf.findGroup("LOCALIZATION");
        if (localization_group.isNull())
        {
            yError() << "Missing LOCALIZATION group!";
            return false;
        }

        Bottle ros_group = m_rf.findGroup("ROS");

        Bottle tf_group = m_rf.findGroup("TF");
        if (tf_group.isNull())
        {
            yError() << "Missing TF group!";
            return false;
        }

        Bottle odometry_group = m_rf.findGroup("ODOMETRY");
        if (odometry_group.isNull())
        {
            yError() << "Missing ODOMETRY group!";
            return false;
        }

        Bottle map_group = m_rf.findGroup("MAP");
        if (map_group.isNull())
        {
            yError() << "Missing MAP group!";
            return false;
        }
        yDebug() << map_group.toString();

        //general group
        if (general_group.check("module_name") == false)
        {
            yError() << "Missing `module_name` in [GENERAL] group";
            return false;
        }
        m_module_name = general_group.find("module_name").asString();

        if (general_group.check("enable_ros") == false)
        {
            yError() << "Missing `ros_enable` in [GENERAL] group";
            return false;
        }
        m_ros_enabled = (general_group.find("enable_ros").asInt()==1);

        //ROS group
        if (m_ros_enabled)
        {
            m_rosNode = new yarp::os::Node("/"+m_module_name);
            
            //initialize an occupancy grid publisher (every time the localization is re-initialized, the map is published too)
            if (ros_group.check ("occupancygrid_topic"))
            {
                m_topic_occupancyGrid = ros_group.find ("occupancygrid_topic").asString();
                if (!m_rosPublisher_occupancyGrid.topic(m_topic_occupancyGrid))
                {
                     if (m_rosNode)
                     {
                         delete m_rosNode;
                         m_rosNode=0;
                     }
                     yError() << "localizationModule: unable to publish data on " << m_topic_occupancyGrid << " topic, check your yarp-ROS network configuration";
                     return false;
                }
            }

             //initialize an initial pose publisher
            if (ros_group.check ("initialpose_topic")) 
            {
                m_topic_initial_pose = ros_group.find ("initialpose_topic").asString();
                {
                    if (!m_rosPublisher_initial_pose.topic(m_topic_initial_pose))
                    {
                        if (m_rosNode)
                        {
                            delete m_rosNode;
                            m_rosNode=0;
                        }
                        yError() << "localizationModule: unable to publish data on " << m_topic_initial_pose << " topic, check your yarp-ROS network configuration";
                        return false;
                    }
                }
            }
        }

        //localization group
        if (localization_group.check("use_localization_from_odometry_port")) { m_use_localization_from_odometry_port = (localization_group.find("use_localization_from_odometry_port").asInt() == 1); }
        if (localization_group.check("use_localization_from_tf"))   { m_use_localization_from_tf = (localization_group.find("use_localization_from_tf").asInt() == 1); }

        if (m_use_localization_from_odometry_port == true && m_use_localization_from_tf == true)
        {
            yError() << "`use_localization_from_tf` and `use_localization_from_odometry_port` cannot be true simultaneously!";
            return false;
        }

        //map server group
        yDebug() << map_group.toString();

        if (map_group.check("connect_to_yarp_mapserver") == false)
        {
            yError() << "Missing `connect_to_yarp_mapserver` in [MAP] group";
            return false;
        }
        m_use_map_server= (map_group.find("connect_to_yarp_mapserver").asInt()==1);

        //tf group
        if (tf_group.check("map_frame_id") == false)
        {
            yError() << "Missing `map_frame_id` in [TF] group";
            return false;
        }
        if (tf_group.check("robot_frame_id") == false)
        {
            yError() << "Missing `robot_frame_id` in [TF] group";
            return false;
        }
        m_frame_map_id = tf_group.find("map_frame_id").asString();
        m_frame_robot_id = tf_group.find("robot_frame_id").asString();

        //odometry group
        if (odometry_group.check("odometry_broadcast_port") == false)
        {
            yError() << "Missing `odometry_port` in [ODOMETRY] group";
            return false;
        }
        m_port_broadcast_odometry_name = odometry_group.find("odometry_broadcast_port").asString();

        //device driver opening and/or connections
        if (m_use_localization_from_odometry_port)
        {
            //opens a YARP port to receive odometry data
            std::string odom_portname = "/" + m_module_name + "/odometry:i";
            bool b1 = m_port_odometry_input.open(odom_portname.c_str());
            bool b2 = yarp::os::Network::sync(odom_portname.c_str(),false);
            bool b3 = yarp::os::Network::connect(m_port_broadcast_odometry_name.c_str(), odom_portname.c_str());
            if (b1 == false || b2 ==false || b3==false)
            {
                yError() << "Unable to initialize odometry port connection from " << m_port_broadcast_odometry_name.c_str()<< "to:" << odom_portname.c_str();
                return false;
            }
        }

        if (m_use_localization_from_tf)
        {
            //opens a client to receive localization data from transformServer
            Property options;
            options.put("device", "transformClient");
            options.put("local", "/"+m_module_name + "/TfClient");
            options.put("remote", "/transformServer");
            if (m_ptf.open(options) == false)
            {
                yError() << "Unable to open transform client";
                return false;
            }
            m_ptf.view(m_iTf);
            if (m_ptf.isValid() == false || m_iTf == 0)
            {
                yError() << "Unable to view iTransform interface";
                return false;
            }
        }

        if (m_use_map_server)
        {
            //opens a client to send/received data from mapServer
            Property map_options;
            map_options.put("device", "map2DClient");
            map_options.put("local", "/" +m_module_name); //This is just a prefix. map2DClient will complete the port name.
            map_options.put("remote", "/mapServer");
            if (m_pmap.open(map_options) == false)
            {
                yWarning() << "Unable to open mapClient";
            }
            else
            {
                yInfo() << "Opened mapClient";
                m_pmap.view(m_iMap);
                if (m_pmap.isValid() == false || m_iMap == 0)
                {
                    yError() << "Unable to view map interface";
                    return false;
                }
            }
        }

        //initial location initialization
        if (initial_group.check("initial_x"))     { m_initial_loc.x = initial_group.find("initial_x").asDouble(); }
        else { yError() << "missing initial_x param"; return false; }
        if (initial_group.check("initial_y"))     { m_initial_loc.y = initial_group.find("initial_y").asDouble(); }
        else { yError() << "missing initial_y param"; return false; }
        if (initial_group.check("initial_theta")) { m_initial_loc.theta = initial_group.find("initial_theta").asDouble(); }
        else { yError() << "missing initial_theta param"; return false; }
        if (initial_group.check("initial_map"))   { m_initial_loc.map_id = initial_group.find("initial_map").asString(); }
        else { yError() << "missing initial_map param"; return false; }
        this->initializeLocalization(m_initial_loc);

        return true;
    }
Esempio n. 25
0
bool BehaviorManager::configure(yarp::os::ResourceFinder &rf)
{
    moduleName = rf.check("name",Value("BehaviorManager")).asString();
    setName(moduleName.c_str());
    yInfo()<<moduleName<<": finding configuration files...";//<<endl;
    period = rf.check("period",Value(1.0)).asDouble();

    Bottle grp = rf.findGroup("BEHAVIORS");
    Bottle behaviorList = *grp.find("behaviors").asList();

    rpc_in_port.open("/" + moduleName + "/trigger:i");
    yInfo() << "RPC_IN : " << rpc_in_port.getName();

    for (int i = 0; i<behaviorList.size(); i++)
    {
        behavior_name = behaviorList.get(i).asString();
        if (behavior_name == "tagging") {
            behaviors.push_back(new Tagging(&mut, rf, "tagging"));
        } else if (behavior_name == "pointing") {
            behaviors.push_back(new Pointing(&mut, rf, "pointing"));
        } else if (behavior_name == "dummy") {
            behaviors.push_back(new Dummy(&mut, rf, "dummy"));
        } else if (behavior_name == "dummy2") {
            behaviors.push_back(new Dummy(&mut, rf, "dummy2"));
        }  else if (behavior_name == "reactions") {
            behaviors.push_back(new Reactions(&mut, rf, "reactions"));
        }  else if (behavior_name == "followingOrder") {
            behaviors.push_back(new FollowingOrder(&mut, rf, "followingOrder"));
        }  else if (behavior_name == "narrate") {
            behaviors.push_back(new Narrate(&mut, rf, "narrate"));
        }  else if (behavior_name == "recognitionOrder") {
            behaviors.push_back(new RecognitionOrder(&mut, rf, "recognitionOrder"));
        }  else if (behavior_name == "greeting") {
            behaviors.push_back(new Greeting(&mut, rf, "greeting"));
        }  else if (behavior_name == "ask") {
            behaviors.push_back(new Ask(&mut, rf, "ask"));
        }  else if (behavior_name == "speech") {
            behaviors.push_back(new Speech(&mut, rf, "speech"));
        } else if (behavior_name == "moveObject") {
            behaviors.push_back(new MoveObject(&mut, rf, "moveObject"));
            // other behaviors here
        }  else {
            yDebug() << "Behavior " + behavior_name + " not implemented";
            return false;
        }
    }

    //Create an iCub Client and check that all dependencies are here before starting
    bool isRFVerbose = false;
    iCub = new wysiwyd::wrdac::ICubClient(moduleName, "behaviorManager","client.ini",isRFVerbose);

    if (!iCub->connect())
    {
        yInfo() << "iCubClient : Some dependencies are not running...";
        Time::delay(1.0);
    }
    if (rf.check("use_ears",Value("false")).asBool())
    {
        yDebug()<<"using ears";
        while (!Network::connect("/ears/behavior:o", rpc_in_port.getName())) {
            yWarning() << "Ears is not reachable";
            yarp::os::Time::delay(0.5);
        }
    }else{
        yDebug()<<"not using ears";
    }

    // id = 0;
    for(auto& beh : behaviors) {
        beh->configure();
        beh->openPorts(moduleName);
        beh->iCub = iCub;

        if (beh->from_sensation_port_name != "None") {
            while (!Network::connect(beh->from_sensation_port_name, beh->sensation_port_in.getName())) {
                yInfo()<<"Connecting "<< beh->from_sensation_port_name << " to " <<  beh->sensation_port_in.getName();// <<endl;
                yarp::os::Time::delay(0.5);
            }
        }
        if (beh->external_port_name != "None") {
            while (!Network::connect(beh->rpc_out_port.getName(), beh->external_port_name)) {
                yInfo()<<"Connecting "<< beh->rpc_out_port.getName() << " to " <<  beh->external_port_name;// <<endl;
                yarp::os::Time::delay(0.5);
            }   
        }

    }

    attach(rpc_in_port);
    yInfo("Init done");

    return true;
}
Esempio n. 26
0
bool Rectification::configure(yarp::os::ResourceFinder &rf)
{    
   /*
    * Process all parameters from 
    *  - command-line 
    *  - rectification.ini file (or whatever file is specified by the --from argument)
    *  - icubEyes.ini file (or whatever file is specified by the --cameraConfig argument
    */

   /* get the module name which will form the stem of all module port names */

   moduleName            = rf.check("name", 
                           Value("rectification"), 
                           "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();


   /* 
    * get the cameraConfig file and read the required parameter values, fx, fy, cx, cy 
    * in both the groups [CAMERA_CALIBRATION_LEFT] and [CAMERA_CALIBRATION_RIGHT]
    */

   cameraConfigFilename  = rf.check("cameraConfig", 
                           Value("icubEyes.ini"), 
                           "camera configuration filename (string)").asString();

   cameraConfigFilename = (rf.findFile(cameraConfigFilename.c_str())).c_str();

   Property cameraProperties;

   if (cameraProperties.fromConfigFile(cameraConfigFilename.c_str()) == false) {
      cout << "rectification: unable to read camera configuration file" << cameraConfigFilename;
      return 0;
   }
   else {
      fxLeft  = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_LEFT").check("fx", Value(225.0), "fx left").asDouble();
      fyLeft  = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_LEFT").check("fy", Value(225.0), "fy left").asDouble();
      cxLeft  = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_LEFT").check("cx", Value(160.0), "cx left").asDouble();
      cyLeft  = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_LEFT").check("cy", Value(120.0), "cy left").asDouble();
      fxRight = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_RIGHT").check("fx", Value(225.0), "fx right").asDouble();
      fyRight = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_RIGHT").check("fy", Value(225.0), "fy right").asDouble();
      cxRight = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_RIGHT").check("cx", Value(160.0), "cx right").asDouble();
      cyRight = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_RIGHT").check("cy", Value(120.0), "cy right").asDouble();
   }

   /* get the name of the input and output ports, automatically prefixing the module name by using getName() */

   leftInputPortName     = "/";
   leftInputPortName    += getName(
                           rf.check("leftImageInPort", 
                           Value("/leftImage:i"),
                           "Left input image port (string)").asString()
                           );
   
   rightInputPortName    = "/";
   rightInputPortName   += getName(
                           rf.check("rightImageInPort", 
                           Value("/rightImage:i"),
                           "Right input image port (string)").asString()
                           );

   leftOutputPortName    = "/";
   leftOutputPortName   += getName(
                           rf.check("leftImageOutPort", 
                           Value("/leftImage:o"),
                           "Left output image port (string)").asString()
                           );
   
   rightOutputPortName   = "/";
   rightOutputPortName  += getName(
                           rf.check("rightImageOutPort", 
                           Value("/rightImage:o"),
                           "Right output image port (string)").asString()
                           );

   robotPortName         = "/";
   robotPortName        += getName(
                           rf.check("headPort", 
                           Value("/head:i"),
                           "Robot head encoder state port (string)").asString()
                           );

   if (debug) {
      printf("rectification: module name is %s\n",moduleName.c_str());
      printf("rectification: robot name is %s\n",robotName.c_str());
      printf("rectification: camera configuration filename is %s\n",cameraConfigFilename.c_str());
      printf("rectification: camera properties are\n%f\n%f\n%f\n%f\n%f\n%f\n%f\n",fxLeft,fyLeft,cxLeft,cyLeft,fxRight,fyRight,cxRight,cyRight);
      printf("rectification: port names are\n%s\n%s\n%s\n%s\n%s\n\n",leftInputPortName.c_str(),
                                                                   rightInputPortName.c_str(),
                                                                   leftOutputPortName.c_str(),
                                                                   rightOutputPortName.c_str(),
                                                                   robotPortName.c_str(),
                                                                   cameraConfigFilename.c_str()
                                                                   );
   }
    


   /* do all initialization here */
     
   /* open ports  */ 
       
   if (!leftImageIn.open(leftInputPortName.c_str())) {
      cout << getName() << ": unable to open port " << leftInputPortName << endl;
      return false;  // unable to open; let RFModule know so that it won't run
   }

   if (!rightImageIn.open(rightInputPortName.c_str())) {
      cout << getName() << ": unable to open port " << rightInputPortName << endl;
      return false;  // unable to open; let RFModule know so that it won't run
   }

   if (!leftImageOut.open(leftOutputPortName.c_str())) {
      cout << getName() << ": unable to open port " << leftOutputPortName << endl;
      return false;  // unable to open; let RFModule know so that it won't run
   }
      
   if (!rightImageOut.open(rightOutputPortName.c_str())) {
      cout << getName() << ": unable to open port " << rightOutputPortName << endl;
      return false;  // unable to open; let RFModule know so that it won't run
   }

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

   /*
    * 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
 
   /* create the thread and pass pointers to the module parameters */

   rectificationThread = new RectificationThread(&leftImageIn, &rightImageIn, &leftImageOut, &rightImageOut, &robotPort,
                                                 &fxLeft, &fyLeft, &cxLeft, &cyLeft, &fxRight, &fyRight, &cxRight, &cyRight);

   /* now start the thread to do the work */

   rectificationThread->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
}
Esempio n. 27
0
bool faceTrackerModule::configure(yarp::os::ResourceFinder &rf) {

    moduleName = rf.check("name",
                        Value("faceTracker"),
                        "module name (string)").asString();

    setName(moduleName.c_str());

    opcName = rf.check("opcName",
                        Value("OPC"),
                        "Opc name (string)").asString();

    string xmlPath = rf.findFileByName("haarcascade_frontalface_default.xml");

    // Create an iCub Client and check that all dependencies are here befor starting
    opc = new OPCClient(moduleName.c_str());
    opc->connect(opcName);
    icub = NULL;

    handlerPortName = "/";
    handlerPortName +=  getName() + "/rpc";

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

	// ==================================================================
    // image port open
    imagePortLeft.open("/facetracking/image/left/in");	// give the left port a name

	// ==================================================================
    // robot
    // ==================================================================
    while(!Network::connect("/icub/camcalib/left/out", "/facetracking/image/left/in"))
    {
        Time::delay(3);
        cout << "try to connect left camera, please wait ..." << endl;
    }

    Property options;
    options.put("device", "remote_controlboard");
    options.put("local", "/tutorial/motor/client");
    options.put("remote", "/icub/head");

    robotHead = new PolyDriver(options);

    if(!robotHead->isValid())
    {
        cout << "Cannot connect to the robot head" << endl;
        return false;
    }

    robotHead->view(pos);
    robotHead->view(vel);
    robotHead->view(enc);

    if(pos==NULL || vel==NULL || enc==NULL)
    {
        cout << "Cannot get interface to robot head" << endl;
        robotHead->close();
        return false;
    }
    jnts = 0;
    pos->getAxes(&jnts);

	setpoints.resize(jnts);
	cur_encoders.resize(jnts);
	prev_encoders.resize(jnts);

	/* prepare command */
	for(int i=0;i<jnts;i++)
	{
		setpoints[i] = 0;
	}

	// ==================================================================
	//// create a opencv window
	cv::namedWindow("cvImage_Left",CV_WINDOW_NORMAL);

	// ==================================================================
	// face detection configuration
    face_classifier_left.load(xmlPath);

    attach(handlerPort);                  // attach to port

	// ==================================================================
    // Parameters
	counter = 0;
	x_buf = 0;
	y_buf = 0;

	mode = 0; // 0: going to a set position, 1: face searching, 2: face tracking, 3: face stuck,
	setpos_counter = 0;
	panning_counter = 0;
	stuck_counter = 0;
	tracking_counter = 0;

    // ==================================================================
	// random motion
	tilt_target = 0;
	pan_target = 0;

	seed = 10000;
	srand(seed);
	pan_max = 80;
	tilt_max = 20;

	cvIplImageLeft = NULL;



    return true ;
}
Esempio n. 28
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);
}
Esempio n. 29
0
void AdaptiveLayer::configureOPC(yarp::os::ResourceFinder &rf)
{
    //Populate the OPC if required
    cout<<"Populating OPC";
    Bottle grpOPC = rf.findGroup("OPC");
    bool shouldPopulate = grpOPC.find("populateOPC").asInt() == 1;
    if (shouldPopulate)
    {
        Bottle *agentList = grpOPC.find("agent").asList();
        if (agentList)
        {
            for(int d=0; d<agentList->size(); d++)
            {
                string name = agentList->get(d).asString().c_str();
                Agent* agent = iCub->opc->addOrRetrieveEntity<Agent>(name);
                agent->m_present = false;
                iCub->opc->commit(agent);
            }
        }

        Bottle *objectList = grpOPC.find("object").asList();
        if (objectList)
        {
            for(int d=0; d<objectList->size(); d++)
            {
                string name = objectList->get(d).asString().c_str();
                Object* o = iCub->opc->addOrRetrieveEntity<Object>(name);
                o->m_present = false;
                iCub->opc->commit(o);
            }
        }

        Bottle *rtobjectList = grpOPC.find("rtobject").asList();
        if (rtobjectList)
        {
            for(int d=0; d<rtobjectList->size(); d++)
            {
                string name = rtobjectList->get(d).asString().c_str();
                RTObject* o = iCub->opc->addOrRetrieveEntity<RTObject>(name);
                o->m_present = false;
                iCub->opc->commit(o);
            }
        }

        Bottle *adjectiveList = grpOPC.find("adjective").asList();
        if (adjectiveList)
        {
            for(int d=0; d<adjectiveList->size(); d++)
            {
                string name = adjectiveList->get(d).asString().c_str();
                iCub->opc->addOrRetrieveEntity<Adjective>(name);
            }
        }

        Bottle *actionList = grpOPC.find("action").asList();
        if (actionList)
        {
            for(int d=0; d<actionList->size(); d++)
            {
                string name = actionList->get(d).asString().c_str();
                iCub->opc->addOrRetrieveEntity<Action>(name);
            }
        }
    }
    cout<<"done"<<endl;
}
Esempio n. 30
0
bool PasarModule::configure(yarp::os::ResourceFinder &rf) {    
    std::string opcName;
    std::string gazePortName;
    std::string handlerPortName;
    std::string saliencyPortName;

    moduleName            = rf.check("name", 
        Value("pasar"), 
        "module name (string)").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(); 

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

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

    isControllingMotors	= rf.check("motorControl", 
        Value(0)).asInt() == 1; 
    //Ports
    opcName             = rf.check("opcName", 
        Value("OPC"), 
        "Opc name (string)").asString();
    opc = new OPCClient(moduleName.c_str());
    opc->connect(opcName);
    if (!opc->isConnected())
        if (!opc->connect("OPC"))
            return false;
    opc->checkout();

    icub = opc->addAgent("icub");

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

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

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

    gazePortName = "/";
    gazePortName += getName() + "/gaze";
    Property option;
    option.put("device","gazecontrollerclient");
    option.put("remote","/iKinGazeCtrl");
    option.put("local", gazePortName.c_str());

    if (isControllingMotors)
    {
        igaze=NULL;
        if (clientGazeCtrl.open(option)) {
            clientGazeCtrl.view(igaze);
        }
        else
        {
            cout<<"Invalid gaze polydriver"<<endl;
            return false;
        }
        igaze->storeContext(&store_context_id);

        double neckTrajTime = rf.check("neckTrajTime", Value(0.75)).asDouble();
        igaze->setNeckTrajTime(neckTrajTime);
    }

    attach(handlerPort);                  // attach to port
    trackedObject = "";
    presentObjectsLastStep.clear();
    return true ;
}