void ReactiveLayer::configureTactile(yarp::os::ResourceFinder &rf) { //Initialise the tactile response Bottle grpTactile = rf.findGroup("TACTILE"); Bottle *tactileStimulus = grpTactile.find("stimuli").asList(); if (tactileStimulus) { for (int d = 0; d<tactileStimulus->size(); d++) { string tactileStimulusName = tactileStimulus->get(d).asString().c_str(); StimulusEmotionalResponse response; Bottle * bSentences = grpTactile.find((tactileStimulusName + "-sentence").c_str()).asList(); for (int s = 0; s<bSentences->size(); s++) { response.m_sentences.push_back(bSentences->get(s).asString().c_str()); } //choregraphies Bottle *bChore = grpTactile.find((tactileStimulusName + "-chore").c_str()).asList(); for (int sC = 0; bChore && sC<bChore->size(); sC++) { response.m_choregraphies.push_back(bChore->get(sC).asString().c_str()); } std::string sGroupTemp = tactileStimulusName; sGroupTemp += "-effect"; Bottle *bEffects = grpTactile.find(sGroupTemp.c_str()).asList(); for (int i = 0; bEffects && i<bEffects->size(); i += 2) { response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble(); } tactileEffects[tactileStimulusName] = response; } } }
void AdaptiveLayer::configureGestures(yarp::os::ResourceFinder &rf) { //Initialise the gestures response Bottle grpGesture = rf.findGroup("GESTURES"); Bottle *gestureStimulus = grpGesture.find("stimuli").asList(); if (gestureStimulus) { for(int d=0; d<gestureStimulus->size(); d++) { string gestureStimulusName = gestureStimulus->get(d).asString().c_str(); StimulusEmotionalResponse response; Bottle * bSentences = grpGesture.find((gestureStimulusName + "-sentence").c_str()).asList(); for(int s=0;s<bSentences->size();s++) { response.m_sentences.push_back(bSentences->get(s).asString().c_str()); } std::string sGroupTemp = gestureStimulusName; sGroupTemp += "-effect"; Bottle *bEffects = grpGesture.find( sGroupTemp.c_str()).asList(); for(int i=0; bEffects && i<bEffects->size(); i += 2) { response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i+1).asDouble(); } gestureEffects[gestureStimulusName] = response; } } }
void ReactiveLayer::configureSalutation(yarp::os::ResourceFinder &rf) { ; //Initialise the gestures response Bottle grpSocial = rf.findGroup("SOCIAL"); salutationLifetime = grpSocial.check("salutationLifetime", Value(15.0)).asDouble(); Bottle *socialStimulus = grpSocial.find("stimuli").asList(); if (socialStimulus) { for (int d = 0; d<socialStimulus->size(); d++) { string socialStimulusName = socialStimulus->get(d).asString().c_str(); StimulusEmotionalResponse response; Bottle * bSentences = grpSocial.find((socialStimulusName + "-sentence").c_str()).asList(); for (int s = 0; s<bSentences->size(); s++) { response.m_sentences.push_back(bSentences->get(s).asString().c_str()); } std::string sGroupTemp = socialStimulusName; sGroupTemp += "-effect"; Bottle *bEffects = grpSocial.find(sGroupTemp.c_str()).asList(); for (int i = 0; bEffects && i<bEffects->size(); i += 2) { response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble(); } salutationEffects[socialStimulusName] = response; } } //Add the relevant Entities for handling salutation iCub->opc->addOrRetrieveAction("is"); iCub->opc->addOrRetrieveAdjective("saluted"); }
bool visualFilterModule::configure(yarp::os::ResourceFinder &rf) { /* Process all parameters from both command-line and .ini file */ /* get the module name which will form the stem of all module port names */ moduleName = rf.check("name", Value("/visualFeaX"), "module name (string)").asString(); /* * before continuing, set the module name before getting any other parameters, * specifically the port names which are dependent on the module name */ setName(moduleName.c_str()); /* * get the robot name which will form the stem of the robot ports names * and append the specific part and device required */ robotName = rf.check("robot", Value("icub"), "Robot name (string)").asString(); robotPortName = "/" + robotName + "/head"; /* * attach a port of the same name as the module (prefixed with a /) to the module * so that messages received from the port are redirected to the respond method */ handlerPortName = ""; handlerPortName += getName(); // use getName() rather than a literal if (!handlerPort.open(handlerPortName.c_str())) { cout << getName() << ": Unable to open port " << handlerPortName << endl; return false; } attach(handlerPort); // attach to port /* create the thread and pass pointers to the module parameters */ printf("trying to start the main thread \n"); vfThread = new visualFilterThread(); vfThread->setName(getName().c_str()); /* now start the thread to do the work */ vfThread->start(); // this calls threadInit() and it if returns true, it then calls run() return true ; // let the RFModule know everything went well // so that it will then run the module }
bool ReactiveLayer::configure(yarp::os::ResourceFinder &rf) { string moduleName = rf.check("name",Value("ReactiveLayer")).asString().c_str(); setName(moduleName.c_str()); cout<<moduleName<<": finding configuration files..."<<endl; period = rf.check("period",Value(0.1)).asDouble(); //Create an iCub Client and check that all dependencies are here before starting bool isRFVerbose = false; iCub = new ICubClient(moduleName,"reactiveLayer","client.ini",isRFVerbose); iCub->opc->isVerbose = false; char rep = 'n'; while (rep!='y'&&!iCub->connect()) { cout<<"iCubClient : Some dependencies are not running..."<<endl; //cout<<"Continue? y,n"<<endl; //cin>>rep; break; //to debug Time::delay(1.0); } //Set the voice std::string ttsOptions = rf.check("ttsOptions", yarp::os::Value("iCubina 85.0")).asString(); if (iCub->getSpeechClient()) iCub->getSpeechClient()->SetOptions(ttsOptions); //Configure the various components configureOPC(rf); configureAllostatic(rf); configureTactile(rf); configureSalutation(rf); cout<<"Configuration done."<<endl; rpc.open ( ("/"+moduleName+"/rpc").c_str()); attach(rpc); //Initialise timers lastFaceUpdate = Time::now(); physicalInteraction = false; someonePresent = false; //iCub->getReactableClient()->SendOSC(yarp::os::Bottle("/event reactable pong start")); return true; }
bool GuiUpdaterModule::configure(yarp::os::ResourceFinder &rf) { iCub = NULL; string moduleName = rf.check("name", Value("guiUpdater"), "module name (string)").asString().c_str(); string opcName = rf.check("OPCname", Value("OPC"), "OPC name (string)").asString().c_str(); setName(moduleName.c_str()); displaySkeleton = rf.check("displaySkeletons"); cout<<"Display skeleton status: "<<displaySkeleton<<endl; w = new OPCClient(getName().c_str()); w->connect(opcName); w->isVerbose = false; //GUI Port string guiPortName = "/"; guiPortName += getName() + "/gui:o"; toGui.open(guiPortName.c_str()); resetGUI(); //GUI Base Port string guiBasePortName = "/"; guiBasePortName += getName() + "/guiBase:o"; toGuiBase.open(guiBasePortName.c_str()); //RPC string handlerPortName = "/"; handlerPortName += getName() + "/rpc"; if (!handlerPort.open(handlerPortName.c_str())) { cout << getName() << ": Unable to open port " << handlerPortName << endl; return false; } attach(handlerPort); // attach to port return true ; }
bool lumaChroma::configure(yarp::os::ResourceFinder &rf) { /* Process all parameters from both command-line and .ini file */ moduleName = rf.check("name", Value("lumaChroma"), "module name (string)").asString(); setName(moduleName.c_str()); imageType = rf.check("image", Value("yuv"), "image type (string)").asString(); whichPort = rf.check("out", Value(""), "default port (string)").asString(); /* * attach a port of the same name as the module (prefixed with a /) to the module * so that messages received from the port are redirected to the respond method */ handlerPortName = "/"; handlerPortName += getName(); // use getName() rather than a literal if (!handlerPort.open(handlerPortName.c_str())) { cout << getName() << ": Unable to open port " << handlerPortName << endl; return false; } attach(handlerPort); // attach to port /* create the thread and pass pointers to the module parameters */ procThread = new PROCThread(moduleName, imageType, whichPort); /* now start the thread to do the work */ procThread->open(); return true ; // let the RFModule know everything went well }
bool actionClass::parseTorqueBalancingSequences(std::string filenamePrefix, std::string filenameSuffix, int partID, std::string format, yarp::os::ResourceFinder &rf) { bool ret = false; actionStructForTorqueBalancing tmp_action; string filename = filenamePrefix + "_" + filenameSuffix + ".txt"; filename = rf.findFile(filename); fprintf(stderr, "[!!!] File found for %s: %s\n", filenameSuffix.c_str(), filename.c_str()); // Open file ifstream data_file( filename.c_str() ); string line; std::deque<double> tmp_com; std::deque<double> tmp_postural; std::deque<double> tmp_constraints; while( std::getline(data_file, line) ) { int col = 0; std::istringstream iss( line ); std::string result; tmp_com.clear(); tmp_postural.clear(); tmp_constraints.clear(); while( std::getline( iss, result , ' ') ) { if ( strcmp(result.c_str(),"") != 0 ) { std::stringstream convertor; convertor.clear(); convertor.str(result); if (partID == COM_ID) tmp_com.push_back(atof(result.c_str())); if (partID == POSTURAL_ID) tmp_postural.push_back(atof(result.c_str())); if (partID == CONSTRAINTS_ID) tmp_constraints.push_back(atoi(result.c_str())); col++; } } if (partID == COM_ID) action_vector_torqueBalancing.com_traj.push_back(tmp_com); if (partID == POSTURAL_ID) action_vector_torqueBalancing.postural_traj.push_back(tmp_postural); if (partID == CONSTRAINTS_ID) action_vector_torqueBalancing.constraints.push_back(tmp_constraints); } data_file.close(); ret = true; return ret; }
bool ShowModule::configure(yarp::os::ResourceFinder &rf) { //Ports string name=rf.check("name",Value("show3D")).asString().c_str(); string robot = rf.check("robot",Value("icub")).asString().c_str(); string cloudpath_file = rf.check("from",Value("cloudsPath.ini")).asString().c_str(); rf.findFile(cloudpath_file.c_str()); ResourceFinder cloudsRF; cloudsRF.setDefaultConfigFile(cloudpath_file.c_str()); cloudsRF.configure(0,NULL); // Set the path that contains previously saved pointclouds if (rf.check("clouds_path")){ cloudpath = rf.find("clouds_path").asString().c_str(); }else{ string defPathFrom = "/share/ICUBcontrib/contexts/toolIncorporation/sampleClouds/"; string localModelsPath = rf.check("local_path")?rf.find("local_path").asString().c_str():defPathFrom; //cloudsRF.find("clouds_path").asString(); string icubContribEnvPath = yarp::os::getenv("ICUBcontrib_DIR"); cloudpath = icubContribEnvPath + localModelsPath; } handlerPort.open("/"+name+"/rpc:i"); attach(handlerPort); cloudsInPort.open("/"+name+"/clouds:i"); // Module rpc parameters closing = false; // Init variables cloudfile = "cloud.ply"; cloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr (new pcl::PointCloud<pcl::PointXYZRGB>); //Threads visThrd = new VisThread(50, "Cloud"); if (!visThrd->start()) { delete visThrd; visThrd = 0; cout << "\nERROR!!! visThread wasn't instantiated!!\n"; return false; } cout << "PCL visualizer Thread istantiated...\n"; cout << endl << "Configuring done."<<endl; printf("Base path: %s \n \n",cloudpath.c_str()); return true; }
void printInstalledFolders(yarp::os::ResourceFinder &rf, folderType ftype) { ResourceFinderOptions opts; opts.searchLocations=ResourceFinderOptions::Installed; yarp::os::Bottle contextPaths=rf.findPaths(getFolderStringName(ftype), opts); printf("**INSTALLED DATA:\n"); for (int curPathId=0; curPathId<contextPaths.size(); ++curPathId) { printf("* Directory : %s\n", contextPaths.get(curPathId).asString().c_str()); printContentDirs(contextPaths.get(curPathId).asString()); } }
/* * Configure function. Receive a previously initialized * resource finder object. Use it to configure your module. * Open port and attach it to message handler. */ bool configure(yarp::os::ResourceFinder &rf) { timeout=rf.check("timeout",Value(60.0)).asDouble(); vergencePort.open("/radHelper/vergence:i"); handlerPort.open("/radHelper/attentionStatus:io"); suspendPort.open("/radHelper/suspend:o"); attach(handlerPort); //Network::connect("/gazeArbiter/icub/left_cam/status:o","/radHelper/vergence:i","udp"); //Network::connect("/radHelper/suspend:o","/gazeArbiter/icub/left_cam"); return true; }
bool InterpersonalDistanceRegulator::configure(yarp::os::ResourceFinder &rf) { isPaused = false; isQuitting = false; string moduleName = rf.check("name",Value("interpersonalDistanceRegulator")).asString().c_str(); setName(moduleName.c_str()); cout<<moduleName<<": finding configuration files..."<<endl; period = rf.check("period",Value(0.1)).asDouble(); preferedDistanceToPeople = rf.check("preferedDistanceToPeople",Value(0.9)).asDouble(); fwdSpeed = rf.check("forwardSpeed", Value(0.075)).asDouble(); backwdSpeed = rf.check("backwardSpeed", Value(0.075)).asDouble(); turningSpeed = rf.check("angularSpeed", Value(0.075)).asDouble(); ikart = new SubSystem_iKart(moduleName); while (!ikart->Connect()) { cout << "Connecting IDR to iKart..." << endl; Time::delay(1.0); } opc = new OPCClient(moduleName); while (!opc->connect("OPC")) { cout << "Connecting InterpersonalThread to OPC..." << endl; Time::delay(1.0); } cout<<"Configuration done."<<endl; rpc.open ( ("/"+moduleName+"/rpc").c_str()); attach(rpc); return true; }
void prepareHomeFolder(yarp::os::ResourceFinder &rf, folderType ftype) { ACE_DIR* dir= YARP_opendir((rf.getDataHome()).c_str()); if (dir!=NULL) YARP_closedir(dir); else { yarp::os::mkdir((rf.getDataHome()).c_str()); } dir= YARP_opendir((rf.getDataHome() + PATH_SEPARATOR + getFolderStringName(ftype)).c_str()); if (dir!=NULL) YARP_closedir(dir); else { yarp::os::mkdir((rf.getDataHome() + PATH_SEPARATOR + getFolderStringName(ftype)).c_str()); } dir= YARP_opendir((rf.getDataHome() + PATH_SEPARATOR + getFolderStringNameHidden(ftype)).c_str()); if (dir!=NULL) YARP_closedir(dir); else { ConstString hiddenPath=(rf.getDataHome() + PATH_SEPARATOR + getFolderStringNameHidden(ftype)); yarp::os::mkdir(hiddenPath.c_str()); #ifdef WIN32 SetFileAttributes(hiddenPath.c_str(), FILE_ATTRIBUTE_HIDDEN); #endif } }
bool QuaternionEKF::readEstimatorParams(yarp::os::ResourceFinder &rf, quaternionEKFParams &estimatorParams) { yarp::os::Bottle botParams; botParams = rf.findGroup("QuaternionEKF"); yInfo("QuaternionEKF params are: %s ", botParams.toString().c_str()); if ( botParams.isNull() ) { yError("[QuaternionEKF::readEstimatorParams] No parameters were read from QuaternionEKF group"); return false; } else { estimatorParams.stateSize = botParams.find("state_size").asInt(); estimatorParams.inputSize = botParams.find("input_size").asInt(); estimatorParams.measurementSize = botParams.find("measurement_size").asInt(); estimatorParams.muSystemNoise = botParams.find("mu_system_noise").asDouble(); estimatorParams.sigmaSystemNoise = botParams.find("sigma_system_noise").asDouble(); estimatorParams.sigmaMeasurementNoise = botParams.find("sigma_measurement_noise").asDouble(); estimatorParams.sigmaGyro = botParams.find("sigma_gyro_noise").asDouble(); estimatorParams.piorMu = botParams.find("prior_mu_state").asDouble(); estimatorParams.priorCovariance = botParams.find("prior_cov_state").asDouble(); estimatorParams.muGyroNoise = botParams.find("mu_gyro_noise").asDouble(); estimatorParams.floatingBaseAttitude = botParams.find("floating_base_attitude").asBool(); estimatorParams.rot_from_ft_to_acc_bottle = new yarp::os::Bottle(*botParams.find("rot_from_ft_to_acc").asList()); } botParams.clear(); botParams = rf.findGroup("module_parameters"); if ( botParams.isNull() ) { yError("[QuaternionEKF::readEstimatorParams] No parameters were read from 'module_params' group. period is needed!"); return false; } else { estimatorParams.period = botParams.find("period").asInt(); estimatorParams.robotPrefix = botParams.find("robot").asString(); estimatorParams.streamMeasurements = botParams.find("stream_measurements").asBool(); } return true; }
bool ears::configure(yarp::os::ResourceFinder &rf) { string moduleName = rf.check("name", Value("ears")).asString().c_str(); setName(moduleName.c_str()); yInfo() << moduleName << " : finding configuration files..."; period = rf.check("period", Value(0.1)).asDouble(); //Create an iCub Client and check that all dependencies are here before starting bool isRFVerbose = false; iCub = new ICubClient(moduleName, "ears", "client.ini", isRFVerbose); iCub->opc->isVerbose = false; if (!iCub->connect()) { yInfo() << " iCubClient : Some dependencies are not running..."; Time::delay(1.0); } rpc.open(("/" + moduleName + "/rpc").c_str()); attach(rpc); portToBehavior.open("/" + moduleName + "/behavior:o"); while (!Network::connect(portToBehavior.getName(),"/BehaviorManager/trigger:i ")) { yWarning() << " Behavior is not reachable"; yarp::os::Time::delay(0.5); } portTarget.open("/" + moduleName + "/target:o"); MainGrammar = rf.findFileByName(rf.check("MainGrammar", Value("MainGrammar.xml")).toString()); bShouldListen = true; yInfo() << "\n \n" << "----------------------------------------------" << "\n \n" << moduleName << " ready ! \n \n "; return true; }
bool AudioPowerMapModule::configure(yarp::os::ResourceFinder &rf) { yInfo("Configuring the module"); // get the module name which will form the stem of all module port names moduleName = rf.check("name", Value("/AudioPowerMap"), "module name (string)").asString(); // before continuing, set the module name before getting any other parameters, // specifically the port names which are dependent on the module name setName(moduleName.c_str()); // get the robot name which will form the stem of the robot ports names // and append the specific part and device required robotName = rf.check("robot", Value("icub"), "Robot name (string)").asString(); robotPortName = "/" + robotName + "/head"; inputPortName = rf.check("inputPortName", Value(":i"), "Input port name (string)").asString(); // attach a port of the same name as the module (prefixed with a /) to the module // so that messages received from the port are redirected to the respond method handlerPortName = ""; handlerPortName += getName(); if (!handlerPort.open(handlerPortName.c_str())) { std::cout << getName() << ": Unable to open port " << handlerPortName << std::endl; return false; } // attach to port attach(handlerPort); if (rf.check("config")) { configFile=rf.findFile(rf.find("config").asString().c_str()); if (configFile=="") { return false; } } else { configFile.clear(); } // create the thread and pass pointers to the module parameters apr = new AudioPowerMapRatethread(robotName, configFile, rf); apr->setName(moduleName); // now start the thread to do the work // this calls threadInit() and it if returns true, it then calls run() bool ret = apr->start(); // let the RFModule know everything went well // so that it will then run the module return ret; }
virtual bool configure(yarp::os::ResourceFinder &rf) { yarp::os::Time::turboBoost(); mNavThread=new Navigator(&rf); if (!mNavThread->start()) { delete mNavThread; return false; } std::string local=rf.check("local",yarp::os::Value("/ikartnav")).asString().c_str(); mHandlerPort.open(local.c_str()); attach(mHandlerPort); //attachTerminal(); return true; }
bool configure( yarp::os::ResourceFinder &rf ) { std::string moduleName = rf.check("name", yarp::os::Value("demoServerModule"), "module name (string)").asString().c_str(); setName(moduleName.c_str()); std::string slash="/"; attach(cmdPort); std::string cmdPortName= "/"; cmdPortName+= getName(); cmdPortName += "/cmd"; if (!cmdPort.open(cmdPortName.c_str())) { std::cout << getName() << ": Unable to open port " << cmdPortName << std::endl; return false; } return true; }
bool configure(yarp::os::ResourceFinder &rf) { path = rf.find("clouds_path").asString(); printf("Path: %s",path.c_str()); handlerPort.open("/mergeModule"); attach(handlerPort); /* Module rpc parameters */ visualizing = false; saving = true; closing = false; /*Init variables*/ initF = true; filesScanned = 0; cout<<"Configuring done."<<endl; return true; }
virtual bool configure(yarp::os::ResourceFinder &rf) { yarp::os::Time::turboBoost(); Property p; ConstString configFile = rf.findFile("from"); if (configFile!="") p.fromConfigFile(configFile.c_str()); gotoThread = new GotoThread(10,rf,p); if (!gotoThread->start()) { delete gotoThread; return false; } rpcPort.open("/ikartGoto/rpc:i"); attach(rpcPort); //attachTerminal(); return true; }
bool Recognition::initPorts(yarp::os::ResourceFinder &rf){ moduleName = rf.check("name", Value("Recognition"), "module name (string)").asString(); /* * before continuing, set the module name before getting any other parameters, * specifically the port names which are dependent on the module name */ setName(moduleName.c_str()); // Open cam left camLeftName = "/"; camLeftName += getName() + "/in/left"; if (!camLeft.open(camLeftName.c_str())) { cout << getName() << ": Unable to open port " << camLeftName << endl; return false; } portAckName = "/"; portAckName += getName() + "/out"; if (!portAck.open(portAckName.c_str())) { cout << getName() << ": Unable to open port " << portAckName << endl; return false; } portInName = "/"; portInName += getName() + "/in"; if (!portIn.open(portInName.c_str())) { cout << getName() << ": Unable to open port " << portInName << endl; return false; } attach(portIn); return true; }
bool DispBlobberModule::configure(yarp::os::ResourceFinder &rf) { moduleName = rf.check("name", Value("dispBlobber"), "module name (string)").asString(); setName(moduleName.c_str()); handlerPortName = "/"; handlerPortName += getName(); handlerPortName += "/rpc:i"; if (!handlerPort.open(handlerPortName.c_str())) { fprintf(stdout, "%s : Unable to open RPC port %s\n", getName().c_str(), handlerPortName.c_str()); return false; } attach(handlerPort); blobPort = new DispBlobberPort( moduleName, rf ); blobPort->open(); closing = false; return true ; }
bool babbler::configure(yarp::os::ResourceFinder &rf) { elapsedCycles = 0; f = 0; string moduleName = rf.check("name", Value("tuneBabbler")).asString().c_str(); setName(moduleName.c_str()); string moduleOutput = rf.check("output", Value("/microphone")).asString().c_str(); int seed = rf.check("seed", Value(1)).asInt(); samples = rf.check("samples", Value(512)).asInt(); rate = rf.check("rate", Value(8000)).asInt(); channels = rf.check("channels", Value(1)).asInt(); bool bEveryThingisGood = true; // create an output port port2output = moduleOutput; if (!portOutput.open(port2output.c_str())) { cout << getName() << ": Unable to open port " << port2output << endl; cout << "The microphone might be turned on" << endl; bEveryThingisGood = false; } cout << moduleName << ": finding configuration files..." << endl; rpc.open(("/" + moduleName + "/rpc").c_str()); attach(rpc); srand(seed); return bEveryThingisGood; }
/** * Performs module configuration, parsing user options stored in the resource finder. * Available options are described in main module documentation. * @return true if the module was successfully configured and opened, false otherwise. */ virtual bool configure(yarp::os::ResourceFinder &rf) { yarp::os::Time::turboBoost(); m_rpcPort.open("/"+m_module_name+"/rpc"); attach(m_rpcPort); //attachTerminal(); m_rf = rf; //configuration file checking Bottle general_group = m_rf.findGroup("GENERAL"); if (general_group.isNull()) { yError() << "Missing GENERAL group!"; return false; } Bottle initial_group = m_rf.findGroup("INITIAL_POS"); if (initial_group.isNull()) { yError() << "Missing INITIAL_POS group!"; return false; } Bottle localization_group = m_rf.findGroup("LOCALIZATION"); if (localization_group.isNull()) { yError() << "Missing LOCALIZATION group!"; return false; } Bottle ros_group = m_rf.findGroup("ROS"); Bottle tf_group = m_rf.findGroup("TF"); if (tf_group.isNull()) { yError() << "Missing TF group!"; return false; } Bottle odometry_group = m_rf.findGroup("ODOMETRY"); if (odometry_group.isNull()) { yError() << "Missing ODOMETRY group!"; return false; } Bottle map_group = m_rf.findGroup("MAP"); if (map_group.isNull()) { yError() << "Missing MAP group!"; return false; } yDebug() << map_group.toString(); //general group if (general_group.check("module_name") == false) { yError() << "Missing `module_name` in [GENERAL] group"; return false; } m_module_name = general_group.find("module_name").asString(); if (general_group.check("enable_ros") == false) { yError() << "Missing `ros_enable` in [GENERAL] group"; return false; } m_ros_enabled = (general_group.find("enable_ros").asInt()==1); //ROS group if (m_ros_enabled) { m_rosNode = new yarp::os::Node("/"+m_module_name); //initialize an occupancy grid publisher (every time the localization is re-initialized, the map is published too) if (ros_group.check ("occupancygrid_topic")) { m_topic_occupancyGrid = ros_group.find ("occupancygrid_topic").asString(); if (!m_rosPublisher_occupancyGrid.topic(m_topic_occupancyGrid)) { if (m_rosNode) { delete m_rosNode; m_rosNode=0; } yError() << "localizationModule: unable to publish data on " << m_topic_occupancyGrid << " topic, check your yarp-ROS network configuration"; return false; } } //initialize an initial pose publisher if (ros_group.check ("initialpose_topic")) { m_topic_initial_pose = ros_group.find ("initialpose_topic").asString(); { if (!m_rosPublisher_initial_pose.topic(m_topic_initial_pose)) { if (m_rosNode) { delete m_rosNode; m_rosNode=0; } yError() << "localizationModule: unable to publish data on " << m_topic_initial_pose << " topic, check your yarp-ROS network configuration"; return false; } } } } //localization group if (localization_group.check("use_localization_from_odometry_port")) { m_use_localization_from_odometry_port = (localization_group.find("use_localization_from_odometry_port").asInt() == 1); } if (localization_group.check("use_localization_from_tf")) { m_use_localization_from_tf = (localization_group.find("use_localization_from_tf").asInt() == 1); } if (m_use_localization_from_odometry_port == true && m_use_localization_from_tf == true) { yError() << "`use_localization_from_tf` and `use_localization_from_odometry_port` cannot be true simultaneously!"; return false; } //map server group yDebug() << map_group.toString(); if (map_group.check("connect_to_yarp_mapserver") == false) { yError() << "Missing `connect_to_yarp_mapserver` in [MAP] group"; return false; } m_use_map_server= (map_group.find("connect_to_yarp_mapserver").asInt()==1); //tf group if (tf_group.check("map_frame_id") == false) { yError() << "Missing `map_frame_id` in [TF] group"; return false; } if (tf_group.check("robot_frame_id") == false) { yError() << "Missing `robot_frame_id` in [TF] group"; return false; } m_frame_map_id = tf_group.find("map_frame_id").asString(); m_frame_robot_id = tf_group.find("robot_frame_id").asString(); //odometry group if (odometry_group.check("odometry_broadcast_port") == false) { yError() << "Missing `odometry_port` in [ODOMETRY] group"; return false; } m_port_broadcast_odometry_name = odometry_group.find("odometry_broadcast_port").asString(); //device driver opening and/or connections if (m_use_localization_from_odometry_port) { //opens a YARP port to receive odometry data std::string odom_portname = "/" + m_module_name + "/odometry:i"; bool b1 = m_port_odometry_input.open(odom_portname.c_str()); bool b2 = yarp::os::Network::sync(odom_portname.c_str(),false); bool b3 = yarp::os::Network::connect(m_port_broadcast_odometry_name.c_str(), odom_portname.c_str()); if (b1 == false || b2 ==false || b3==false) { yError() << "Unable to initialize odometry port connection from " << m_port_broadcast_odometry_name.c_str()<< "to:" << odom_portname.c_str(); return false; } } if (m_use_localization_from_tf) { //opens a client to receive localization data from transformServer Property options; options.put("device", "transformClient"); options.put("local", "/"+m_module_name + "/TfClient"); options.put("remote", "/transformServer"); if (m_ptf.open(options) == false) { yError() << "Unable to open transform client"; return false; } m_ptf.view(m_iTf); if (m_ptf.isValid() == false || m_iTf == 0) { yError() << "Unable to view iTransform interface"; return false; } } if (m_use_map_server) { //opens a client to send/received data from mapServer Property map_options; map_options.put("device", "map2DClient"); map_options.put("local", "/" +m_module_name); //This is just a prefix. map2DClient will complete the port name. map_options.put("remote", "/mapServer"); if (m_pmap.open(map_options) == false) { yWarning() << "Unable to open mapClient"; } else { yInfo() << "Opened mapClient"; m_pmap.view(m_iMap); if (m_pmap.isValid() == false || m_iMap == 0) { yError() << "Unable to view map interface"; return false; } } } //initial location initialization if (initial_group.check("initial_x")) { m_initial_loc.x = initial_group.find("initial_x").asDouble(); } else { yError() << "missing initial_x param"; return false; } if (initial_group.check("initial_y")) { m_initial_loc.y = initial_group.find("initial_y").asDouble(); } else { yError() << "missing initial_y param"; return false; } if (initial_group.check("initial_theta")) { m_initial_loc.theta = initial_group.find("initial_theta").asDouble(); } else { yError() << "missing initial_theta param"; return false; } if (initial_group.check("initial_map")) { m_initial_loc.map_id = initial_group.find("initial_map").asString(); } else { yError() << "missing initial_map param"; return false; } this->initializeLocalization(m_initial_loc); return true; }
bool BehaviorManager::configure(yarp::os::ResourceFinder &rf) { moduleName = rf.check("name",Value("BehaviorManager")).asString(); setName(moduleName.c_str()); yInfo()<<moduleName<<": finding configuration files...";//<<endl; period = rf.check("period",Value(1.0)).asDouble(); Bottle grp = rf.findGroup("BEHAVIORS"); Bottle behaviorList = *grp.find("behaviors").asList(); rpc_in_port.open("/" + moduleName + "/trigger:i"); yInfo() << "RPC_IN : " << rpc_in_port.getName(); for (int i = 0; i<behaviorList.size(); i++) { behavior_name = behaviorList.get(i).asString(); if (behavior_name == "tagging") { behaviors.push_back(new Tagging(&mut, rf, "tagging")); } else if (behavior_name == "pointing") { behaviors.push_back(new Pointing(&mut, rf, "pointing")); } else if (behavior_name == "dummy") { behaviors.push_back(new Dummy(&mut, rf, "dummy")); } else if (behavior_name == "dummy2") { behaviors.push_back(new Dummy(&mut, rf, "dummy2")); } else if (behavior_name == "reactions") { behaviors.push_back(new Reactions(&mut, rf, "reactions")); } else if (behavior_name == "followingOrder") { behaviors.push_back(new FollowingOrder(&mut, rf, "followingOrder")); } else if (behavior_name == "narrate") { behaviors.push_back(new Narrate(&mut, rf, "narrate")); } else if (behavior_name == "recognitionOrder") { behaviors.push_back(new RecognitionOrder(&mut, rf, "recognitionOrder")); } else if (behavior_name == "greeting") { behaviors.push_back(new Greeting(&mut, rf, "greeting")); } else if (behavior_name == "ask") { behaviors.push_back(new Ask(&mut, rf, "ask")); } else if (behavior_name == "speech") { behaviors.push_back(new Speech(&mut, rf, "speech")); } else if (behavior_name == "moveObject") { behaviors.push_back(new MoveObject(&mut, rf, "moveObject")); // other behaviors here } else { yDebug() << "Behavior " + behavior_name + " not implemented"; return false; } } //Create an iCub Client and check that all dependencies are here before starting bool isRFVerbose = false; iCub = new wysiwyd::wrdac::ICubClient(moduleName, "behaviorManager","client.ini",isRFVerbose); if (!iCub->connect()) { yInfo() << "iCubClient : Some dependencies are not running..."; Time::delay(1.0); } if (rf.check("use_ears",Value("false")).asBool()) { yDebug()<<"using ears"; while (!Network::connect("/ears/behavior:o", rpc_in_port.getName())) { yWarning() << "Ears is not reachable"; yarp::os::Time::delay(0.5); } }else{ yDebug()<<"not using ears"; } // id = 0; for(auto& beh : behaviors) { beh->configure(); beh->openPorts(moduleName); beh->iCub = iCub; if (beh->from_sensation_port_name != "None") { while (!Network::connect(beh->from_sensation_port_name, beh->sensation_port_in.getName())) { yInfo()<<"Connecting "<< beh->from_sensation_port_name << " to " << beh->sensation_port_in.getName();// <<endl; yarp::os::Time::delay(0.5); } } if (beh->external_port_name != "None") { while (!Network::connect(beh->rpc_out_port.getName(), beh->external_port_name)) { yInfo()<<"Connecting "<< beh->rpc_out_port.getName() << " to " << beh->external_port_name;// <<endl; yarp::os::Time::delay(0.5); } } } attach(rpc_in_port); yInfo("Init done"); return true; }
bool Rectification::configure(yarp::os::ResourceFinder &rf) { /* * Process all parameters from * - command-line * - rectification.ini file (or whatever file is specified by the --from argument) * - icubEyes.ini file (or whatever file is specified by the --cameraConfig argument */ /* get the module name which will form the stem of all module port names */ moduleName = rf.check("name", Value("rectification"), "module name (string)").asString(); /* * before continuing, set the module name before getting any other parameters, * specifically the port names which are dependent on the module name */ setName(moduleName.c_str()); /* * get the robot name which will form the stem of the robot ports names * and append the specific part and device required */ robotName = rf.check("robot", Value("icub"), "Robot name (string)").asString(); /* * get the cameraConfig file and read the required parameter values, fx, fy, cx, cy * in both the groups [CAMERA_CALIBRATION_LEFT] and [CAMERA_CALIBRATION_RIGHT] */ cameraConfigFilename = rf.check("cameraConfig", Value("icubEyes.ini"), "camera configuration filename (string)").asString(); cameraConfigFilename = (rf.findFile(cameraConfigFilename.c_str())).c_str(); Property cameraProperties; if (cameraProperties.fromConfigFile(cameraConfigFilename.c_str()) == false) { cout << "rectification: unable to read camera configuration file" << cameraConfigFilename; return 0; } else { fxLeft = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_LEFT").check("fx", Value(225.0), "fx left").asDouble(); fyLeft = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_LEFT").check("fy", Value(225.0), "fy left").asDouble(); cxLeft = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_LEFT").check("cx", Value(160.0), "cx left").asDouble(); cyLeft = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_LEFT").check("cy", Value(120.0), "cy left").asDouble(); fxRight = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_RIGHT").check("fx", Value(225.0), "fx right").asDouble(); fyRight = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_RIGHT").check("fy", Value(225.0), "fy right").asDouble(); cxRight = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_RIGHT").check("cx", Value(160.0), "cx right").asDouble(); cyRight = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_RIGHT").check("cy", Value(120.0), "cy right").asDouble(); } /* get the name of the input and output ports, automatically prefixing the module name by using getName() */ leftInputPortName = "/"; leftInputPortName += getName( rf.check("leftImageInPort", Value("/leftImage:i"), "Left input image port (string)").asString() ); rightInputPortName = "/"; rightInputPortName += getName( rf.check("rightImageInPort", Value("/rightImage:i"), "Right input image port (string)").asString() ); leftOutputPortName = "/"; leftOutputPortName += getName( rf.check("leftImageOutPort", Value("/leftImage:o"), "Left output image port (string)").asString() ); rightOutputPortName = "/"; rightOutputPortName += getName( rf.check("rightImageOutPort", Value("/rightImage:o"), "Right output image port (string)").asString() ); robotPortName = "/"; robotPortName += getName( rf.check("headPort", Value("/head:i"), "Robot head encoder state port (string)").asString() ); if (debug) { printf("rectification: module name is %s\n",moduleName.c_str()); printf("rectification: robot name is %s\n",robotName.c_str()); printf("rectification: camera configuration filename is %s\n",cameraConfigFilename.c_str()); printf("rectification: camera properties are\n%f\n%f\n%f\n%f\n%f\n%f\n%f\n",fxLeft,fyLeft,cxLeft,cyLeft,fxRight,fyRight,cxRight,cyRight); printf("rectification: port names are\n%s\n%s\n%s\n%s\n%s\n\n",leftInputPortName.c_str(), rightInputPortName.c_str(), leftOutputPortName.c_str(), rightOutputPortName.c_str(), robotPortName.c_str(), cameraConfigFilename.c_str() ); } /* do all initialization here */ /* open ports */ if (!leftImageIn.open(leftInputPortName.c_str())) { cout << getName() << ": unable to open port " << leftInputPortName << endl; return false; // unable to open; let RFModule know so that it won't run } if (!rightImageIn.open(rightInputPortName.c_str())) { cout << getName() << ": unable to open port " << rightInputPortName << endl; return false; // unable to open; let RFModule know so that it won't run } if (!leftImageOut.open(leftOutputPortName.c_str())) { cout << getName() << ": unable to open port " << leftOutputPortName << endl; return false; // unable to open; let RFModule know so that it won't run } if (!rightImageOut.open(rightOutputPortName.c_str())) { cout << getName() << ": unable to open port " << rightOutputPortName << endl; return false; // unable to open; let RFModule know so that it won't run } if (!robotPort.open(robotPortName.c_str())) { cout << getName() << ": Unable to open port " << robotPortName << endl; return false; } /* * attach a port of the same name as the module (prefixed with a /) to the module * so that messages received from the port are redirected to the respond method */ handlerPortName = "/"; handlerPortName += getName(); // use getName() rather than a literal if (!handlerPort.open(handlerPortName.c_str())) { cout << getName() << ": Unable to open port " << handlerPortName << endl; return false; } attach(handlerPort); // attach to port /* create the thread and pass pointers to the module parameters */ rectificationThread = new RectificationThread(&leftImageIn, &rightImageIn, &leftImageOut, &rightImageOut, &robotPort, &fxLeft, &fyLeft, &cxLeft, &cyLeft, &fxRight, &fyRight, &cxRight, &cyRight); /* now start the thread to do the work */ rectificationThread->start(); // this calls threadInit() and it if returns true, it then calls run() return true ; // let the RFModule know everything went well // so that it will then run the module }
bool 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 ; }
SimoxRobotViewer::SimoxRobotViewer(const std::string &title,yarp::os::ResourceFinder &rf) :QMainWindow(NULL) { pingValue = 0; visualisationTypeRobot = VirtualRobot::SceneObject::Full; visualizationRobotEnabled = true; viewer = NULL; sceneSep = new SoSeparator(); sceneSep->ref(); robotSep = new SoSeparator(); robotSep->ref(); reachSep = new SoSeparator(); reachSep->ref(); isClosed = false; enableViewer = true; setupUI(title); if (rf.check("SimoxDataPath")) { ConstString dataPath=rf.find("SimoxDataPath").asString(); VR_INFO << "Adding rf.SimoxDataPath: " << dataPath.c_str() << endl; VirtualRobot::RuntimeEnvironment::addDataPath(dataPath.c_str()); } if (rf.check("RobotFile")) { ConstString robotFile=rf.find("RobotFile").asString(); std::string robFileComplete = robotFile.c_str(); if (!VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFileComplete)) { VR_ERROR << " Could not find file: " << robFileComplete << endl; } else { std::cout << "Using robot file: " << robFileComplete << endl; loadRobot(robFileComplete); } } if (!robot) VR_INFO << "No robot..." << endl; if (robot && rf.check("EndEffector")) { std::string eef = rf.find("EndEffector").asString().c_str(); VR_INFO << "Selecting rf.EndEffector: " << eef << endl; selectEEF(eef); } if (robot && !currentEEF) { // select first eef std::vector<EndEffectorPtr> eefs; robot->getEndEffectors(eefs); if (eefs.size()>0) { VR_INFO << "Selecting first EEF: " << eefs[0]->getName() << endl; selectEEF(eefs[0]->getName()); } } if (!currentEEF) VR_INFO << "Skipping EEF definition..." << endl; if (robot && currentEEF && rf.check("ReachabilityFile")) { std::string reachFile = rf.find("ReachabilityFile").asString().c_str(); std::string reachFileComplete = reachFile.c_str(); if (!VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(reachFileComplete)) { VR_ERROR << " Could not find file: " << reachFileComplete << endl; } else loadReachability(reachFileComplete); } if (!reachSpace) VR_INFO << "Skipping reachability information..." << endl; if (viewer) viewer->viewAll(); SoQt::show(this); }
void AdaptiveLayer::configureOPC(yarp::os::ResourceFinder &rf) { //Populate the OPC if required cout<<"Populating OPC"; Bottle grpOPC = rf.findGroup("OPC"); bool shouldPopulate = grpOPC.find("populateOPC").asInt() == 1; if (shouldPopulate) { Bottle *agentList = grpOPC.find("agent").asList(); if (agentList) { for(int d=0; d<agentList->size(); d++) { string name = agentList->get(d).asString().c_str(); Agent* agent = iCub->opc->addOrRetrieveEntity<Agent>(name); agent->m_present = false; iCub->opc->commit(agent); } } Bottle *objectList = grpOPC.find("object").asList(); if (objectList) { for(int d=0; d<objectList->size(); d++) { string name = objectList->get(d).asString().c_str(); Object* o = iCub->opc->addOrRetrieveEntity<Object>(name); o->m_present = false; iCub->opc->commit(o); } } Bottle *rtobjectList = grpOPC.find("rtobject").asList(); if (rtobjectList) { for(int d=0; d<rtobjectList->size(); d++) { string name = rtobjectList->get(d).asString().c_str(); RTObject* o = iCub->opc->addOrRetrieveEntity<RTObject>(name); o->m_present = false; iCub->opc->commit(o); } } Bottle *adjectiveList = grpOPC.find("adjective").asList(); if (adjectiveList) { for(int d=0; d<adjectiveList->size(); d++) { string name = adjectiveList->get(d).asString().c_str(); iCub->opc->addOrRetrieveEntity<Adjective>(name); } } Bottle *actionList = grpOPC.find("action").asList(); if (actionList) { for(int d=0; d<actionList->size(); d++) { string name = actionList->get(d).asString().c_str(); iCub->opc->addOrRetrieveEntity<Action>(name); } } } cout<<"done"<<endl; }
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 ; }