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); }
bool asvGrabberModule::configure(yarp::os::ResourceFinder &rf) { /* Process all parameters from both command-line and .ini file */ /* get the module name which will form the stem of all module port names */ moduleName = rf.check("name", Value("/asvGrabber"), "module name (string)").asString(); /* * before continuing, set the module name before getting any other parameters, * specifically the port names which are dependent on the module name */ setName(moduleName.c_str()); /* * get the robot name which will form the stem of the robot ports names * and append the specific part and device required */ robotName = rf.check("robot", Value("icub"), "Robot name (string)").asString(); robotPortName = "/" + robotName + "/head"; /* * get the device name which will be used to read events */ deviceName = rf.check("deviceName", Value("/dev/aerfx2_0"), "Device name (string)").asString(); devicePortName = deviceName ; printf("trying to connect to the device %s \n",devicePortName.c_str()); /* * attach a port of the same name as the module (prefixed with a /) to the module * so that messages received from the port are redirected to the respond method */ handlerPortName = ""; handlerPortName += getName(); // use getName() rather than a literal if (!handlerPort.open(handlerPortName.c_str())) { cout << getName() << ": Unable to open port " << handlerPortName << endl; return false; } attach(handlerPort); // attach to port bool _save = false; std::string deviceNum = "0"; /* * get the file name of binaries when the biases are read from this file */ binaryName = rf.check("file", Value("none"), "filename of the binary (string)").asString(); printf("trying to read %s for biases \n",binaryName.c_str()); binaryNameComplete = rf.findFile(binaryName.c_str()); /* * get the file name of binaries when the biases are read from this file */ dumpNameComplete = ""; dumpName = rf.check("dumpFile", Value("none"), "filename of the binary (string)").asString(); printf("trying to save events in %s \n",dumpName.c_str()); dumpNameComplete = rf.findFile(dumpName.c_str()); dumpPathDirectory = rf.getContextPath(); string pathDirectory = dumpPathDirectory.substr(0, 52); pathDirectory += "/"; printf("saving directory %s \n",dumpPathDirectory.c_str()); if(!strcmp(binaryName.c_str(),"none")) { printf("not reading from binary \n"); D2Y=new asvGrabberThread(devicePortName, false,binaryName); //D2Y->setFromBinary(false); } else { printf("reading from binary \n"); //D2Y->setFromBinary(true); D2Y=new asvGrabberThread(devicePortName, true, binaryNameComplete); //D2Y->setBinaryFile(f); } printf("dumpEventSet \n"); if (strcmp(dumpNameComplete.c_str(),"" )) { printf("set dumping event true \n"); D2Y->setDumpEvent(true); D2Y->setDumpFile(dumpNameComplete); D2Y->setWorkingDirectory(pathDirectory); } else { printf("set dumping event false \n"); D2Y->setDumpEvent(false); } printf("starting the processor \n"); D2Y->start(); return true ; // let the RFModule know everything went well // so that it will then run the module }
bool 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 ; }
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 }
bool SimToolLoaderModule::configure(yarp::os::ResourceFinder &rf) { Bottle table; Bottle temp; string objectName = "obj"; /* module name */ moduleName = rf.check("name", Value("simtoolloader"), "Module name (string)").asString(); setName(moduleName.c_str()); /* Hand used */ hand=rf.find("hand").asString().c_str(); if ((hand!="left") && (hand!="right")) hand="right"; /* port names */ simToolLoaderSimOutputPortName = "/"; simToolLoaderSimOutputPortName += getName( rf.check("simToolLoaderSimOutputPort", Value("/sim:rpc"), "Loader output port(string)") .asString() ); simToolLoaderCmdInputPortName = "/"; simToolLoaderCmdInputPortName += getName( rf.check("simToolLoaderCmdInputPort", Value("/cmd:i"), "Loader input port(string)") .asString() ); /* open ports */ if (!simToolLoaderSimOutputPort.open( simToolLoaderSimOutputPortName.c_str())) { cout << getName() << ": unable to open port" << simToolLoaderSimOutputPortName << endl; return false; } if (!simToolLoaderCmdInputPort.open( simToolLoaderCmdInputPortName.c_str())) { cout << getName() << ": unable to open port" << simToolLoaderCmdInputPortName << endl; return false; } /* Rate thread period */ threadPeriod = rf.check("threadPeriod", Value(0.5), "Control rate thread period key value(double) in seconds ").asDouble(); /* Read Table Configuration */ table = rf.findGroup("table"); /* Read the Objects configurations */ vector<Bottle> object; cout << "Loading objects to buffer" << endl; bool noMoreModels = false; int n =0; while (!noMoreModels){ // read until there are no more objects. stringstream s; s.str(""); s << objectName << n; string objNameNum = s.str(); temp = rf.findGroup("objects").findGroup(objNameNum); if(!temp.isNull()) { cout << "object" << n << ", id: " << objNameNum; cout << ", model: " << temp.get(2).asString() << endl; object.push_back(temp); temp.clear(); n++; }else { noMoreModels = true; } } numberObjs = n; cout << "Loaded " << object.size() << " objects " << endl; /* Create the control rate thread */ ctrlThread = new CtrlThread(&simToolLoaderSimOutputPort, &simToolLoaderCmdInputPort, threadPeriod, table, object,hand); /* Starts the thread */ if (!ctrlThread->start()) { delete ctrlThread; return false; } return true; }
bool 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 }
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 }
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; }
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 }
bool ImageSource::configure(yarp::os::ResourceFinder &rf) { debug = true; /* Process all parameters from both command-line and .ini file */ /* get the module name which will form the stem of all module port names */ moduleName = rf.check("name", Value("imageSource"), "module name (string)").asString(); /* * before continuing, set the module name before getting any other parameters, * specifically the port names which are dependent on the module name */ setName(moduleName.c_str()); /* now, get the rest of the parameters */ /* * get the imageFilename */ imageFilename = rf.check("imageFile", Value("image.bmp"), "image filename (string)").asString(); imageFilename = (rf.findFile(imageFilename.c_str())).c_str(); /* get the complete name of the image output port */ outputPortName = rf.check("outputPort", Value("/image:o"), "Output image port (string)").asString(); /* get the complete name of the gaze output port */ gazePortName = rf.check("gazePort", Value("/gaze:o"), "Output gaze port (string)").asString(); /* get the complete name of the encoder state output port */ encoderPortName = rf.check("encoderPort", Value("/icub/head/state:o"), "Output encoder port (string)").asString(); /* get the frequency, width, height, standard deviation, horizontalViewAngle, and verticalViewAngle values */ frequency = rf.check("frequency", Value(10), "frequency key value (int)").asInt(); width = rf.check("width", Value(320), "output width key value (int)").asInt(); height = rf.check("height", Value(240), "output height key value (int)").asInt(); noiseLevel = rf.check("noise", Value(20), "noise level key value (int)").asInt(); window = rf.check("window", Value(0), "window flag key value (int)").asInt(); random = rf.check("random", Value(0), "random flag key value (int)").asInt(); xShift = rf.check("xShift", Value(5), "horizontal shift key value (int)").asInt(); yShift = rf.check("yShift", Value(5), "vertical shift key value (int)").asInt(); horizontalViewAngle = rf.check("horizontalViewAngle", Value(120.0), "horizontal field of view angle key value (double)").asDouble(); verticalViewAngle = rf.check("verticalViewAngle", Value(90.0), "vertical field of view angle key value (double)").asDouble(); if (debug) { cout << "imageSource::configure: image file name " << imageFilename << endl; cout << "imageSource::configure: output port name " << outputPortName << endl; cout << "imageSource::configure: gaze port name " << gazePortName << endl; cout << "imageSource::configure: encoder port name " << encoderPortName << endl; cout << "imageSource::configure: frequency " << frequency << endl; cout << "imageSource::configure: width " << width << endl; cout << "imageSource::configure: height " << height << endl; cout << "imageSource::configure: noise level " << noiseLevel << endl; cout << "imageSource::configure: window flag " << window << endl; cout << "imageSource::configure: random flag " << random << endl; cout << "imageSource::configure: x shift " << xShift << endl; cout << "imageSource::configure: y shift " << yShift << endl; cout << "imageSource::configure: horizontal FoV " << horizontalViewAngle << endl; cout << "imageSource::configure: vertical FoV " << verticalViewAngle << endl; } /* do all initialization here */ /* open ports */ if (!imageOut.open(outputPortName.c_str())) { cout << "imageSource::configure" << ": unable to open port " << outputPortName << endl; return false; // unable to open; let RFModule know so that it won't run } if (!gazeOut.open(gazePortName.c_str())) { cout << "imageSource::configure" << ": unable to open port " << gazePortName << endl; return false; // unable to open; let RFModule know so that it won't run } if (!encoderOut.open(encoderPortName.c_str())) { cout << "imageSource::configure" << ": unable to open port " << encoderPortName << endl; return false; // unable to open; let RFModule know so that it won't run } /* * attach a port of the same name as the module (prefixed with a /) to the module * so that messages received from the port are redirected to the respond method */ handlerPortName = "/"; handlerPortName += getName(); // use getName() rather than a literal if (!handlerPort.open(handlerPortName.c_str())) { cout << getName() << ": Unable to open port " << handlerPortName << endl; return false; } attach(handlerPort); // attach to port /* create the thread and pass pointers to the module parameters */ //cout << "imageSource::configure: calling Thread constructor" << endl; imageSourceThread = new ImageSourceThread(&imageOut, &gazeOut, &encoderOut, &imageFilename, (int)(1000 / frequency), &width, &height, &noiseLevel, &window, &random, &xShift, &yShift, &horizontalViewAngle, &verticalViewAngle); //cout << "imageSource::configure: returning from Thread constructor" << endl; /* now start the thread to do the work */ imageSourceThread->start(); // this calls threadInit() and it if returns true, it then calls run() return true; // let the RFModule know everything went well // so that it will then run the module }
bool 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 }
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 }
bool demoModule::configure(yarp::os::ResourceFinder &rf) { /* * PLEASE remove useless comments when writing actual code. If needed then use Doxygen comments and tags. */ /* Process all parameters from both command-line and .ini file */ /* get the module name which will form the stem of all module port names */ moduleName = rf.check("name", Value("demoModule"), "module name (string)").asString(); /* * before continuing, set the module name before getting any other parameters, * specifically the port names which are dependent on the module name */ setName(moduleName.c_str()); /* now, get the rest of the parameters */ /* * get the robot name which will form the stem of the robot ports names * and append the specific part and device required */ robotName = rf.check("robot", Value("icub"), "Robot name (string)").asString(); robotPortName = "/" + robotName + "/head"; /* * get the cameraConfig file and read the required parameter values cx, cy * in both the groups [CAMERA_CALIBRATION_LEFT] and [CAMERA_CALIBRATION_RIGHT] */ cameraConfigFilename = rf.check("cameraConfig", Value("icubEyes.ini"), "camera configuration filename (string)").asString(); cameraConfigFilename = (rf.findFile(cameraConfigFilename.c_str())).c_str(); Property cameraProperties; if (cameraProperties.fromConfigFile(cameraConfigFilename.c_str()) == false) { cout << "myModule: unable to read camera configuration file" << cameraConfigFilename; return 0; } else { cxLeft = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_LEFT").check("cx", Value(160.0), "cx left").asDouble(); cyLeft = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_LEFT").check("cy", Value(120.0), "cy left").asDouble(); cxRight = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_RIGHT").check("cx", Value(160.0), "cx right").asDouble(); cyRight = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_RIGHT").check("cy", Value(120.0), "cy right").asDouble(); } /* get the name of the input and output ports, automatically prefixing the module name by using getName() */ inputPortName = "/"; inputPortName += getName( rf.check("myInputPort", Value("/demoModule/image:i"), "Input image port (string)").asString() ); outputPortName = "/"; outputPortName += getName( rf.check("myOutputPort", Value("/demoModule/image:o"), "Output image port (string)").asString() ); /* get the threshold value */ thresholdValue = rf.check("threshold", Value(8), "Key value (int)").asInt(); /* do all initialization here */ /* * attach a port of the same name as the module (prefixed with a /) to the module * so that messages received from the port are redirected to the respond method */ handlerPortName = "/"; handlerPortName += getName(); // use getName() rather than a literal if (!handlerPort.open(handlerPortName.c_str())) { cout << getName() << ": Unable to open port " << handlerPortName << endl; return false; } attach(handlerPort); // attach to port /* create the thread and pass pointers to the module parameters */ dThread = new demoThread(&thresholdValue); /* now start the thread to do the work */ dThread->start(); // this calls threadInit() and it if returns true, it then calls run() return true ; // let the RFModule know everything went well // so that it will then run the module }
bool 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; }
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 }
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 }
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; }
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; }
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; }
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; } }
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; }
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())); }
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; }
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; }
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; }
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 ; }
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); }