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 ;
}
