Esempio n. 1
0
bool swTeleop::SWIcubArm::init( yarp::os::ResourceFinder &oRf, bool bLeftArm)
{

    if(m_bInitialized)
    {
        std::cerr << "Icub Arm is already initialized. " << std::endl;
        return true;
    }

    if(bLeftArm)
    {
        m_sArm = "left";
    }
    else
    {
        m_sArm = "right";
    }

    // gets the module name which will form the stem of all module port names
        m_sModuleName   = oRf.check("name", yarp::os::Value("teleoperation_iCub"), "Teleoperation/iCub Module name (string)").asString();
        m_sRobotName    = oRf.check("robot",yarp::os::Value("icubSim"),  "Robot name (string)").asString();

    // robot parts to control
        m_bArmHandActivated = oRf.check(std::string(m_sArm + "ArmHandActivated").c_str(), yarp::os::Value(m_bArmHandActivatedDefault),     std::string(m_sArm + " Arm/hand activated (int)").c_str()).asInt() != 0;
        m_bFingersActivated = oRf.check(std::string(m_sArm + "FingersActivated").c_str(), yarp::os::Value(m_bFingersActivatedDefault), std::string(m_sArm + " Fingers activated (int)").c_str()).asInt() != 0;

        m_i32RateVelocityControl = oRf.check("armsRateVelocityControl", yarp::os::Value(m_i32RateVelocityControlDefault), "Arms rate velocity control (int)").asInt();

        if(!m_bArmHandActivated && !m_bFingersActivated)
        {
            std::cout << m_sArm + " arm/hand and " + m_sArm  +" fingers not activated, icub " + m_sArm + " arm initialization aborted. " << std::endl;
            return (m_bInitialized = false);
        }

    // min / max values for iCub Torso joints
        for(uint ii = 0; ii < m_vArmJointVelocityAcceleration.size(); ++ii)
        {
            std::ostringstream l_os;
            l_os << ii;

            std::string l_sMinJoint(m_sArm + "ArmMinValueJoint" + l_os.str());
            std::string l_sMaxJoint(m_sArm + "ArmMaxValueJoint" + l_os.str());
            std::string l_sArmResetPosition(m_sArm + "ArmResetPosition" + l_os.str());
            std::string l_sArmJointVelocityAcceleration(m_sArm + "ArmJointVelocityAcceleration" + l_os.str());
            std::string l_sArmJointVelocityK(m_sArm + "ArmJointVelocityK" + l_os.str());
            std::string l_sArmJointPositionAcceleration(m_sArm + "ArmJointPositionAcceleration" + l_os.str());
            std::string l_sArmJointPositionSpeed(m_sArm + "ArmJointPositionSpeed" + l_os.str());

            std::string l_sMinJointInfo(m_sArm + " arm minimum joint" + l_os.str() + " Value (double)");
            std::string l_sMaxJointInfo(m_sArm + " arm maximum joint" + l_os.str() + " Value (double)");
            std::string l_sArmResetPositionInfo(m_sArm + " arm reset position " + l_os.str() + " Value (double)");
            std::string l_sArmJointVelocityAccelerationInfo(m_sArm + " arm joint velocity acceleration " + l_os.str() + " Value (double)");
            std::string l_sArmJointVelocityKInfo(m_sArm + " arm joint velocity K coeff"+ l_os.str() + " Value (double)");
            std::string l_sArmJointPositionAccelerationInfo(m_sArm + " arm joint position acceleration " + l_os.str() + " Value (double)");
            std::string l_sArmJointPositionSpeedInfo(m_sArm + " arm joint position speed " + l_os.str() + " Value (double)");

            m_vArmMinJoint[ii] = oRf.check(l_sMinJoint.c_str(), m_vArmMinJointDefault[ii], l_sMinJointInfo.c_str()).asDouble();
            m_vArmMaxJoint[ii] = oRf.check(l_sMaxJoint.c_str(), m_vArmMaxJointDefault[ii], l_sMaxJointInfo.c_str()).asDouble();
            m_vArmResetPosition[ii] = oRf.check(l_sArmResetPosition.c_str(), m_vArmResetPositionDefault[ii], l_sArmResetPositionInfo.c_str()).asDouble();
            m_vArmJointVelocityAcceleration[ii]= oRf.check(l_sArmJointVelocityAcceleration.c_str(), m_vArmJointVelocityAccelerationDefault[ii], l_sArmJointVelocityAccelerationInfo.c_str()).asDouble();
            m_vArmJointPositionAcceleration[ii]= oRf.check(l_sArmJointPositionAcceleration.c_str(), m_vArmJointPositionAccelerationDefault[ii], l_sArmJointPositionAccelerationInfo.c_str()).asDouble();
            m_vArmJointPositionSpeed[ii]       = oRf.check(l_sArmJointPositionSpeed.c_str(),        m_vArmJointPositionSpeedDefault[ii],        l_sArmJointPositionSpeedInfo.c_str()).asDouble();
            m_vArmJointVelocityK[ii]           = oRf.check(l_sArmJointVelocityK.c_str(),            m_vArmJointVelocityKDefault[ii],            l_sArmJointVelocityKInfo.c_str()).asDouble();
        }

    // miscellaneous
        m_i32TimeoutArmReset   = oRf.check(std::string(m_sArm + "ArmTimeoutReset").c_str(),       yarp::os::Value(m_i32TimeoutArmResetDefault), std::string(m_sArm + " arm timeout reset iCub (int)").c_str()).asInt();

    // set polydriver options
        m_oArmOptions.put("robot",     m_sRobotName.c_str());
        m_oArmOptions.put("device",    "remote_controlboard");
        m_oArmOptions.put("local",    ("/local/" + m_sRobotName + "/" + m_sArm + "_arm").c_str());
        m_oArmOptions.put("name",     ("/" + m_sRobotName + "/" + m_sArm + "_arm").c_str());
        m_oArmOptions.put("remote",   ("/" + m_sRobotName + "/" + m_sArm + "_arm").c_str());

    // init polydriver
        m_oRobotArm.open(m_oArmOptions);
        if(!m_oRobotArm.isValid())
        {
            std::cerr << std::endl <<"-ERROR: " << m_sArm << " robotArm is not valid, escape arm initialization. " << std::endl <<std::endl;
            return (m_bInitialized=false);
        }

    // initializing controllers
        if (!m_oRobotArm.view(m_pIArmVelocity) || !m_oRobotArm.view(m_pIArmPosition) || !m_oRobotArm.view(m_pIArmEncoders))
        {
            std::cerr << std::endl <<  "-ERROR: " << m_sArm << " while getting required robot Arm interfaces." << std::endl <<std::endl;
            m_oRobotArm.close();
            return (m_bInitialized=false);
        }


    // init ports
        m_sHandTrackerPortName         = "/teleoperation/" + m_sRobotName + "/" + m_sArm + "_arm/hand";
        m_sHandFingersTrackerPortName  = "/teleoperation/" + m_sRobotName + "/" + m_sArm + "_arm/hand_fingers";

    // open ports
        bool l_bPortOpeningSuccess = true;
        if(m_bArmHandActivated || m_bFingersActivated)
        {            
            l_bPortOpeningSuccess = m_oHandFingersTrackerPort.open(m_sHandFingersTrackerPortName.c_str());

            if(l_bPortOpeningSuccess)
                 l_bPortOpeningSuccess = m_oHandTrackerPort.open(m_sHandTrackerPortName.c_str());
        }

        if(!l_bPortOpeningSuccess)
        {
            std::cerr << std::endl <<"-ERROR: Unable to open ports." << std::endl <<std::endl;
            m_oRobotArm.close();
            return (m_bInitialized=false);
        }

    // retrieve Torso number of joints
        m_pIArmPosition->getAxes(&m_i32ArmJointsNb);

    // set accelerations/speeds
        for(int ii = 0; ii < m_i32ArmJointsNb; ++ii)
        {
            m_pIArmPosition->setRefAcceleration(ii, m_vArmJointPositionAcceleration[ii]);
            m_pIArmPosition->setRefSpeed(ii, m_vArmJointPositionSpeed[ii]);
            m_pIArmVelocity->setRefAcceleration(ii, m_vArmJointVelocityAcceleration[ii]);
        }

    // init controller
        m_pVelocityController = new swTeleop::SWArmVelocityController(m_pIArmEncoders, m_pIArmVelocity, m_vArmJointVelocityK, m_i32RateVelocityControl);
        m_pVelocityController->enable(m_bArmHandActivated, m_bFingersActivated);

        // display parameters
            std::cout << std::endl << std::endl;
            displayDebug(m_sArm + std::string(" arm/hand activated"), m_bArmHandActivated);
            displayDebug(m_sArm + std::string(" fingers activated"), m_bFingersActivated);
            displayDebug(std::string("Gaze activated"), m_i32TimeoutArmReset);
            displayDebug(std::string("Rate velocity control"), m_i32RateVelocityControl);
            std::cout << std::endl;
            displayVectorDebug(m_sArm + std::string(" arm min joint                  : "), m_vArmMinJoint);
            displayVectorDebug(m_sArm + std::string(" arm max joint                  : "), m_vArmMaxJoint);
            displayVectorDebug(m_sArm + std::string(" arm reset position joint       : "), m_vArmResetPosition);
            displayVectorDebug(m_sArm + std::string(" arm joint velocity acceleration: "), m_vArmJointVelocityAcceleration);
            displayVectorDebug(m_sArm + std::string(" arm joint position acceleration: "), m_vArmJointPositionAcceleration);
            displayVectorDebug(m_sArm + std::string(" arm joint position speed       : "), m_vArmJointPositionSpeed);
            displayVectorDebug(m_sArm + std::string(" arm joint velocity             : "), m_vArmJointVelocityK);
            std::cout << std::endl << std::endl;

    return (m_bIsRunning=m_bInitialized=true);
}
Esempio n. 2
0
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
}
Esempio n. 3
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. 4
0
bool tutorialModule::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("/tutorial"), 
                           "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();
    }


    /* create the thread and pass pointers to the module parameters */
    rThread = new tutorialRatethread(robotName, configFile);
    rThread->setName(getName().c_str());
    //rThread->setInputPortName(inputPortName.c_str());
    
    /* now start the thread to do the work */
    rThread->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   SimToolLoaderModule::configure(yarp::os::ResourceFinder &rf) {

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

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

    setName(moduleName.c_str());

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    return true;
}
Esempio n. 6
0
bool skinManager::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_DEFAULT.c_str()), "module name (string)").asString();
    robotName			= rf.check("robot", Value(ROBOT_NAME_DEFAULT.c_str()), "name of the robot (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 some other values from the configuration file */
    int period				= (int)rf.check("period", Value(PERIOD_DEFAULT), 
       "Period of the thread in ms (positive int)").asInt();
    float minBaseline		= (float)rf.check("minBaseline", Value(MIN_BASELINE_DEFAULT), 
       "If the baseline reaches this value then, if allowed, a calibration is executed (float in [0,255])").asDouble();
    float compGain			= (float)rf.check("compensationGain", Value(COMPENSATION_GAIN_DEFAULT), 
       "Gain of the compensation algorithm (float)").asDouble();
    float contCompGain		= (float)rf.check("contactCompensationGain", Value(CONTACT_COMPENSATION_GAIN_DEFAULT), 
       "Gain of the compensation algorithm during contact (float)").asDouble();
    int addThreshold		= (int)rf.check("addThreshold", Value(ADD_THRESHOLD_DEFAULT), 
       "Value added to all the touch thresholds (positive int)").asInt();
    
    bool zeroUpRawData = rf.check("zeroUpRawData", ZERO_UP_RAW_DATA_DEFAULT, 
        "if true the raw data are considered from zero up, otherwise from 255 down (bool)").asBool();
    bool smoothFilter = rf.check("smoothFilter", SMOOTH_FILTER_DEFAULT,
            "if true then the smoothing filter is active (bool)").asBool();
    float smoothFactor		= (float) rf.check("smoothFactor", Value(SMOOTH_FACTOR_DEFAULT), 
       "Determine the smoothing intensity (float in [0,1])").asDouble();
    bool binarization = rf.check("binarization", BINARIZATION_DEFAULT,
            "if true then the binarization is active (bool)").asBool();

    /*
    * 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
    */
    string handlerPortName = "/";
    handlerPortName += getName(rf.check("handlerPort", Value(RPC_PORT_DEFAULT.c_str())).asString());
    if (!handlerPort.open(handlerPortName.c_str())) {
	    cout << getName() << ": Unable to open port " << handlerPortName << endl;  
	    return false;
    }
    attach(handlerPort);                  // attach to port	
    handlerPort.setRpcMode(true);


    /* create the thread and pass pointers to the module parameters */
    myThread = new CompensationThread(moduleName, &rf, robotName, compGain, contCompGain, addThreshold, minBaseline, 
	    zeroUpRawData, period, binarization, smoothFilter, smoothFactor);
    /* now start the thread to do the work */
    return myThread->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. 7
0
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
}
Esempio n. 8
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");
    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"));
            // 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 ICubClient(moduleName, "behaviorManager","client.ini",isRFVerbose);

    if (!iCub->connect())
    {
        yInfo() << "iCubClient : Some dependencies are not running...";
        Time::delay(1.0);
    }

    while (!Network::connect("/ears/behavior:o", rpc_in_port.getName())) {
        yWarning() << "Ears is not reachable";
        yarp::os::Time::delay(0.5);
    }

    // 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. 9
