/* * 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) { imp.controlled_part = rf.find("part").asString(); imp.robotname = rf.find("robot").asString(); if(imp.controlled_part == "right_arm") { imp.limb = new iCubArm("right"); imp.gcomp_port = GCOMP_PORT_RIGHT_ARM; } else if(imp.controlled_part == "left_arm") { imp.limb = new iCubArm("left"); imp.gcomp_port = GCOMP_PORT_LEFT_ARM; } else { fprintf(stderr, "part not recognised\n"); return false; } if (!imp.open()) { fprintf(stderr, "Error opening detector\n"); return false; } fflush(stdout); return true; }
qavimator::qavimator(yarp::os::ResourceFinder &config, QWidget *parent) : QMainWindow(parent), ui(new Ui::qavimator) { ui->setupUi(this); setupToolBar(); ui->animationView->init(config); nFPS=10; width=850; height=600; readSettings(); // default size if (config.check("name")) { GUI_NAME=std::string(config.find("name").asString().c_str()); } if (GUI_NAME[0]!='/') { GUI_NAME=std::string("/")+GUI_NAME; } if (config.check("width")) { width=config.find("width").asInt(); } if (config.check("height")) { height=config.find("height").asInt(); } //sanity check if(width<100) { width=100; } if(height<100) { height=100; } this->resize(width, height); int xpos=32,ypos=32; if (config.check("xpos")) { xpos=config.find("xpos").asInt(); } if (config.check("ypos")) { ypos=config.find("ypos").asInt(); } this->move(xpos,ypos); setWindowTitle(GUI_NAME.c_str()); connect(ui->animationView,SIGNAL(backgroundClicked()),this,SLOT(backgroundClicked())); connect(this,SIGNAL(resetCamera()),ui->animationView,SLOT(resetCamera())); ui->animationView->startTimer(1000/nFPS); }
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; }
bool AudioPowerMapModule::configure(yarp::os::ResourceFinder &rf) { yInfo("Configuring the module"); // get the module name which will form the stem of all module port names moduleName = rf.check("name", Value("/AudioPowerMap"), "module name (string)").asString(); // before continuing, set the module name before getting any other parameters, // specifically the port names which are dependent on the module name setName(moduleName.c_str()); // get the robot name which will form the stem of the robot ports names // and append the specific part and device required robotName = rf.check("robot", Value("icub"), "Robot name (string)").asString(); robotPortName = "/" + robotName + "/head"; inputPortName = rf.check("inputPortName", Value(":i"), "Input port name (string)").asString(); // attach a port of the same name as the module (prefixed with a /) to the module // so that messages received from the port are redirected to the respond method handlerPortName = ""; handlerPortName += getName(); if (!handlerPort.open(handlerPortName.c_str())) { std::cout << getName() << ": Unable to open port " << handlerPortName << std::endl; return false; } // attach to port attach(handlerPort); if (rf.check("config")) { configFile=rf.findFile(rf.find("config").asString().c_str()); if (configFile=="") { return false; } } else { configFile.clear(); } // create the thread and pass pointers to the module parameters apr = new AudioPowerMapRatethread(robotName, configFile, rf); apr->setName(moduleName); // now start the thread to do the work // this calls threadInit() and it if returns true, it then calls run() bool ret = apr->start(); // let the RFModule know everything went well // so that it will then run the module return ret; }
bool Module::configure(yarp::os::ResourceFinder &rf) { // Parse the configuration file. controller_options.robotName = rf.check("robot") ? rf.find("robot").asString().c_str() : "icub"; controller_options.threadPeriod = rf.check("threadPeriod") ? rf.find("threadPeriod").asInt() : DEFAULT_THREAD_PERIOD; dangerPeriodLoopTime = 1.3 * controller_options.threadPeriod; controller_options.serverName = rf.check("local") ? rf.find("local").asString().c_str() : "OcraControllerServer"; controller_options.runInDebugMode = rf.check("debug"); controller_options.noOutputMode = rf.check("noOutput"); controller_options.isFloatingBase = rf.check("floatingBase"); controller_options.useOdometry = rf.check("useOdometry"); controller_options.idleAnkles = rf.check("idleAnkles"); if ( controller_options.idleAnkles ) { if ( !rf.find("idleAnkles").isNull() ) { controller_options.idleAnkleTime = rf.find("idleAnkles").asDouble(); if ( controller_options.idleAnkleTime < 0.0 ) { controller_options.idleAnkleTime = 0.0; OCRA_WARNING("Ankle idling time must be > 0. Setting to 0.0s.") } }
void AdaptiveLayer::configureSpeech(yarp::os::ResourceFinder &rf) { //Port for broadcasting recognized keywords to other modules //pSpeechRecognizerKeywordOut.open("/"+getName()+"/speechGrammar/keyword:o"); //Set the tts options string ttsOptions = rf.check("ttsOptions",Value("iCub")).asString().c_str(); iCub->getSpeechClient()->SetOptions(ttsOptions); //Populate the speech reco if needed bool shouldPopulateGrammar = rf.find("shouldPopulateGrammar").asInt() == 1; if (shouldPopulateGrammar) populateSpeechRecognizerVocabulary(); }
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; }
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 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 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 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 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; }
/** * @brief constructor of the generic thread. * * @param module_prefix module name. * @param thread_period period of the run thread in millisecond. * @param rf resource finder. **/ generic_thread( std::string module_prefix, yarp::os::ResourceFinder rf, std::shared_ptr<paramHelp::ParamHelperServer> ph ): module_prefix( module_prefix ), thread_period( rf.find("thread_period").asInt() ), robot_name( rf.find("robot_name").asString() ), rf( rf ), ph( ph ), RateThread( rf.find("thread_period").asInt() ) { }
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 ImageSplitter::configure(yarp::os::ResourceFinder &rf) { // Check input parameters if(rf.check("align")) { yarp::os::ConstString align = rf.find("align").asString(); if(align == "vertical") { horizontal = false; } else if(align == "horizontal") { horizontal = true; } else { yError() << " Incorrect parameter. Supported values for alignment are 'horizontal' or 'vertical'"; return false; } } yarp::os::ConstString inputPortName = "/imageSplitter/input:i"; yarp::os::ConstString outLeftPortName = "/imageSplitter/left:o"; yarp::os::ConstString outRightPortName = "/imageSplitter/right:o"; if(rf.check("nameLeft")) { outLeftPortName = rf.find("nameLeft").asString(); } if(rf.check("nameRight")) { outRightPortName = rf.find("nameRight").asString(); } if(!rf.check("remote")) { yError() << "Missing 'remote' parameter. Please insert the name of the port to connect to."; return false; } yarp::os::ConstString remotePortName = rf.find("remote").asString(); // opening ports bool ret=true; ret &= inputPort.open(inputPortName); ret &= outLeftPort.open(outLeftPortName); ret &= outRightPort.open(outRightPortName); if(!ret) { yError() << " Cannot open ports"; return false; } // Connections if(! yarp::os::Network::connect(remotePortName, inputPortName)) { yError() << "Cannot connect to remote port " << remotePortName; return false; } // choose filling method if(rf.check("m")) { yarp::os::ConstString align = rf.find("m").asString(); if(align == "pixel") { method = 0; } else if(align == "pixel2") { method = 1; } else if(align == "line") { method = 2; } else if(align == "whole") { if(horizontal) yError() << "Cannot use 'whole' method for input image horizontally aligned"; method = 3; } else { yError() << "Methods are pixel, line, whole; got " << align; return false; } } yInfo() << "using method " << method; return true; }
bool EdisonSegmModule::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("edisonSeg"); //defaults for the parameters - as the ones in the EDISON GUI application height_ = 240; width_ = 320; dim_ = 3; sigmaS = 7; sigmaR = 6.5; minRegion = 20; gradWindRad = 2; threshold = 0.3; mixture = 0.2; speedup = MED_SPEEDUP; //override defaults if specified - TODO: range checking if(rf.check("height")) height_ = rf.find("height").asInt(); if(rf.check("width")) width_ = rf.find("width").asInt(); //if(rf.check("dim")) dim_ = rf.find("dim").asInt(); // not required - should always be 3 if(rf.check("sigmaS")) sigmaS = rf.find("sigmaS").asInt(); if(rf.check("sigmaR")) sigmaR = rf.find("sigmaR").asDouble(); if(rf.check("minRegion")) minRegion = rf.find("minRegion").asInt(); if(rf.check("gradWinRad")) gradWindRad = rf.find("gradWinRad").asDouble(); if(rf.check("threshold")) threshold = rf.find("threshold").asDouble(); if(rf.check("mixture")) mixture = rf.find("mixture").asDouble(); if(rf.check("speedup")) { int spdp = rf.find("speedup").asInt(); setSpeedUpValue(spdp); } // name of the camera calibration file // ConstString strCamConfigPath=rf.findFile("camera"); std::string slash="/"; _imgPort.open((slash + getName("/rawImg:i").c_str()).c_str()); _configPort.open((slash + getName("/conf").c_str()).c_str()); _filtPort.open((slash + getName("/filtImg:o").c_str()).c_str()); _labelPort.open((slash +getName("/labeledImg:o").c_str()).c_str()); _viewPort.open((slash +getName("/viewImg:o").c_str()).c_str()); _rawPort.open((slash +getName("/rawImg:o").c_str()).c_str()); _labelViewPort.open((slash +getName("/debugImg:o").c_str()).c_str()); //attach(_configPort, true); 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; orig_height_ = yrpImgIn->height(); orig_width_ = yrpImgIn->width(); //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; }
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); }
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 eventDrivenModule::configure(yarp::os::ResourceFinder &rf) { /* Process all parameters from both command-line and .ini file */ /* get the module name which will form the stem of all module port names */ moduleName = rf.check("name", Value("/eventDriven"), "module name (string)").asString(); /* * before continuing, set the module name before getting any other parameters, * specifically the port names which are dependent on the module name */ setName(moduleName.c_str()); /* * get the robot name which will form the stem of the robot ports names * and append the specific part and device required */ robotName = rf.check("robot", Value("icub"), "Robot name (string)").asString(); robotPortName = "/" + robotName + "/head"; inputPortName = rf.check("inputPortName", Value("/AER:i"), "Input port name (string)").asString(); /* * attach a port of the same name as the module (prefixed with a /) to the module * so that messages received from the port are redirected to the respond method */ handlerPortName = ""; handlerPortName += getName(); // use getName() rather than a literal if (!handlerPort.open(handlerPortName.c_str())) { cout << getName() << ": Unable to open port " << handlerPortName << endl; return false; } attach(handlerPort); // attach to port if (rf.check("config")) { configFile=rf.findFile(rf.find("config").asString().c_str()); if (configFile=="") { return false; } } else { configFile.clear(); } /* create the thread and pass pointers to the module parameters */ edThread = new eventDrivenThread(robotName, configFile); edThread->setName(getName().c_str()); edThread->setInputPortName(inputPortName.c_str()); /* now start the thread to do the work */ edThread->start(); // this calls threadInit() and it if returns true, it then calls run() return true ; // let the RFModule know everything went well // so that it will then run the module }
bool quaternionEKFModule::configure ( yarp::os::ResourceFinder& rf ) { if( rf.check("robot") ) { robotName = rf.find("robot").asString(); } else { yError("[quaternionEKFModule::configure] Configuration failed! No robot param was found"); return false; } if( rf.check("rate") && rf.find("rate").asDouble() ) { period = rf.find("rate").asDouble(); } else { yError("[quaternionEKFModule::configure] Configuration failed. No rate was specified. Remember it should be a double."); return false; } if( rf.check("local") ) { local = rf.find("local").asString(); } else { yError("[quaternionEKFModule::configure] Configuration failed. No local name was foundd."); return false; } if( rf.check("autoconnect") ) { autoconnect = rf.find("autoconnect").asBool(); } else { yError("[quaternionEKFModule::configure] Configuration failed. No value for autoconnect was found."); return false; } if( rf.check("mode") ) { mode = rf.find("mode").asString(); } else { yError("[quaternionEKFModule::configure] Configuration failed. No value for mode was found."); return false; } if ( rf.check("usingXSens") ) { usingxsens = rf.find("usingXSens").asBool(); } else { yError ("[quaternionEKFModule::configure] Configuration failed. No value for usingXSens was found."); return false; } if ( rf.check("verbose") ) { verbose = rf.find("verbose").asBool(); } else { yError ("[quaternionEKFModule::configure] Configuration failed. No value for verbose was found."); return false; } if ( rf.check("sensorPortName") ) { sensorPortName = rf.find("sensorPortName").asString(); } else { yError ("[quaternionEKFModule::configure] Configuration failed. No value for sensorPortName was found"); return false; } if ( rf.check("usingEKF") ) { usingEKF = rf.find("usingEKF").asBool(); } else { yError ("[quaternionEKFModule::configure] Configuration failed. No value for usingEKF was found."); return false; } if (rf.check("usingSkin")) { usingSkin = rf.find("usingSkin").asBool(); } else { yError ("[quaternionEKFModule::configure] Configuration failed. No value for usingSkin was found."); return false; } if (rf.check("inWorldRefFrame")) { inWorldRefFrame = rf.find("inWorldRefFrame").asBool(); } else { yError ("[quaternionEKFModule::configure] Configuration failed. No value for inWorldRefFrame was found."); return false; } if (rf.check("gravityVec")) { gravityVec = rf.find("gravityVec").asDouble(); } else { yError ("[quaternionEKFModule::configure] Configuration failed. No value for gravityVec was found."); return false; } if (rf.check("debugGyro")) { debugGyro = rf.find("debugGyro").asBool(); } else { yError ("[quaternionEKFModule::configure] Configuration failed. No value for debugGyro was found. "); return false; } if (rf.check("debugAcc")) { debugAcc = rf.find("debugAcc").asBool(); } else { yError ("[quaternionEKFModule::configure] Configuration failed. No value for debugAcc was found. "); return false; } if (rf.check("using2acc")) { using2acc = rf.find("using2acc").asBool(); } else { yError ("[quaternionEKFModule::configure] Configuration failed. No value for using2acc was found."); return false; } if ( rf.check("calib") ) { calib = rf.find("calib").asBool(); } else { yError ("[quaternionEKFModule::configure] Configuration failed. No value for calib was found."); return false; } // ------------ IMU PORT --------------------------------------- /*TODO This should be configurable! The number of input ports depending on the amount of sensor readings.*/ std::string tmpOffline = "offline"; std::string tmpOnline = "online"; // If the estimate is done online if (!calib) { if (!tmpOnline.compare(mode)) { yInfo(" [quaternionEKFModule::configure] Online estimation will be performed"); std::string gyroMeasPortName = "/"; gyroMeasPortName += local; gyroMeasPortName += "/imu:i"; if (!gyroMeasPort.open(gyroMeasPortName.c_str())) { yError("[quaternionEKFModule::configure] Could not open gyroMeasPort"); return false; } // If using two accelerometers, open another port for the second reading if (using2acc) { std::string gyroMeasPortName2 = "/"; gyroMeasPortName2 += local; gyroMeasPortName2 += "/imu2:i"; if (!gyroMeasPort2.open(gyroMeasPortName2.c_str())) { yError("[quaternionEKFModule::configure] Coult not open gyroMeasPort2"); return false; } } // Obtaining filter parameters from configuration file yarp::os::Property filterParams; // If using Direct method with atan2 if(!usingEKF) { if( !rf.check(DIRECT_GROUP_PARAMS_NAME) ) { yError("[quaternionEKFModule::configure] Could not load DIRECT-FILTER-PARAMS group from config file"); return false; } else { filterParams.fromString(rf.findGroup(DIRECT_GROUP_PARAMS_NAME).tail().toString()); yInfo(" [quaternionEKFModule::configure] Filter parameters are: %s ", filterParams.toString().c_str()); } } else { if( !rf.check(FILTER_GROUP_PARAMS_NAME) ) { yError("[quaternionEKFModule::configure] Could not load EKF-PARAMS group from config file"); return false; } else { filterParams.fromString(rf.findGroup(FILTER_GROUP_PARAMS_NAME).tail().toString()); yInfo(" [quaternionEKFModule::configure] Filter parameters are: %s ", filterParams.toString().c_str()); } } // ----------- THREAD INSTANTIATION AND CALLING ----------------- quatEKFThread = new quaternionEKFThread(period, local, robotName, autoconnect, usingxsens, usingEKF, inWorldRefFrame, gravityVec, usingSkin, sensorPortName, debugGyro, debugAcc, verbose, filterParams, &gyroMeasPort, &gyroMeasPort2); if (!quatEKFThread->start()) { yError("Error starting quaternionEKFThread!"); return false; } yInfo(" [quaternionEKFModule::configure] quaternionEKFThread started"); } else { // If the estimate is done offline, read from file with a datadumper format and don't create the thread. if(!tmpOffline.compare(mode)) { yInfo(" [quaternionEKFModule::configure] Offline batch estimation will be performed"); // **** Initialization // Create dataDumper parser m_parser = new dataDumperParser(DATAFILE); m_parser->parseFile(); m_parser->countLines(); // Change period of the module thread } else { yError("[quaternionEKFModule::configure] An invalid option was passed to 'mode'. Available options are 'offline' or 'online'."); return false; } } } else { std::cout << "Calibrating only... Done bby updateModule()" << std::endl; } return true; }