bool actionClass::openFile2(std::string filename, yarp::os::ResourceFinder &rf)
{
    bool ret = true;
    FILE* data_file1 = 0;
    FILE* data_file2 = 0;
    FILE* data_file3 = 0;
    string filename_left  = filename + "_left" + ".txt";
    string filename_right = filename + "_right" + ".txt";
    string filename_torso = filename + "_torso" + ".txt";
    filename_left  = rf.findFile(filename_left);
    filename_right = rf.findFile(filename_right);
    filename_torso = rf.findFile(filename_torso);
    fprintf(stderr, "||| File found for left leg: %s\n", filename_left.c_str());
    fprintf(stderr, "||| File found for right leg: %s\n", filename_right.c_str());
    fprintf(stderr, "||| File found for torso: %s\n", filename_torso.c_str());
    data_file1 = fopen(filename_left.c_str(),"r");
    data_file2 = fopen(filename_right.c_str(),"r");
    data_file3 = fopen(filename_torso.c_str(),"r");

    if (data_file1 != NULL && data_file2 != NULL && data_file3 != NULL)
    {
        char* bb1 = 0;
        char* bb2 = 0;
        char* bb3 = 0;
        int   line =0;
        do
        {
            char trajectory_line1[1024];
            char trajectory_line2[1024];
            char trajectory_line3[1024];
            bb1 = fgets (trajectory_line1, 1024, data_file1);
            bb2 = fgets (trajectory_line2, 1024, data_file2);
            bb3 = fgets (trajectory_line3, 1024, data_file3);
            if (bb1 == 0 || bb2 == 0 || bb3 == 0) break;
            if(!parseCommandLine2(trajectory_line1, trajectory_line2, trajectory_line3, line++))
                {
                    printf ("error parsing file, line %d\n", line);
                    ret = false;
                    break;
                };
        }
        while (1);

        fclose (data_file1);
        fclose (data_file2);
        fclose (data_file3);
    }
    else
    {
        //file not opened
        ret = false;
    }
    return ret;
}
Beispiel #2
0
bool RobotInterface::Module::configure(yarp::os::ResourceFinder &rf)
{
    if (!rf.check("config")) {
        yFatal() << "Missing \"config\" argument";
    }

    const yarp::os::ConstString &filename = rf.findFile("config");
    yTrace() << "Reading robot config file" << filename;

    RobotInterface::XMLReader reader;
    mPriv->robot = reader.getRobot(filename.c_str());
    // yDebug() << mPriv->robot;

    // User can use YARP_PORT_PREFIX environment variable to override
    // the default name, so we don't care of handling the --name
    // argument
    setName(mPriv->robot.portprefix().c_str());

    // Enter startup phase
    if (!mPriv->robot.enterPhase(RobotInterface::ActionPhaseStartup)) {
        yError() << "Error in startup phase... see previous messages for more info";
        return false;
    }

    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;         
}
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;
}
Beispiel #5
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;
}
Beispiel #6
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;
    }
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
}
Beispiel #8
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
}
bool asvGrabberModule::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("/asvGrabber"),
                                     "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";

    /*
    * get the device name which will be used to read events
    */
    deviceName             = rf.check("deviceName",
                                      Value("/dev/aerfx2_0"),
                                      "Device name (string)").asString();
    devicePortName         =  deviceName ;
    printf("trying to connect to the device %s \n",devicePortName.c_str());

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

    bool _save = false;
    std::string deviceNum = "0";

    /*
    * get the file name of binaries when the biases are read from this file
    */
    binaryName             = rf.check("file",
                                      Value("none"),
                                      "filename of the binary (string)").asString();
    printf("trying to read %s  for biases \n",binaryName.c_str());
    binaryNameComplete = rf.findFile(binaryName.c_str());

    /*
    * get the file name of binaries when the biases are read from this file
    */
    dumpNameComplete = "";
    dumpName             = rf.check("dumpFile",
                                    Value("none"),
                                    "filename of the binary (string)").asString();
    printf("trying to save events in %s  \n",dumpName.c_str());
    dumpNameComplete  = rf.findFile(dumpName.c_str());
    dumpPathDirectory = rf.getContextPath();
    string pathDirectory = dumpPathDirectory.substr(0,  52);
    pathDirectory += "/";
    printf("saving directory %s  \n",dumpPathDirectory.c_str());



    if(!strcmp(binaryName.c_str(),"none")) {
        printf("not reading from binary \n");
        D2Y=new asvGrabberThread(devicePortName, false,binaryName);
        //D2Y->setFromBinary(false);
    }
    else {
        printf("reading from binary \n");
        //D2Y->setFromBinary(true);
        D2Y=new asvGrabberThread(devicePortName, true, binaryNameComplete);
        //D2Y->setBinaryFile(f);
    }
    printf("dumpEventSet \n");
    if (strcmp(dumpNameComplete.c_str(),"" )) {
        printf("set dumping event true \n");
        D2Y->setDumpEvent(true);
        D2Y->setDumpFile(dumpNameComplete);
        D2Y->setWorkingDirectory(pathDirectory);
    }
    else {
        printf("set dumping event false \n");
        D2Y->setDumpEvent(false);
    }
    printf("starting the processor \n");
    D2Y->start();

    return true ;       // let the RFModule know everything went well
    // so that it will then run the module
}
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;
    }
}
bool targetFinderModule::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("/targetFinder"), 
                           "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";

    configName             = rf.check("config", 
                           Value("icubEyes.ini"), 
                           "Config file for intrinsic parameters (string)").asString();
    printf("configFile: %s \n", configName.c_str());

    if (strcmp(configName.c_str(),"")) {
        printf("looking for the config file \n");
        configFile=rf.findFile(configName.c_str());
        printf("config file %s \n", configFile.c_str());
        if (configFile=="") {
            printf("ERROR: file not found");
            return false;
        }
    }
    else {
        configFile.clear();
    }
    
    /*
    * set the operating mode which correspond as well with the file map saved in conf
    */
    mapName             = rf.check("mode", 
                                   Value("intensity"), 
                                   "file map name (string)").asString();
    mapName += ".txt";
    mapNameComplete = rf.findFile(mapName.c_str());

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

    tf = new targetFinderThread();
    tf->setMapURL(mapNameComplete);
    tf->setRobotName(robotName);
    tf->setConfigFile(configFile);
    tf->setName(getName().c_str());
    tf->start();

    return true ;       // let the RFModule know everything went well
                        // so that it will then run the module
}
Beispiel #12
0
bool ImageSource::configure(yarp::os::ResourceFinder &rf)
{    

   debug = true;
   
   /* 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("imageSource"), 
                           "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());

   /* now, get the rest of the parameters */

   /* 
    * get the imageFilename
    */

   imageFilename  = rf.check("imageFile", 
                             Value("image.bmp"), 
                             "image filename (string)").asString();

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

   /* get the complete name of the image output port */

   outputPortName        = rf.check("outputPort", 
                           Value("/image:o"),
                           "Output image port (string)").asString();

   /* get the complete name of the gaze output port */

   gazePortName          = rf.check("gazePort", 
                           Value("/gaze:o"),
                           "Output gaze port (string)").asString();

   /* get the complete name of the encoder state output port */

   encoderPortName       = rf.check("encoderPort", 
                           Value("/icub/head/state:o"),
                           "Output encoder port (string)").asString();

   /* get the frequency, width, height, standard deviation, horizontalViewAngle, and verticalViewAngle values */

   frequency             = rf.check("frequency",
                           Value(10),
                           "frequency key value (int)").asInt();

   width                 = rf.check("width",
                           Value(320),
                           "output width key value (int)").asInt();

   height                = rf.check("height",
                           Value(240),
                           "output height key value (int)").asInt();

   noiseLevel            = rf.check("noise",
                           Value(20),
                           "noise level key value (int)").asInt();
 
   window                = rf.check("window",
                           Value(0),
                           "window flag key value (int)").asInt();
 
   random                = rf.check("random",
                           Value(0),
                           "random flag key value (int)").asInt();
 
   xShift                = rf.check("xShift",
                           Value(5),
                           "horizontal shift key value (int)").asInt();

   yShift                = rf.check("yShift",
                           Value(5),
                           "vertical shift key value (int)").asInt();

   horizontalViewAngle   = rf.check("horizontalViewAngle",
                           Value(120.0),
                           "horizontal field of view angle key value (double)").asDouble();

   verticalViewAngle     = rf.check("verticalViewAngle",
                           Value(90.0),
                           "vertical field of view angle key value (double)").asDouble();


   if (debug) {
      cout << "imageSource::configure: image file name   " << imageFilename << endl;
      cout << "imageSource::configure: output port name  " << outputPortName << endl;
      cout << "imageSource::configure: gaze port name    " << gazePortName << endl;
      cout << "imageSource::configure: encoder port name " << encoderPortName << endl;
      cout << "imageSource::configure: frequency         " << frequency << endl;
      cout << "imageSource::configure: width             " << width << endl;
      cout << "imageSource::configure: height            " << height << endl;
      cout << "imageSource::configure: noise level       " << noiseLevel << endl;
      cout << "imageSource::configure: window flag       " << window << endl;
      cout << "imageSource::configure: random flag       " << random << endl;
      cout << "imageSource::configure: x shift           " << xShift << endl;
      cout << "imageSource::configure: y shift           " << yShift << endl;
      cout << "imageSource::configure: horizontal FoV    " << horizontalViewAngle << endl;
      cout << "imageSource::configure: vertical FoV      " << verticalViewAngle << endl;
   }

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

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

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


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

   //cout << "imageSource::configure: calling Thread constructor"   << endl;

   imageSourceThread = new ImageSourceThread(&imageOut, &gazeOut, &encoderOut, &imageFilename, 
                                             (int)(1000 / frequency), &width, &height, &noiseLevel, &window, &random,
                                             &xShift, &yShift,
                                             &horizontalViewAngle, &verticalViewAngle);

   //cout << "imageSource::configure: returning from Thread constructor"   << endl;

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

   imageSourceThread->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 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
}
bool demoModule::configure(yarp::os::ResourceFinder &rf) {    
    /*
     * PLEASE remove useless comments when writing actual code. If needed then use Doxygen comments and tags.
     */

    /* 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("demoModule"), 
                           "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());

    /* now, get the rest of the parameters */
    /*
    * 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";

    /* 
    * get the cameraConfig file and read the required parameter values 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 << "myModule: unable to read camera configuration file" << cameraConfigFilename;
        return 0;
    }
    else {
        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();
        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() */
    inputPortName         = "/";
    inputPortName        += getName(
                           rf.check("myInputPort", 
                           Value("/demoModule/image:i"),
                           "Input image port (string)").asString()
                           );

    outputPortName        = "/";
    outputPortName       += getName(
                           rf.check("myOutputPort", 
                           Value("/demoModule/image:o"),
                           "Output image port (string)").asString()
                           );

    /* get the threshold value */
    thresholdValue        = rf.check("threshold",
                           Value(8),
                           "Key value (int)").asInt();


    /* do all initialization here */
    /*
    * 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 */
    dThread = new demoThread(&thresholdValue);

    /* now start the thread to do the work */
    dThread->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 wingsTranslatorModule::configure(yarp::os::ResourceFinder &rf) {
    /* Process all parameters from both command-line and .ini file */
     if(rf.check("help")) {
        printf("HELP \n");
        printf("====== \n");
        printf("--name         : changes the rootname of the module ports \n");
        printf("--config       : changes the eye config file e.g. icubEyes.ini");
        printf("--robot        : changes the name of the robot where the module interfaces to  \n"); 
        printf("--tableHeight  : changes the reference height of the plane for homography");
        printf("--kinWingsLeft : look for the kinematic constraint of the camera ");
        printf("--kinWingsRight: look for the kinematic constraint of the camera ");
        printf("press CTRL-C to continue.. \n");
        return true;
    }

    /* get the module name which will form the stem of all module port names */
    moduleName            = rf.check("name", 
                           Value("/wingsTranslator"), 
                           "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";

    /* setting the table height for homography */
    tableHeight             = rf.check("tableHeight", 
                           Value(0.12), 
                           "sets the plane z-axis height for homography (double)").asDouble();
    printf("tableHeight: %f \n", tableHeight);


    /* eyes config file for projection into the image plane */
    configName             = rf.check("config", 
                           Value("icubEyes.ini"), 
                           "Config file for intrinsic parameters (string)").asString();
    printf("configFile: %s \n", configName.c_str());
    if (strcmp(configName.c_str(),"")) {
        printf("looking for the config file \n");
        configFile=rf.findFile(configName.c_str());
        printf("config file %s \n", configFile.c_str());
        if (configFile=="") {
            printf("file not found; the program will proceed with standard values \n");
            return false;
        }
    }
    else {
        configFile.clear();
    }

    /*******************************************************************************************************/
    isOnWings = true;
    printf("trying to read the kinematic chain for the left \n");
    wingsLeftName          = rf.check("kinWingLeft", 
                                      Value("null"),   //wingsKinematic.ini"
                           "Config file for kinematics left wing (string)").asString();
    printf("wingLeftName: %s \n", wingsLeftName.c_str());
    if (strcmp(wingsLeftName.c_str(),"null")) {
        printf("looking for the wingsLeft file \n");
        wingsLeftFile=rf.findFile(wingsLeftName.c_str());
        printf("wings left file %s \n", wingsLeftFile.c_str());
        if (wingsLeftFile=="") {
            printf("ERROR: file not found; the program will proceed with standard values \n");
            isOnWings = false;
            //return false;
        }
    }
    else {
        printf("left : setting isOnWings false because not found \n");
        isOnWings = false;
        wingsLeftFile.clear();
    }

    /******************************************************************************************************/
    printf("trying to read the kinematic chain for the right \n");
    wingsRightName          = rf.check("kinWingRight", 
                                       Value("null"),   //wingsKinematic.ini"
                           "Config file for kinematics right wing (string)").asString();
    printf("wingRightName: %s \n", wingsRightName.c_str());
    if (strcmp(wingsRightName.c_str(),"null")) {
        printf("looking for the wingsRight file \n");
        wingsRightFile=rf.findFile(wingsRightName.c_str());
        printf("wings right file %s \n", wingsRightFile.c_str());
        if (wingsRightFile=="") {
            printf("ERROR: file kinematic right eye not found; the program will proceed with standard values \n");
            isOnWings = false;
            //return false;
        }
    }
    else {
        printf("left : setting isOnWings false because not found \n");
        isOnWings = false;
        wingsRightFile.clear();
    }


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

    tf = new wingsTranslatorThread();
    tf->setMapURL(mapNameComplete);
    tf->setRobotName(robotName);
    tf->setConfigFile(configFile);
    if(isOnWings) {
        printf("setting the isOnWings TRUE in the module \n");
        tf->setIsOnWings(true);
        tf->setWingsLeftFile(wingsLeftFile);
        tf->setWingsRightFile(wingsRightFile);
    }
    else {
        printf("setting the isOnWings FALSE in the module \n");
        tf->setIsOnWings(false);
    }
    tf->setTableHeight(tableHeight);
    tf->setName(getName().c_str());
    tf->start();

    return true ;       // let the RFModule know everything went well
                        // so that it will then run the module
}
bool efExtractorModule::configure(yarp::os::ResourceFinder &rf) {
    /* Process all parameters from both command-line and .ini file */

    if(rf.check("help")) {
        printf("Help \n");
        printf("====  \n");
        printf("--name : name of the module \n");
        printf("--mode : (intensity) mapping to be used \n");
        printf("--bottleHanlder             : the user select to send events only through bottle port esclusively  \n");
        printf("--verbose                   : saves relevant information in files \n");
        printf("--plotLatency               : saves relevant information about latency \n");
        printf("press CTRL-C to continue... \n");
        return true;
    }
    

    /* get the module name which will form the stem of all module port names */
    moduleName            = rf.check("name", 
                           Value("/eventFeatureExtractor"), 
                           "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";


    /*
    * set the operating mode which correspond as well with the file map saved in conf
    */
    mapName             = rf.check("mode", 
                                   Value("intensity"), 
                                   "file map name (string)").asString();
    mapName += ".txt";
    mapNameComplete = rf.findFile(mapName.c_str());

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

    efeThread = new efExtractorThread();
    efeThread->setMapURL(mapNameComplete);
    efeThread->setName(getName().c_str());

    /* 
     *checking whether the user wants exclusively to send events as bottles
     */
    if( rf.check("bottleHandler")) {
        printf("--------------------------->set the bottleHandler flag true \n");
        efeThread->setBottleHandler(true);
    }
    else {
        printf("--------------------------->set the bottleHandler flag false \n");
        efeThread->setBottleHandler(false);
    }
    
    if(rf.check("verbose")) {
        efeThread->setVERBOSE(true);
    }
    else {
        efeThread->setVERBOSE(false);
    }

    if(rf.check("plotLatency")) {
        efeThread->setPlotLatency(true);
    }
    else {
        efeThread->setPlotLatency(false);
    }

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

    return true ;       // let the RFModule know everything went well
                        // so that it will then run the module
}