0
bool EndogenousSalience::configure(yarp::os::ResourceFinder &rf)
{    
   /*
    * Process all parameters from 
    *  - command-line 
    *  - endogenousSalience.ini file (or whatever file is specified by the --from argument)
    */

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

   moduleName            = rf.check("name", 
                           Value("endogenousSalience"), 
                           "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 name of the input and output ports, automatically prefixing the module name by using getName() */

   cartesianInputPortName     = "/";
   cartesianInputPortName    += getName(
                                rf.check("cartesianImageInPort", 
                                Value("/cartesianImage:i"),
                                "Input image port (string)").asString()
                                );
   
   logpolarInputPortName      = "/";
   logpolarInputPortName     += getName(
                                rf.check("logpolarImageInPort", 
                                Value("/logpolarImage:i"),
                                "Exemplar image port (string)").asString()
                                );

   salienceOutputPortName     = "/";
   salienceOutputPortName    += getName(
                                rf.check("salienceImageOutPort", 
                                Value("/salienceImage:o"),
                                "Left output image port (string)").asString()
                                );

   hueTolerance               = rf.check("hueTolerance",
                                Value(10),
                                "Tolerance for hue value (int)").asInt();

   saturationTolerance        = rf.check("saturationTolerance",
                                Value(10),
                                "Tolerance for saturation value (int)").asInt();

   hueBins                    = rf.check("hueBins",
                                Value(32),
                                "Number of hue bins in HS histogram (int)").asInt();

   saturationBins             = rf.check("saturationBins",
                                Value(32),
                                "Number of saturation bins in HS histogram (int)").asInt();

   filterRadius               = rf.check("filterRadius",
                                Value(10),
                                "Radius of the morphological opening filter (int)").asInt();

   if (debug) {
      printf("endogenousSalience: module name is                    %s\n",moduleName.c_str());
      printf("endogenousSalience: robot name is                     %s\n",robotName.c_str());
      printf("endogenousSalience: hue and saturation tolerances are %d %d\n",hueTolerance,saturationTolerance);
      printf("endogenousSalience: hue and saturation bins are       %d %d\n",hueBins,saturationBins);
      printf("endogenousSalience: port names are\n%s\n%s\n%s\n\n",cartesianInputPortName.c_str(),
                                                                  logpolarInputPortName.c_str(),
                                                                  salienceOutputPortName.c_str());
   }
    
   /* do all initialization here */
     
   /* open ports  */ 
       
   if (!cartesianImageIn.open(cartesianInputPortName.c_str())) {
      cout << getName() << ": unable to open port " << cartesianInputPortName << endl;
      return false;  // unable to open; let RFModule know so that it won't run
   }

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

   if (!salienceImageOut.open(salienceOutputPortName.c_str())) {
      cout << getName() << ": unable to open port " << salienceOutputPortName << 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 */

   endogenousSalienceThread = new EndogenousSalienceThread(&cartesianImageIn, 
                                                           &logpolarImageIn, 
                                                           &salienceImageOut, 
                                                           &hueTolerance, 
                                                           &saturationTolerance, 
                                                           &hueBins, 
                                                           &saturationBins, 
                                                           &filterRadius);

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

   endogenousSalienceThread->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. 10
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
}
Esempio n. 11
0
bool fileFeederModule::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("/fileFeeder"), 
                           "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

    ffThread=new fileFeederThread();
    ffThread->setName(getName().c_str());
    ffThread->start();

    /* get the filename start which will form the name of the read images */
    filenameStart          = rf.check("filenameStart", 
                           Value("."), 
                           "module name (string)").asString();
    ffThread->setStartFilename(filenameStart);

    /* get the filename end which will form the name of the read images */
    filenameEnd            = rf.check("filenameEnd", 
                           Value(").jpg"), 
                           "module name (string)").asString();
    ffThread->setLastFilename(filenameEnd);

    /* get the number of images that are going to be read */
    numberImages           = rf.check("numberImages", 
                           Value(100), 
                           "module name (int)").asInt();
    ffThread->setNumberOfImages(numberImages);
    ffThread->setStartFilename(filenameStart);

    return true ;       // let the RFModule know everything went well
                        // so that it will then run the module
}
Esempio n. 12
0
bool TrainModule::configure(yarp::os::ResourceFinder& opt) {
    /* Implementation note:
     * Calling open() in the base class (i.e. PredictModule) is cumbersome due
     * to different ordering and dynamic binding (e.g. it calls
     * registerAllPorts()) and because we do bother with an incoming model port.
     */

    // read for the general specifiers:
    yarp::os::Value* val;
    std::string machineName;

    // cache resource finder
    this->setResourceFinder(&opt);

    // check for help request
    if(opt.check("help")) {
        this->printOptions();
        return false;
    }

    // check for algorithm listing request
    if(opt.check("list")) {
        this->printMachineList();
        return false;
    }

    // check for port specifier: portSuffix
    if(opt.check("port", val)) {
        this->portPrefix = val->asString().c_str();
    }

    // check for filename to load machine from
    if(opt.check("load", val)) {
        this->getMachinePortable().readFromFile(val->asString().c_str());
    } else{
        // not loading anything, require machine name
        if(opt.check("machine", val)) {
            machineName = val->asString().c_str();
        } else {
            this->printOptions("No machine type specified");
            return false;
        }

        // construct new machine
        this->getMachinePortable().setWrapped(machineName);

        // send configuration options to the machine
        this->getMachine().configure(opt);
    }


    // add replier for incoming data (prediction requests)
    this->predict_inout.setReplier(this->predictProcessor);

    // add processor for incoming data (training samples)
    this->train_in.useCallback(trainProcessor);

    // register ports before connecting
    this->registerAllPorts();

    // and finally load command file
    if(opt.check("commands", val)) {
        this->loadCommandFile(val->asString().c_str());
    }

    // attach to the incoming command port and terminal
    this->attach(cmd_in);
    this->attachTerminal();

    return true;
}
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
}
Esempio n. 14
0
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
}
Esempio n. 15
0
bool learnPrimitive::configure(yarp::os::ResourceFinder &rf)
{
    string moduleName = rf.check("name", Value("learnPrimitive")).asString().c_str();
    setName(moduleName.c_str());

    GrammarYesNo           = rf.findFileByName(rf.check("GrammarYesNo", Value("nodeYesNo.xml")).toString());
    GrammarNameAction      = rf.findFileByName(rf.check("GrammarNameAction", Value("GrammarNameAction.xml")).toString());
    GrammarTypeAction      = rf.findFileByName(rf.check("GrammarTypeAction", Value("GrammarTypeAction.xml")).toString());

    yInfo() << "findFileByName " << rf.findFileByName("learnPrimitive.ini") ;
    pathToIniFile = rf.findFileByName("learnPrimitive.ini") ;

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

    //bool    bEveryThingisGood = true;

    //Create an iCub Client and check that all dependencies are here before starting
    bool isRFVerbose = true;
    iCub = new ICubClient(moduleName, "learnPrimitive", "client.ini", isRFVerbose);
    iCub->opc->isVerbose &= true;

    string robot = rf.check("robot", Value("icubSim")).asString().c_str();
    string arm   = rf.check("arm", Value("left_arm")).asString().c_str();

    portToArm.open(("/" + moduleName + "/toArm:rpc").c_str());
    string portRobotArmName = "/" + robot + "/" + arm + "/rpc:i";

    yInfo() << "================> port controlling the arm : " << portRobotArmName;
    if (!Network::connect(portToArm.getName().c_str(),portRobotArmName))
    {
        yWarning() << "WARNING PORT TO CONTROL ARM (" << portRobotArmName << ") IS NOT CONNECTED";
    }


    if (!iCub->connect())
    {
        cout << "iCubClient : Some dependencies are not running..." << endl;
        Time::delay(1.0);
    }

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

    if (!iCub->getRecogClient())
    {
        yWarning() << "WARNING SPEECH RECOGNIZER NOT CONNECTED";
    }
    if (!iCub->getABMClient())
    {
       yWarning() << "WARNING ABM NOT CONNECTED";
    }

    updateProtoAction(rf);
    updatePrimitive(rf);
    updateAction(rf);

    yInfo() << "\n \n" << "----------------------------------------------" << "\n \n" << moduleName << " ready ! \n \n ";

    iCub->say("learn Primitive is ready", false);



    return true;
}
Esempio n. 16
0
bool lfCollectorModule::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("/logpolarFrameCollector"), 
                           "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
    

    // --------------------------------------------
    lfThread=new lfCollectorThread();
    lfThread->setName(getName().c_str());
    
    /*
    * set the period between two successive synchronisations between viewer and events
    */
    synchPeriod            = rf.check("synchPeriod", 
                           Value(10000), 
                           "synchronisation period (int)").asInt();
    lfThread->setSynchPeriod(synchPeriod);

    /*
    * set the retinaSize (considering squared retina)
    */
    retinalSize            = rf.check("retinalSize", 
                           Value(128), 
                           "retinalSize (int)").asInt();
    lfThread->setRetinalSize(retinalSize);

    
    /* checking whether the module synchronizes with single camera or stereo camera
     */
    if( rf.check("stereo")) {
        lfThread->setStereo(true);
    }
    else {
        lfThread->setStereo(false);
    }

    /* checking whether the module synchronizes with single camera or stereo camera
     */
    if( rf.check("em")) {
        lfThread->setEM(true);
    }
    else {
        lfThread->setEM(false);
    }
    
    /**
     * checking whether the viewer represent log-polar information
     */
    if( rf.check("logpolar")) {
        lfThread->setLogPolar(true);
    }
    else {
        lfThread->setLogPolar(false);
    }
    lfThread->start();

    return true ;       // let the RFModule know everything went well
                        // so that it will then run the module
}
Esempio n. 17
0
bool cfCollectorModule::configure(yarp::os::ResourceFinder &rf) {
    /* Process all parameters from both command-line and .ini file */
    if(rf.check("help")) {
        printf("HELP \n");
        printf("=======");
        printf("--name             (string) : specifies the rootname\n");
        printf("--robot            (string) : indicates the name of the robot to connect to\n");
        printf("--retinalSize      (int)    : defines the dimension of the retina (input)\n");
        printf("--responseGradient (int)    : the increment for any single event in the register\n");
        printf("--sychPeriod       (int)    : period for synchronization of the variable lastTimestamp\n");
        printf("--windowSize       (int)    : size of the window where events are collected \n ");
        printf("--stereo                    : if present both left and right events are represented \n ");
        printf("--bottleHanlder             : the user select to send events only through bottle port esclusively  \n");
        printf("--verbose                   : enable debug savings of events in files");
        printf("\n press CTRL-C to continue \n");
        return true;
    }



    printf("initialization of the main thread \n");
    /* get the module name which will form the stem of all module port names */
    moduleName            = rf.check("name", 
                           Value("/cartesianFrameCollector"), 
                           "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;
    }
    printf("attaching handler port2 \n");
    attach(handlerPort);                  // attach to port
    
    // --------------------------------------------
    printf("starting cfCollector Thread \n");
    cfThread=new cfCollectorThread();
    cfThread->setName(getName().c_str());
    
    printf("name of the cfThread correctly set \n");

    /*
    * set the period between two successive synchronisations between viewer and events
    */
    synchPeriod            = rf.check("synchPeriod", 
                           Value(10000), 
                           "synchronisation period (int)").asInt();
    cfThread->setSynchPeriod(synchPeriod);

    /*
    * set the windowSize (dimension of temporal window collected events)
    */
    windowSize            = rf.check("windowSize", 
                           Value(10), 
                           "windowSize (int)").asInt();
    printf("looking for the windowSize; found value %d \n", windowSize);
    cfThread->setWindowSize(windowSize);

    
    /*
    * set the retinaSize (considering squared retina)
    */
    printf("looking for the retinalSize \n");
    retinalSize            = rf.check("retinalSize", 
                           Value(128), 
                           "retinalSize (int)").asInt();
    cfThread->setRetinalSize(retinalSize);


    
    /*
    * set the retinaSize (considering squared retina)
    */
    printf("looking for the responseGradient \n");
    responseGradient       = rf.check("responseGradient", 
                           Value(127), 
                           "responseGradient (int)").asInt();
    cfThread->setResponseGradient(responseGradient);

    
    /* 
     *checking whether the module synchronizes with single camera or stereo camera
     */
    if( rf.check("stereo")) {
        cfThread->setStereo(true);
    }
    else {
        cfThread->setStereo(false);
    }

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

    /* 
     *checking whether the user wants exclusively to send events as bottles
     */
    if( rf.check("verbose")) {
        printf("set the verbose mode for all the components \n");
        cfThread->setVerbose(true);
    }
    else {
        printf("verbose mode deactivated \n");
        cfThread->setVerbose(false);
    }

    /* 
     *set option for mapping three states into 3baseline graylevels
     */
    if( rf.check("tristate")) {
        cfThread->setTristate(true);
    }
    else {
        cfThread->setTristate(false);
    }


    /* 
     *checking whether the module synchronizes with single camera or stereo camera
     */
    printf("Looking for asvMode ... \n");
    if( rf.check("asvMode")) {
        printf("setting asvMode = true, dvsMode = false \n");
        cfThread->setASVMode(true);
        cfThread->setDVSMode(false);
    }
    else {
        printf("setting asvMode = false, dvsMode = false \n");
        cfThread->setASVMode(false);
    }

    
    /* checking whether the module synchronizes with single camera or stereo camera
     */
    if( rf.check("dvsMode")) {
        cfThread->setDVSMode(true);
        cfThread->setASVMode(false);
    }
    else {
        cfThread->setDVSMode(false);
    }

    /**
     * checking whether the viewer represent log-polar information
     */
    if( rf.check("logpolar")) {
        cfThread->setLogPolar(true);
    }
    else {
        cfThread->setLogPolar(false);
    }
    cfThread->start();

    return true ;       // let the RFModule know everything went well
                        // so that it will then run the module
}
Esempio n. 18
0
bool rd::RobotDevastation::configure(yarp::os::ResourceFinder &rf)
{
    //-- Show help
    //printf("--------------------------------------------------------------\n");
    if (rf.check("help")) {
        printf("RobotDevastation options:\n");
        printf("\t--help (this help)\t--from [file.ini]\t--context [path]\n");
        printf("\t--id integer\n");
        printf("\t--name string\n");
        printf("\t--team integer\n");
        printf("\t--robotName string\n");
        // Do not exit: let last layer exit so we get help from the complete chain.
    }
    printf("RobotDevastation using no additional special options.\n");

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    ConstString str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
    setName(str.c_str()); // modulePortName

    double maxDelay = rf.check("maxDelay", Value(0.010), "Max delay between image and encoders").asDouble();

    // pass configuration over to bottle
    Bottle botConfig(rf.toString().c_str());
    botConfig.setMonitor(rf.getMonitor());
    // Load from configuration group ([<group_name>]), if group option present
    Value *valGroup; // check assigns pointer to reference
    if(botConfig.check("group", valGroup, "Configuration group to load module options from (string).")) {
        strGroup = valGroup->asString().c_str();
        // is group a valid bottle?
        if (botConfig.check(strGroup.c_str())){
            Bottle &group=botConfig.findGroup(strGroup.c_str(),string("Loading configuration from group " + strGroup).c_str());
            botConfig.fromString(group.toString());
        } else {
            yError() << "Group " << strGroup << " not found.";
            return false;
        }
    } else {
        yError ("There seem to be an error loading parameters (group section missing), stopping module");
        return false;
    }

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

    _calibTool = CalibToolFactories::getPool().get(calibToolName.c_str());
    if (_calibTool!=NULL) {
        bool ok = _calibTool->open(botConfig);
        if (!ok) {
            delete _calibTool;
            _calibTool = NULL;
            return false;
        }
    }

    if (yarp::os::Network::exists(getName("/in"))) {
        yWarning() << "port " << getName("/in") << " already in use";
    }
    if (yarp::os::Network::exists(getName("/out"))) {
        yWarning() << "port " << getName("/out") << " already in use";
    }
    if (yarp::os::Network::exists(getName("/conf"))) {
        yWarning() << "port " << getName("/conf") << " already in use";
    }
    _prtImgIn.setSaturation(rf.check("saturation",Value(1.0)).asDouble());
    _prtImgIn.open(getName("/in"));
    _prtImgIn.setPointers(&_prtImgOut,_calibTool);
    _prtImgIn.setVerbose(rf.check("verbose"));
    _prtImgIn.setLeftEye((strGroup == "CAMERA_CALIBRATION_LEFT") ? true : false);
    _prtImgIn.setMaxDelay(maxDelay);
    _prtImgIn.setUseIMU(rf.check("useIMU"));
    _prtImgIn.setUseIMU(rf.check("useTorso"));
    _prtImgIn.setUseIMU(rf.check("useEyes"));
    _prtImgIn.useCallback();
    _prtImgOut.open(getName("/out"));
    _configPort.open(getName("/conf"));

    _prtTEncsIn.open(getName("/torso_encs/in"));
    _prtTEncsIn._prtImgIn = &_prtImgIn;
//    _prtTEncsIn.setStrict();
    _prtTEncsIn.useCallback();

    _prtHEncsIn.open(getName("/head_encs/in"));
    _prtHEncsIn._prtImgIn = &_prtImgIn;
//    _prtHEncsIn.setStrict();
    _prtHEncsIn.useCallback();

    _prtImuIn.open(getName("/imu/in"));
    _prtImuIn._prtImgIn = &_prtImgIn;
//    _prtImuIn.setStrict();
    _prtImuIn.useCallback();

    attach(_configPort);
    fflush(stdout);

    _prtImgIn.rpyPort.open(getName("/rpy"));

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

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

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

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

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

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

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

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

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

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

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

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

    if (yarp::os::Network::exists(getName("/out")))
    {
        yWarning() << "port " << getName("/out") << " already in use";
    }
    if (yarp::os::Network::exists(getName("/conf")))
    {
        yWarning() << "port " << getName("/conf") << " already in use";
    }
    imageOut.open(getName("/out:o"));
    configPort.open(getName("/conf"));
    attach(configPort);
    return true;
}
Esempio n. 21
0
bool objectGeneratorSim::configure(yarp::os::ResourceFinder &rf)
{

    Matrix iH;
    iH = zeros(4,4);
    iH(0,1)=-1;
    iH(1,2)=1;
    iH(1,3)=0.5976;
    iH(2,0)=-1;
    iH(2,3)=-0.026;
    iH(3,3)=1;
    H=SE3inv(iH);
    elapsedCycles = 0;
    listSize[0]=0;
    listSize[1]=0;
    listSize[2]=0;
    string moduleName = rf.check("name", Value("objectGeneratorSim")).asString().c_str();
    //setName(moduleName.c_str());

    string moduleOutput = rf.check("output", Value("/obstacles:o")).asString().c_str();
    string moduleOutputTarget = rf.check("output", Value("/reachingTarget:o")).asString().c_str();
    string moduleInput = "/pos:i";

    bool    bEveryThingisGood = true;

    string port2icubsim = "/" + moduleName + "/sim:o";
    if (!portSim.open(port2icubsim.c_str())) {
        cout << getName() << ": Unable to open port " << port2icubsim << endl;
        bEveryThingisGood = false;
    }    

    // create an output port
    std::string port2output = "/" + moduleName + moduleOutput;

    if (!portOutput.open(port2output.c_str())) {
        cout << getName() << ": Unable to open port " << port2output << endl;
        bEveryThingisGood = false;
    }
    std::string port2outputT = "/" + moduleName + moduleOutputTarget;

    if (!portOutputTarget.open(port2outputT.c_str())) {
        cout << getName() << ": Unable to open port " << port2outputT << endl;
        bEveryThingisGood = false;
    }

    std::string port2input = "/" + moduleName + moduleInput;

    if (!portInput.open(port2input.c_str())) {
        cout << getName() << ": Unable to open port " << port2input << endl;
        bEveryThingisGood = false;
    }
    std::string port2world = "/icubSim/world";
    while (!Network::connect(port2icubsim, port2world.c_str()))
    {
        std::cout << "Trying to get input from "<< port2icubsim << "..." << std::endl;
        yarp::os::Time::delay(1.0);
    }

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

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

    Bottle cmd;
    cmd.clear();
    cmd.addString("world");
    cmd.addString("del");
    cmd.addString("all");

    portSim.write(cmd);
    yarp::os::Time::delay(1);   
    spamTable();
    yarp::os::Time::delay(3);
    
    yarp::os::Bottle pos1;

    pos1.clear();
    pos1.addDouble(-0.01);
    pos1.addDouble(0.61);
    pos1.addDouble(0.36);
    objectGeneratorSim::createObject(pos1, true);
    yarp::os::Time::delay(3);

    pos1.clear();
    pos1.addDouble(-0.07);
    pos1.addDouble(0.61);
    pos1.addDouble(0.29);
    objectGeneratorSim::createObject(pos1);
    yarp::os::Time::delay(3);

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

    return bEveryThingisGood;
}
Esempio n. 22
0
bool LogPolarTransform::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("logpolarTransform"), 
                           "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 name of the input and output ports, automatically prefixing the module name by using getName() */

   inputPortName         = "/";
   inputPortName        += getName(
                           rf.check("imageInputPort", 
                           Value("/image:i"),
                           "Input image port (string)").asString()
                           );
   
   outputPortName        = "/";
   outputPortName       += getName(
                           rf.check("imageOutputPort", 
                           Value("/image:o"),
                           "Output image port (string)").asString()
                           );

   /* get the direction of the transform */

   transformDirection    = rf.check("direction",
                           Value("CARTESIAN2LOGPOLAR"),
                           "Key value (int)").asString();

   cout << "Configuration of logpolar " << transformDirection << endl;

   if (transformDirection == "CARTESIAN2LOGPOLAR") {
      direction = CARTESIAN2LOGPOLAR;
   }
   else {
      direction = LOGPOLAR2CARTESIAN;
   }

   /* get the number of angles */

   numberOfAngles        = rf.check("angles",
                           Value(252),
                           "Key value (int)").asInt();

   /* get the number of rings */

   numberOfRings         = rf.check("rings",
                           Value(152),
                           "Key value (int)").asInt();
 
   /* get the size of the X dimension */

   xSize                 = rf.check("xsize",
                           Value(320),
                           "Key value (int)").asInt();

   /* get the size of the Y dimension */

   ySize                 = rf.check("ysize",
                           Value(240),
                           "Key value (int)").asInt();


   /* get the overlap ratio */

   overlap               = rf.check("overlap",
                           Value(1.0),
                           "Key value (int)").asDouble();


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

   if (!imageOut.open(outputPortName.c_str())) {
      cout << getName() << ": unable to open port " << outputPortName << 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
 
   //attachTerminal();                     // attach to terminal


   /* create the thread and pass pointers to the module parameters */

   logPolarTransformThread = new LogPolarTransformThread(&imageIn, &imageOut, 
                                                         &direction, 
                                                         &xSize, &ySize,
                                                         &numberOfAngles, &numberOfRings, 
                                                         &overlap);

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

   logPolarTransformThread->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 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;
    }
}
Esempio n. 24
0
bool SalienceModule::configure(yarp::os::ResourceFinder &rf){
    
	ConstString str = rf.check("name", Value("/salience"), "module name (string)").asString();
	setName(str.c_str()); // modulePortName  
	attachTerminal();

	// framerate
	_intFPS = rf.check("fps", Value(20), "Try to achieve this number of frames per second (int).").asInt();
	_intPrintFPSAfterNumFrames = rf.check("fpsOutputFrequency", Value(20), "Print the achieved framerate after this number of frames (int).").asInt();
	_dblTPF = 1.0f/((float)_intFPS);
	_intFPSAchieved = 0;
	_intFC = 0;
	_dblTPFAchieved = 0.0;
	_dblStartTime = 0.0;

    numBlurPasses = rf.check("numBlurPasses",
                                    Value(0),
                                    "Blur the output map numBlurPasses times with a gaussian 3x3 kernel (int).").asInt();
    drawSaliencePeak = rf.check("drawSaliencePeak",
                                Value(1),
                                "Draw a crosshair at salience peak onto the output visualization image (int [0|1]).").asInt()!=0;
	thresholdSalience = rf.check("thresholdSalience",
                                    Value(0.0),
                                    "Set salience map values < threshold to zero (double).").asDouble();
    activateIOR = rf.check("activateInhibitionOfReturn",
                           Value(0),
                           "Use IOR (int [0|1]).").asInt()!=0;
    if (activateIOR){
        ior.open(rf);
    }  

    filter = NULL;
    ConstString filterName = rf.check("filter",
                                          Value(""),
                                          "filter to use (string [group|intensity|color|directional|motion|emd|ruddy|face])").asString();

    if (filterName=="") {
        printf("*** Please specify a filter, e.g. --filter motion\n");
        vector<string> names = SalienceFactories::getPool().getNames();
        printf("*** Filters available: ");
        for (unsigned int i=0; i<names.size(); i++) {
            printf("%s ", names[i].c_str());
        }
        printf("\n");
        return false;
    }

    filter = SalienceFactories::getPool().get(filterName);
    if (filter!=NULL) {
        bool ok = filter->open(rf);
        if (!ok) {
            delete filter;
            filter = NULL;
            return false;
        }
    }

    imgPort.open(getName("/view"));
	peakPort.open(getName("/peak")); //For streaming saliency peak coordinates (Alex, 31/05/08)
    filteredPort.open(getName("/map"));
    configPort.open(getName("/conf"));
    attach(configPort);

    oldSizeX = -1;
    oldSizeY = -1;
    needInit = true;

	fflush(stdout);
    return true;
}
Esempio n. 25
0
MainWindow::MainWindow(yarp::os::ResourceFinder &rf, QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow),
    loadingWidget(this)
{
    ui->setupUi(this);
    setWindowTitle(APP_NAME);
    utilities = nullptr;
    pressed = false;
    initThread = nullptr;

    moduleName =  QString("%1").arg(rf.check("name", Value("yarpdataplayer"), "module name (string)").asString().c_str());

    if (rf.check("withExtraTimeCol")){
        withExtraTimeCol = true;
        column = rf.check("withExtraTimeCol",Value(1)).asInt32();

        if (column < 1 || column > 2 ){
            column = 1;
        }

        LOG( "Selected timestamp column to check is %d \n", column);

    } else {
        withExtraTimeCol = false;
        column = 0;
    }

    add_prefix = rf.check("add_prefix");
    createUtilities();

    subDirCnt = 0;
    utilities = nullptr;
    setWindowTitle(moduleName);
    setupActions();
    setupSignals();

    QString port = QString("/%1/rpc:i").arg(moduleName);
    rpcPort.open( port.toLatin1().data() );

    ::signal(SIGINT, sighandler);
    ::signal(SIGTERM, sighandler);

    attach(rpcPort);

    quitFromCmd = false;

    connect(ui->mainWidget,SIGNAL(itemDoubleClicked(QTreeWidgetItem*,int)),this,SLOT(onItemDoubleClicked(QTreeWidgetItem*,int)));
    connect(this,SIGNAL(internalLoad(QString)),this,SLOT(onInternalLoad(QString)),Qt::QueuedConnection);
    connect(this,SIGNAL(internalPlay()),this,SLOT(onInternalPlay()),Qt::BlockingQueuedConnection);
    connect(this,SIGNAL(internalPause()),this,SLOT(onInternalPause()),Qt::BlockingQueuedConnection);
    connect(this,SIGNAL(internalStop()),this,SLOT(onInternalStop()),Qt::BlockingQueuedConnection);
    connect(this,SIGNAL(internalStep(Bottle*)),this,SLOT(onInternalStep(Bottle*)),Qt::BlockingQueuedConnection);
    connect(this,SIGNAL(internalSetFrame(std::string,int)),this,SLOT(onInternalSetFrame(std::string,int)),Qt::BlockingQueuedConnection);
    connect(this,SIGNAL(internalGetFrame(std::string, int*)),this,SLOT(onInternalGetFrame(std::string,int*)),Qt::BlockingQueuedConnection);
    connect(this,SIGNAL(internalQuit()),this,SLOT(onInternalQuit()),Qt::QueuedConnection);

    QShortcut *openShortcut = new QShortcut(QKeySequence("Ctrl+O"), parent);
    QObject::connect(openShortcut, SIGNAL(activated()), this, SLOT(onInternalLoad(QString)));

    QShortcut *closeShortcut = new QShortcut(QKeySequence("Ctrl+Q"), parent);
    QObject::connect(closeShortcut, SIGNAL(activated()), this, SLOT(onInternalQuit()));

}
Esempio n. 26
0
bool GBSegmModule::configure (yarp::os::ResourceFinder &rf)
{
    if (rf.check("help","if present, display usage message")) {
        printf("Call with --from configfile.ini\n");
        return false;
    }
   
    if (rf.check("name"))
        setName(rf.find("name").asString().c_str());
    else setName("GBSeg");


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

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

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

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

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

    //Get conf parameters
    moduleName = rf.check("name",Value("frontalEyeField")).asString();
    setName(moduleName.c_str());
    tau = rf.check("tau", Value(2.0)).asDouble();
    retinaW = rf.check("retinaW", Value(3)).asInt();
    retinaH = rf.check("retinaH", Value(3)).asInt();
    string nameSourcePrefix = rf.check("nameSplitterPrefix", Value("/v1Retina/in_")).asString();
    string nameSourceSuffix = rf.check("nameSplitterSuffix", Value("/error:o")).asString();
	cameraUsed = (rf.check("camera", Value("left")).asString() == "right");

    //string nameSourcePrefix = rf.check("nameSplitterPrefix", Value("/retina/")).asString();
    //string nameSourceSuffix = rf.check("nameSplitterSuffix", Value("/retina/error:o")).asString();


	//----------------------------//
	//Configure the egocentric error cvz
	stringstream configEgocentricError;
	configEgocentricError
		<< "type" << '\t' << cvz::core::TYPE_MMCM << endl
		<< "name" << '\t' << "egocentricError" << endl
		<< "width" << '\t' << 10 << endl
		<< "height" << '\t' << 10 << endl
		<< "layers" << '\t' << 5 << endl
		<< "sigmaFactor" << '\t' << 0.75 << endl
		<< "learningRate" << '\t' << 0.01 << endl << endl;

	//Add the proprioception
	configEgocentricError
		<< "[modality_0]" << endl
		<< "name" << '\t' << "fixationPoint" << endl
		<< "type" << '\t' << "yarpVector" << endl
		<< "size" << '\t' << 3 << endl
		<< "minBounds" << '\t' << "(-2.0 -1.0 -1.0)" << endl
		<< "maxBounds" << '\t' << "(-1.0 1.0 1.0)" << endl
		<< "autoconnect" << '\t' << "/iKinGazeCtrl/x:o" << endl << endl;

	//Add the v1Retina
	configEgocentricError
		<< "[modality_1]" << endl
		<< "name" << '\t' << "v2Error" << endl
		<< "type" << '\t' << "yarpVector" << endl
		<< "size" << '\t' << 1 << endl
		//<< "isBlocking" << endl
		<< "autoconnect" << '\t' << "/v2/v1Retina/error:o"<<endl;

	Property prop;
	prop.fromConfig(configEgocentricError.str().c_str());
	mmcmErrorPrediction = new cvz::core::ThreadedCvz(prop, 100);
	mmcmErrorPrediction->start();
	//cvz::core::CvzBuilder::allocate(&mmcmErrorPrediction, cvz::core::TYPE_MMCM);
	//mmcmErrorPrediction->configure(prop);

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

    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);
    igaze->bindNeckPitch(-15.0, 10.0);
    igaze->bindNeckRoll(-10.0, 10.0);
    igaze->bindNeckYaw(-20.0, 20.0);
	igaze->setSaccadesStatus(false);
	igaze->blockEyes(0.0);

    //Open the retina input ports
    retinaInput.resize(retinaW);
    errorMap.resize(retinaW);
    for (int x = 0; x < retinaW; x++)
    {
        retinaInput[x].resize(retinaH);
        errorMap[x].resize(retinaH);
        for (int y = 0; y < retinaH; y++)
        {
            stringstream ss;
            ss << "/" << moduleName << "/error/" << x << "_" << y << ":i";
            retinaInput[x][y] = new BufferedPort<Bottle>();
            retinaInput[x][y]->open(ss.str().c_str());
        }
    }
    //Wait for the connection
    connectErrorInput(nameSourcePrefix, nameSourceSuffix);
	timeNextSaccade = Time::now() + tau;
    return true;
}
Esempio n. 28
0
bool PasarModule::configure(yarp::os::ResourceFinder &rf) {
    std::string opcName;
    std::string gazePortName;
    std::string handlerPortName;
    std::string saliencyPortName;

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


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

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

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

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

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

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

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

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

    opc->checkout();

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



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


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

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

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


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

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

    attach(handlerPort);                  // attach to port
    trackedObject = "";
    presentObjectsLastStep.clear();
    return true;
}
Esempio n. 29
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 ;
}
Esempio n. 30
0
bool swTeleop::SWIcubTorso::init( yarp::os::ResourceFinder &oRf)
{
	if(m_bInitialized)
	{
		std::cerr << "Icub Torso is already initialized. " << std::endl;
		return true;
	}

	// gets the module name which will form the stem of all module port names
        m_sModuleName   = oRf.check("name", Value("teleoperation_iCub"), "Teleoperation/iCub Module name (string)").asString();
        m_sRobotName    = oRf.check("robot",Value("icubSim"),  "Robot name (string)").asString();

        m_i32RateVelocityControl = oRf.check("torsoRateVelocityControl", Value(m_i32RateVelocityControlDefault), "Torso rate velocity control (int)").asInt();

	// robot parts to control
        m_bTorsoActivated = oRf.check("torsoActivated", Value(m_bTorsoActivatedDefault), "Torso activated (int)").asInt() != 0;
        if(!m_bTorsoActivated)
        {
            std::cout << "Torso not activated, icub torso initialization aborted. " << std::endl;
            return (m_bInitialized=false);
        }

	// min / max values for iCub Torso joints
        for(uint ii = 0; ii < m_vTorsoJointVelocityAcceleration.size(); ++ii)
        {
            std::ostringstream l_os;
            l_os << ii;

            std::string l_sMinJoint("torsoMinValueJoint" + l_os.str());
            std::string l_sMaxJoint("torsoMaxValueJoint" + l_os.str());
            std::string l_sTorsoJointVelocityAcceleration("torsoJointVelocityAcceleration" + l_os.str());
            std::string l_sTorsoJointVelocityK("torsoJointVelocityK" + l_os.str());
            std::string l_sTorsoJointPositionAcceleration("torsoJointPositionAcceleration" + l_os.str());
            std::string l_sTorsoJointPositionSpeed("torsoJointPositionSpeed" + l_os.str());
            std::string l_sTorsoResetPosition("torsoResetPosition" + l_os.str());

            std::string l_sMinJointInfo("torso minimum joint" + l_os.str() + " Value (double)");
            std::string l_sMaxJointInfo("torso maximum joint" + l_os.str() + " Value (double)");
            std::string l_sTorsoJointVelocityAccelerationInfo("torso joint velocity acceleration " + l_os.str() + " Value (double)");
            std::string l_sTorsoJointVelocityKInfo("torso joint velocity K coeff"+ l_os.str() + " Value (double)");
            std::string l_sTorsoJointPositionAccelerationInfo("torso joint position acceleration " + l_os.str() + " Value (double)");
            std::string l_sTorsoJointPositionSpeedInfo("torso joint position speed " + l_os.str() + " Value (double)");
            std::string l_sTorsoResetPositionInfo("torso reset position " + l_os.str() + " Value (double)");

            m_vTorsoMinJoint[ii]                    = oRf.check(l_sMinJoint.c_str(), m_vTorsoMinJointDefault[ii], l_sMinJointInfo.c_str()).asDouble();
            m_vTorsoMaxJoint[ii]                    = oRf.check(l_sMaxJoint.c_str(), m_vTorsoMaxJointDefault[ii], l_sMaxJointInfo.c_str()).asDouble();
            m_vTorsoResetPosition[ii]               = oRf.check(l_sTorsoResetPosition.c_str(), m_vTorsoResetPositionDefault[ii], l_sTorsoResetPositionInfo.c_str()).asDouble();
            m_vTorsoJointVelocityAcceleration[ii]   = oRf.check(l_sTorsoJointVelocityAcceleration.c_str(), m_vTorsoJointVelocityAccelerationDefault[ii], l_sTorsoJointVelocityAccelerationInfo.c_str()).asDouble();
            m_vTorsoJointPositionAcceleration[ii]   = oRf.check(l_sTorsoJointPositionAcceleration.c_str(), m_vTorsoJointPositionAccelerationDefault[ii], l_sTorsoJointPositionAccelerationInfo.c_str()).asDouble();
            m_vTorsoJointPositionSpeed[ii]          = oRf.check(l_sTorsoJointPositionSpeed.c_str(),        m_vTorsoJointPositionSpeedDefault[ii],        l_sTorsoJointPositionSpeedInfo.c_str()).asDouble();
            m_vTorsoJointVelocityK[ii]              = oRf.check(l_sTorsoJointVelocityK.c_str(),            m_vTorsoJointVelocityKDefault[ii],            l_sTorsoJointVelocityKInfo.c_str()).asDouble();
        }

    // miscellaneous
        m_i32TimeoutTorsoReset   = oRf.check("torsoTimeoutReset",   Value(m_i32TimeoutTorsoResetDefault), "torso timeout reset iCub (int)").asInt();

    // set polydriver options
        m_oTorsoOptions.put("robot",     m_sRobotName.c_str());
        m_oTorsoOptions.put("device",    "remote_controlboard");
        m_oTorsoOptions.put("local",    ("/local/" + m_sRobotName + "/torso").c_str());
        m_oTorsoOptions.put("name",     ("/" + m_sRobotName + "/torso").c_str());
        m_oTorsoOptions.put("remote",   ("/" + m_sRobotName + "/torso").c_str());

    // init polydriver
        m_oRobotTorso.open(m_oTorsoOptions);
        if(!m_oRobotTorso.isValid())
        {
            std::cerr << "-ERROR: robotTorso is not valid, escape torso initialization. " << std::endl;
            return (m_bInitialized=false);
        }

    // initializing controllers
        if (!m_oRobotTorso.view(m_pITorsoVelocity) || !m_oRobotTorso.view(m_pITorsoPosition) || !m_oRobotTorso.view(m_pITorsoEncoders) ||!m_oRobotTorso.view(m_pITorsoControlMode))
        {
            std::cerr << std::endl << "-ERROR: while getting required robot Torso interfaces." << std::endl << std::endl;
            m_oRobotTorso.close();
            return (m_bInitialized=false);
        }

    // init ports
        m_sTorsoTrackerPortName  = "/teleoperation/" + m_sRobotName + "/torso";

    // open ports
        bool l_bPortOpeningSuccess = true;
        if(m_bTorsoActivated)
        {
            l_bPortOpeningSuccess = m_oTorsoTrackerPort.open(m_sTorsoTrackerPortName.c_str());
        }

        if(!l_bPortOpeningSuccess)
        {
            std::cerr << std::endl << "-ERROR: Unable to open ports." << std::endl << std::endl;
            m_oRobotTorso.close();
            return (m_bInitialized=false);
        }

    // retrieve Torso number of joints
        m_pITorsoPosition->getAxes(&m_i32TorsoJointsNb);

    // set accelerations/speeds
        for(int ii = 0; ii < m_i32TorsoJointsNb; ++ii)
        {
            m_pITorsoPosition->setRefAcceleration(ii, m_vTorsoJointPositionAcceleration[ii]);
            m_pITorsoPosition->setRefSpeed(ii, m_vTorsoJointPositionSpeed[ii]);
            m_pITorsoVelocity->setRefAcceleration(ii, m_vTorsoJointVelocityAcceleration[ii]);
        }

    // init controller                
        m_pVelocityController = new swTeleop::SWTorsoVelocityController(m_pITorsoEncoders, m_pITorsoVelocity, m_pITorsoControlMode, m_vTorsoJointVelocityK, m_i32RateVelocityControl);
        m_pVelocityController->enableTorso(m_bTorsoActivated);

    // display parameters
        std::cout << std::endl << std::endl;
        displayDebug(std::string("Torso activated"), m_bTorsoActivated);
        displayDebug(std::string("Timeout torso reset"), m_i32TimeoutTorsoReset);
        displayDebug(std::string("Rate velocity control"), m_i32RateVelocityControl);
        std::cout << std::endl;
        displayVectorDebug(std::string("Torso min joint                  : "), m_vTorsoMinJoint);
        displayVectorDebug(std::string("Torso max joint                  : "), m_vTorsoMaxJoint);
        displayVectorDebug(std::string("Torso reset position joint       : "), m_vTorsoResetPosition);
        displayVectorDebug(std::string("Torso joint velocity acceleration: "), m_vTorsoJointVelocityAcceleration);
        displayVectorDebug(std::string("Torso joint position acceleration: "), m_vTorsoJointPositionAcceleration);
        displayVectorDebug(std::string("Torso joint position speed       : "), m_vTorsoJointPositionSpeed);
        displayVectorDebug(std::string("Torso head joint velocity K      : "), m_vTorsoJointVelocityK);
        std::cout << std::endl << std::endl;


	return (m_bIsRunning=m_bInitialized=true);
}