virtual bool configure(ResourceFinder &rf)
{
Time::turboBoost();
//check if the yarp networ is running
if (yarp.checkNetwork()==false)
{
return false;
}
moduleName = rf.check("name", Value(1), "module name (string)").asString();
setName(moduleName.c_str());
period = rf.check("period", Value(5000), "update period (int)").asInt();
string pname = "/" + moduleName + ":o";
monitorOutput.open(pname.c_str());
picBlocks = rf.findFile(rf.check("pic_blocks", Value(1), "module name (string)").asString());
picBackground = rf.findFile(rf.check("pic_background", Value(1), "module name (string)").asString());
picNumbers = rf.findFile(rf.check("pic_numbers", Value(1), "module name (string)").asString());
graphics = new GraphicsManager(picBackground.c_str(),picBlocks.c_str(),picNumbers.c_str());
m_timer = Glib::signal_timeout().connect(sigc::mem_fun(*this, &CtrlModule::on_timeout), period);
on_timeout();
//start GTK loop
gtk_main->run(*graphics);
return true;
}
void testBasics() {
report(0,"testing the basics of RF...");
ResourceFinder rf;
const char *fname0 = "_yarp_regression_test.ini";
const char *fname1 = "_yarp_regression_test_rf1.txt";
const char *fname2 = "_yarp_regression_test_rf2.txt";
// create some test files
{
FILE *fout = fopen(fname0,"w");
yAssert(fout!=NULL);
fprintf(fout,"style capability\n");
fprintf(fout,"capability_directory \".\"\n");
fprintf(fout,"default_capability \".\"\n");
fclose(fout);
fout = NULL;
fout = fopen(fname1,"w");
yAssert(fout!=NULL);
fprintf(fout,"alt %s\n", fname2);
fclose(fout);
fout = NULL;
fout = fopen(fname2,"w");
fprintf(fout,"x 14\n");
fclose(fout);
fout = NULL;
const char *argv[] = { "ignore",
"--_yarp_regression_test", ".",
"--from", fname1,
"--verbose", "0",
NULL };
int argc = 7;
rf.configure(argc,(char **)argv);
ConstString alt = rf.findFile("alt");
checkTrue(alt!="","found ini file");
rf.setDefault("alt2",fname2);
alt = rf.findFile("alt2");
checkTrue(alt!="","default setting worked");
rf.setDefault("alt3","_yarp_nonexistent.txt");
alt = rf.findFile("alt3");
checkTrue(alt=="","cannot find nonexistent files");
rf.setDefault("alt","_yarp_nonexistent.txt");
alt = rf.findFile("alt");
checkTrue(alt!="","default setting is safe");
checkTrue(rf.findPath()!="","existing path found");
alt=rf.findFileByName(fname1);
checkTrue(alt!="","found file by name");
}
}
bool configure(ResourceFinder &rf)
{
string name=rf.find("name").asString().c_str();
setName(name.c_str());
Property config; config.fromConfigFile(rf.findFile("from").c_str());
Bottle &bGeneral=config.findGroup("general");
if (bGeneral.isNull())
{
cout<<"Error: group general is missing!"<<endl;
return false;
}
// parsing general config options
Property option(bGeneral.toString().c_str());
option.put("local",name.c_str());
option.put("part","left_arm");
option.put("grasp_model_type",rf.find("grasp_model_type").asString().c_str());
option.put("grasp_model_file",rf.findFile("grasp_model_file").c_str());
option.put("hand_sequences_file",rf.findFile("hand_sequences_file").c_str());
// parsing arm dependent config options
Bottle &bArm=config.findGroup("arm_dependent");
getArmDependentOptions(bArm,graspOrien,graspDisp,dOffs,dLift,home_x);
cout<<"***** Instantiating primitives for left arm"<<endl;
action=new AFFACTIONPRIMITIVESLAYER(option);
if (!action->isValid())
{
delete action;
return false;
}
deque<string> q=action->getHandSeqList();
cout<<"***** List of available hand sequence keys:"<<endl;
for (size_t i=0; i<q.size(); i++)
cout<<q[i]<<endl;
string fwslash="/";
inPort.open((fwslash+name+"/in").c_str());
rpcPort.open((fwslash+name+"/rpc").c_str());
attach(rpcPort);
// check whether the grasp model is calibrated,
// otherwise calibrate it
Model *model; action->getGraspModel(model);
if (model!=NULL)
{
if (!model->isCalibrated())
{
Property prop;
prop.put("finger","all");
model->calibrate(prop);
}
}
return true;
}
bool ActiveSeg::configure(ResourceFinder &rf)
{
string texture13=rf.findFile("texture_filterbank_13.txt").c_str();
string texture19=rf.findFile("texture_filterbank_19.txt").c_str();
string textonsFile=rf.findFile("textons.txt").c_str();
fileLoc.put("texture_13", texture13);
fileLoc.put("texture_19", texture19);
fileLoc.put("textons", textonsFile);
return true;
}
//Function to setup OpenGl
bool setup_opengl(ResourceFinder& finder){
glShadeModel( GL_SMOOTH );
// Culling
glCullFace( GL_BACK );
glFrontFace( GL_CCW );
glEnable( GL_CULL_FACE );
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LESS);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0); // OpenGL light
glEnable(GL_NORMALIZE);
glEnable(GL_COLOR_MATERIAL);
glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
//glLightfv(GL_LIGHT1, GL_POSITION, light_position );
/// Some OpenGL settings
GLfloat light_color[] = {0.0,0.0,0.0,1.0}; // colour
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, light_color);
//glMaterialfv(GL_FRONT,GL_DIFFUSE,light_color); // colour
glClearColor(0.0,0.0,0.0,0); // background color
string floor = finder.findFile("floor");
Texture[0] = LoadTextureRAW( floor.c_str(), false );
string body1 = finder.findFile("body1");
Texture[1] = LoadTextureRAW( body1.c_str(), false );
string body2 = finder.findFile("body2");
Texture[2] = LoadTextureRAW( body2.c_str(), false );
string skybox_ft = finder.findFile("skybox_ft");
Texture[3] = LoadTextureRAW( skybox_ft.c_str(), false );
string skybox_bk = finder.findFile("skybox_bk");
Texture[4] = LoadTextureRAW( skybox_bk.c_str(), false );
string skybox_lt = finder.findFile("skybox_lt");
Texture[5] = LoadTextureRAW( skybox_lt.c_str(), false );
string skybox_rt = finder.findFile("skybox_rt");
Texture[6] = LoadTextureRAW( skybox_rt.c_str(), false );
string skybox_up = finder.findFile("skybox_up");
Texture[7] = LoadTextureRAW( skybox_up.c_str(), false );
string face = finder.findFile("face");
Texture[8] = LoadTextureRAW( face.c_str(), false );
if (!Texture[1]){
yError("No texture loaded\n");
return false;
}
return true;
}
bool BotRpcServerModule::configure(ResourceFinder &rf)
{
module_name_ = rf.check("name", Value("botRpcServer"), "module name (string)").asString();
setName(module_name_.c_str());
dictionary_config_filename_ = rf.check("dictionaryConfig", Value(""), "dictionary configuration filename (string)").asString();
if (dictionary_config_filename_ != "")
{
dictionary_config_filename_ = (rf.findFile(dictionary_config_filename_.c_str())).c_str();
if (dictionary_.fromConfigFile(dictionary_config_filename_.c_str()) == false) {
cerr << "botRpcServerModule: unable to read dictionary configuration file" << dictionary_config_filename_;
return false;
}
}
// get the name of the input and output ports, automatically prefixing the module name by using getName()
port_name_ = "/";
port_name_ += getName(rf.check("port", Value("/rpc:io"), "Rpc port (string)").asString());
// open ports
if (!rpc_port_.open(port_name_.c_str())) {
cerr << getName() << ": unable to open port " << port_name_ << endl;
return false;
}
default_answer_ = rf.check("defaultAnswer", Value("ack"), "Default answer (string)").asString();
delay_ = rf.check("delay", Value("0"), "Delay (double)").asDouble();
// create the thread and pass pointers to the module parameters
thread_ = new BotRpcServerThread(&rpc_port_, &default_answer_, &dictionary_, delay_);
// now start the thread to do the work
thread_->start(); // this calls threadInit() and it if returns true, it then calls run()
return true ;
}
bool FingersModelModule::configure(ResourceFinder &rf)
{
module_name_ = rf.check("name", Value("fingersModel"), "module name (string)").asString();
setName(module_name_.c_str());
// get the name of the input and output ports, automatically prefixing the module name by using getName()
springy_port_name_ = "/";
springy_port_name_ += getName(rf.check("springy_port", Value("/springy:o"), "Springy port (string)").asString());
// open ports
if (!springy_port_.open(springy_port_name_.c_str())) {
cerr << getName() << ": unable to open port " << springy_port_name_ << endl;
return false;
}
double default_period = 1.0 / 30 * 1000;
period_ = rf.check("period", Value(default_period), "Grasp duration (double)").asDouble();
model_config_filename_ = rf.check("modelConf", Value("grasp_model_left.ini"), "Model config filename (string)").asString();
cerr << model_config_filename_ << endl;
// create the thread and pass pointers to the module parameters
thread_ = new FingersModelThread(period_, &springy_port_, rf.findFile(model_config_filename_));
// now start the thread to do the work
thread_->start(); // this calls threadInit() and it if returns true, it then calls run()
return true ;
}
bool configure(ResourceFinder &rf)
{
string name="demoActionRecognition";
string rpcName="/"+name+"/rpc";
string outExecName="/"+name+"/exec:o";
string inName="/"+name+"/scores:i";
string outGestRecModName="/"+name+"/gestRec:o";
rpc.open(rpcName.c_str());
attach(rpc);
outExecModule.open(outExecName.c_str());
inSequence.open(inName.c_str());
string outspeakname="/"+name+"/outspeak";
outspeak.open(outspeakname.c_str());
outGestRecModule.open(outGestRecModName.c_str());
string filename=rf.findFile("actions").c_str();
Property config; config.fromConfigFile(filename.c_str());
Bottle& bgeneral=config.findGroup("general");
numActions=bgeneral.find("numActions").asInt();
sequence="";
currentSequence="";
myturn=false;
yourturn=false;
gameEnding=true;
count=0;
temp=0;
return true;
}
void TouchDetectorModule::initializeParameters(ResourceFinder &rf)
{
moduleName = rf.check("name", Value("touchDetector"), "Module name (string)").asString();
setName(moduleName.c_str());
period = rf.check("period", Value(1000 / 30), "Thread period (string)").asInt();
threshold = rf.check("threshold", Value(50), "Activation threshold (int)").asInt();
rf.setDefault("clustersConfFile", Value("clustersConfig.ini"));
clustersConfFilepath = rf.findFile("clustersConfFile");
// get the name of the input and output ports, automatically prefixing the module name by using getName()
torsoPortName = "/";
torsoPortName += getName(rf.check("torsoPort", Value("/torso:i"), "Torso input port (string)").asString());
leftArmPortName = "/";
leftArmPortName += getName(rf.check("leftArmPort", Value("/left_arm:i"), "Left arm input port (string)").asString());
rightArmPortName = "/";
rightArmPortName += getName(rf.check("rightArmPort", Value("/right_arm:i"), "Right arm input port (string)").asString());
leftForearmPortName = "/";
leftForearmPortName += getName(rf.check("leftForearmPort", Value("/left_forearm:i"), "Left forearm input port (string)").asString());
rightForearmPortName = "/";
rightForearmPortName += getName(rf.check("rightForearmPort", Value("/right_forearm:i"), "Right forearm input port (string)").asString());
leftHandPortName = "/";
leftHandPortName += getName(rf.check("leftHandPort", Value("/left_hand:i"), "Left hand input port (string)").asString());
rightHandPortName = "/";
rightHandPortName += getName(rf.check("rightHandPort", Value("/right_hand:i"), "Right hand input port (string)").asString());
touchPortName = "/";
touchPortName += getName(rf.check("touchPort", Value("/touch:o"), "Touch output port (string)").asString());
}
int main(int argc, char *argv[])
{
Network yarp;
ResourceFinder rf;
rf.setVerbose();
rf.setDefaultConfigFile("or.ini");
rf.setDefaultContext("orBottle");
rf.configure(argc, argv);
std::string robotName=rf.find("robot").asString();
std::string model=rf.findFile("model");
cout<<"Running with:"<<endl;
cout<<"robot: "<<robotName.c_str()<<endl;
if (model=="")
{
cout<<"Sorry no model was found, check config parameters"<<endl;
return -1;
}
cout << "Using object model: " << model.c_str() << endl;
int times=4;
while(times--)
{
cout << ".";
Time::delay(0.5);
}
cout << "done!" <<endl;
return 0;
}
HOGThread::HOGThread(ResourceFinder &rf) : RateThread(20)
{
initPosition=NULL;
frameDiff=NULL;
this->levels=(rf.check("HOGlevels",Value(3)).asInt())-1;
this->nbins=rf.check("HOGnbins",Value(128)).asInt();
this->useDictionary=rf.check("dictionary");
string pooling=rf.check("pool",Value("concatenate")).asString().c_str();
if (pooling=="max")
this->pool=MAXPOOLING;
else
this->pool=CONCATENATE;
//string contextPath=rf.getHomeContextPath().c_str();
//string dictionary_name=rf.check("dictionary_file",Value("dictionary_hog.ini")).asString().c_str();
if (useDictionary)
{
string dictionary_path=rf.findFile("dictionary_hog").c_str();
string dictionary_group=rf.check("dictionary_group",Value("DICTIONARY")).asString().c_str();
sparse_coder=new DictionaryLearning(dictionary_path,dictionary_group);
}
work=false;
done=true;
}
int main(int argc, char **argv)
{
Network yarp;
ResourceFinder rf;
rf.setVerbose();
rf.setDefaultConfigFile("or.ini");
rf.setDefaultContext("traza/orBottle");
rf.configure("ICUB_ROOT", argc, argv);
ConstString robotName=rf.find("robot").asString();
ConstString model=rf.findFile("model");
cout<<"Running with:"<<endl;
cout<<"robot: "<<robotName.c_str()<<endl;
if (model=="")
{
cout<<"Sorry no model was found, check config parameters"<<endl;
// return -1;
}
cout << "Using object model: " << model.c_str() << endl;
if (!yarp.checkNetwork())
{
printf("No yarp network, quitting\n");
return false;
}
FuserThread* fuserThread = new FuserThread(10);
cout << "----------------------> going to call fuserthread..." << endl;
fuserThread->start();
//fuserThread->wait();
while(true)
{
//if ((Time::now()-startTime)>5)
//done=true;
}
cout << "main.cpp...returning 0" << endl;
return 0;
}
bool configure(ResourceFinder &rf)
{
string taxelPosFile="";
string filePath="";
int verbosity = rf.check("verbosity",Value(0)).asInt();
//************* skinTaxels' Resource finder **************
ResourceFinder skinRF;
skinRF.setVerbose(false);
skinRF.setDefaultContext("skinGui"); //overridden by --context parameter
skinRF.setDefaultConfigFile("skinManForearms.ini"); //overridden by --from parameter
skinRF.configure(0,NULL);
Bottle &skinEventsConf = skinRF.findGroup("SKIN_EVENTS");
if(!skinEventsConf.isNull())
{
if(skinEventsConf.check("skinParts"))
{
Bottle* skinPartList = skinEventsConf.find("skinParts").asList();
}
if(skinEventsConf.check("taxelPositionFiles"))
{
Bottle *taxelPosFiles = skinEventsConf.find("taxelPositionFiles").asList();
taxelPosFile = taxelPosFiles->get(1).asString().c_str();
filePath = skinRF.findFile(taxelPosFile.c_str());
}
}
else
{
yError(" No skin configuration files found.");
return 0;
}
// iCub::skinDynLib::skinPart sP;
// sP.setTaxelPosesFromFile(filePath);
// sP.print(1);
iCub::skinDynLib::iCubSkin iCS;
// iCub::skinDynLib::iCubSkin iCS("skinManForearms.ini","skinGui");
iCS.print(verbosity);
return true;
}
int main(int argc, char ** argv) {
ResourceFinder rf;
rf.setVerbose(true);
rf.setDefaultContext("cartesianForceWBC/conf");
rf.setDefaultConfigFile("cartesianForceWBC.ini");
rf.configure("ICUB_ROOT",argc,argv);
/*
Network yarp;
if (!Network::checkNetwork()) {
printf("Please start a yarp name server first\n");
return(-1);
}
Property options;
options.put("device","remote_controlboard");
options.put("remote","/icubSim/left_arm");
options.put("local","/local");
PolyDriver dd(options);
if(!dd.isValid()) {
printf("ravebot device not available.\n");
dd.close();
Network::fini();
return 1;
}
dd.view(pos);
*/
ConstString saixml=rf.findFile("saixml");
const char *c_saixml(saixml.c_str());
printf("Loading SAIxml from file '%s'...\n",c_saixml);
try {
model.reset(jspace::test::parse_sai_xml_file(c_saixml, true));
jtask.reset(new tut03::JTask());
}
catch (std::runtime_error const & ee) {
errx(EXIT_FAILURE, "%s", ee.what());
}
tutsim::set_draw_cb(draw_cb);
tutsim::set_robot_filename(c_saixml);
return tutsim::run(servo_cb);
}
bool handle_ctp_file(const Bottle &cmd, Bottle &reply)
{
if (cmd.size()<2)
return false;
string fileName=rf->findFile(cmd.get(1).asString());
bool ret = velThread.go(fileName);
if (ret)
{
reply.addVocab(Vocab::encode("ack"));
}
else
{
reply.addVocab(Vocab::encode("nack"));
reply.addString("Unable to load file");
}
return ret;
}
bool configure(ResourceFinder &rf)
{
name = "visuoTactileRF";
robot = "icub";
modality = "1D";
verbosity = 0; // verbosity
rate = 20; // rate of the vtRFThread
//******************************************************
//********************** CONFIGS ***********************
//******************* NAME ******************
if (rf.check("name"))
{
name = rf.find("name").asString();
yInfo("Module name set to %s", name.c_str());
}
else yInfo("Module name set to default, i.e. %s", name.c_str());
setName(name.c_str());
//******************* ROBOT ******************
if (rf.check("robot"))
{
robot = rf.find("robot").asString();
yInfo("Robot is %s", robot.c_str());
}
else yInfo("Could not find robot option in the config file; using %s as default",robot.c_str());
//***************** MODALITY *****************
if (rf.check("modality"))
{
modality = rf.find("modality").asString();
yInfo("modality is %s", modality.c_str());
}
else yInfo("Could not find modality option in the config file; using %s as default",modality.c_str());
//******************* VERBOSE ******************
if (rf.check("verbosity"))
{
verbosity = rf.find("verbosity").asInt();
yInfo("vtRFThread verbosity set to %i", verbosity);
}
else yInfo("Could not find verbosity option in config file; using %i as default",verbosity);
//****************** rate ******************
if (rf.check("rate"))
{
rate = rf.find("rate").asInt();
yInfo("vtRFThread working at %i ms", rate);
}
else yInfo("Could not find rate in the config file; using %i ms as default",rate);
//************* skinTaxels' Resource finder **************
ResourceFinder skinRF;
skinRF.setVerbose(false);
skinRF.setDefaultContext("skinGui"); //overridden by --context parameter
skinRF.setDefaultConfigFile("skinManForearms.ini"); //overridden by --from parameter
skinRF.configure(0,NULL);
vector<string> filenames;
int partNum=4;
Bottle &skinEventsConf = skinRF.findGroup("SKIN_EVENTS");
if(!skinEventsConf.isNull())
{
yInfo("SKIN_EVENTS section found\n");
if(skinEventsConf.check("skinParts"))
{
Bottle* skinPartList = skinEventsConf.find("skinParts").asList();
partNum=skinPartList->size();
}
if(skinEventsConf.check("taxelPositionFiles"))
{
Bottle *taxelPosFiles = skinEventsConf.find("taxelPositionFiles").asList();
if (rf.check("leftHand") || rf.check("leftForeArm") || rf.check("rightHand") || rf.check("rightForeArm"))
{
if (rf.check("leftHand"))
{
string taxelPosFile = taxelPosFiles->get(0).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",0,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("leftForeArm"))
{
string taxelPosFile = taxelPosFiles->get(1).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",1,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("rightHand"))
{
string taxelPosFile = taxelPosFiles->get(2).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",2,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("rightForeArm"))
{
string taxelPosFile = taxelPosFiles->get(3).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",3,filePath.c_str());
filenames.push_back(filePath);
}
}
}
else
{
for(int i=0;i<partNum;i++) // all of the skinparts
{
string taxelPosFile = taxelPosFiles->get(i).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",i,filePath.c_str());
filenames.push_back(filePath);
}
}
}
}
}
else
{
yError(" No skin's configuration files found.");
return 0;
}
//*************** eyes' Resource finder ****************
ResourceFinder gazeRF;
gazeRF.setVerbose(verbosity!=0);
gazeRF.setDefaultContext("iKinGazeCtrl");
robot=="icub"?gazeRF.setDefaultConfigFile("config.ini"):gazeRF.setDefaultConfigFile("configSim.ini");
gazeRF.configure(0,NULL);
double head_version=gazeRF.check("head_version",Value(1.0)).asDouble();
ResourceFinder eyeAlignRF;
Bottle &camerasGroup = gazeRF.findGroup("cameras");
if(!camerasGroup.isNull())
{
eyeAlignRF.setVerbose(verbosity!=0);
camerasGroup.check("context")?
eyeAlignRF.setDefaultContext(camerasGroup.find("context").asString().c_str()):
eyeAlignRF.setDefaultContext(gazeRF.getContext().c_str());
eyeAlignRF.setDefaultConfigFile(camerasGroup.find("file").asString().c_str());
eyeAlignRF.configure(0,NULL);
}
else
{
yWarning("Did not find camera calibration group into iKinGazeCtrl ResourceFinder!");
}
//******************************************************
//*********************** THREAD **********************
if( filenames.size() > 0 )
{
vtRFThrd = new vtRFThread(rate, name, robot, modality, verbosity, rf,
filenames, head_version, eyeAlignRF);
if (!vtRFThrd -> start())
{
delete vtRFThrd;
vtRFThrd = 0;
yError("vtRFThread wasn't instantiated!!");
return false;
}
yInfo("VISUO TACTILE RECEPTIVE FIELDS: vtRFThread instantiated...");
}
else {
vtRFThrd = 0;
yError("vtRFThread wasn't instantiated. No filenames have been given!");
return false;
}
//******************************************************
//************************ PORTS ***********************
rpcClnt.open(("/"+name+"/rpc:o").c_str());
rpcSrvr.open(("/"+name+"/rpc:i").c_str());
attach(rpcSrvr);
return true;
}
bool BehaviorModule::configure(ResourceFinder &rf) {
robotName = rf.find("robot").asString().c_str();
std::cout << robotName << " is the name of our robot" << std::endl;
action_left = NULL;
action_right = NULL;
options_left.put("device", "remote_controlboard");
options_left.put("local", "/pos_ctrl_left_arm");
options_left.put("remote", ("/" + robotName + "/left_arm").c_str());
options_right.put("device", "remote_controlboard");
options_right.put("local", "/pos_ctrl_right_arm");
options_right.put("remote", ("/" + robotName + "/right_arm").c_str());
driver_left.open(options_left);
driver_right.open(options_right);
options_head.put("device", "remote_controlboard");
options_head.put("local", "/pos_ctrl_head");
options_head.put("remote", ("/" + robotName + "/head").c_str());
driver_head.open(options_head);
options_torso.put("device", "remote_controlboard");
options_torso.put("local", "/pos_ctrl_torso");
options_torso.put("remote", ("/" + robotName + "/torso").c_str());
driver_torso.open(options_torso);
emotP.open("/local/emoInt");
Network::connect("/local/emoInt", "/icub/face/emotions/in");
// become happy
happy();
if (!driver_left.isValid() || !driver_right.isValid() || !driver_head.isValid() || !driver_torso.isValid()) {
cerr << "A device is not available. Here are the known devices:"
<< endl;
cerr << Drivers::factory().toString().c_str() << endl;
exit(-1);
}
bool ok = true;
ok = ok && driver_left.view(pos_ctrl_left);
ok = ok && driver_left.view(encoders_left);
ok = ok && driver_right.view(pos_ctrl_right);
ok = ok && driver_right.view(encoders_right);
ok = ok && driver_head.view(pos_ctrl_head);
ok = ok && driver_head.view(encoders_head);
ok = ok && driver_torso.view(pos_ctrl_torso);
ok = ok && driver_torso.view(encoders_torso);
ok = ok && driver_left.view(ictrl_left);
ok = ok && driver_left.view(iimp_left);
ok = ok && driver_left.view(itrq_left);
ok = ok && driver_right.view(ictrl_right);
ok = ok && driver_right.view(iimp_right);
ok = ok && driver_right.view(itrq_right);
chosen_arm = "left";
// needed for impedance controller
//ok = ok && driver_right.view(ictrl);
//ok = ok && driver_right.view(trq_ctrl_left);
//ok = ok && driver_right.view(trq_ctrl_right);
options_gaze.put("device", "gazecontrollerclient");
options_gaze.put("remote", "/iKinGazeCtrl");
options_gaze.put("local", "/client/gaze");
driver_gaze.open(options_gaze);
driver_gaze.view(igaze);
if (driver_gaze.isValid()) {
ok = ok && driver_gaze.view(igaze);
}
igaze->setTrackingMode(false);
igaze->setEyesTrajTime(1.0);
igaze->setNeckTrajTime(2.0);
igaze->bindNeckPitch(-38, 0);
igaze->bindNeckRoll(-5, 5);
igaze->bindNeckYaw(-30.0, 35.0);
if (!ok) {
cerr << "error getting interfaces" << std::endl;
exit(-1);
}
int n_jnts = 0;
pos_ctrl_left->getAxes(&n_jnts);
positions_left_cmd.resize(n_jnts);
positions_left_enc.resize(n_jnts);
pos_ctrl_right->getAxes(&n_jnts);
positions_right_cmd.resize(n_jnts);
positions_right_enc.resize(n_jnts);
pos_ctrl_head->getAxes(&n_jnts);
positions_head_enc.resize(n_jnts);
positions_head_cmd.resize(n_jnts);
pos_ctrl_torso->getAxes(&n_jnts);
positions_torso_enc.resize(n_jnts);
positions_torso_cmd.resize(n_jnts);
openPorts = false;
firstRun = true;
sim = false;
string name = rf.find("name").asString().c_str();
// setName(name.c_str());
string sim_or_real = rf.find("sim").asString().c_str();
if (sim_or_real == "on")
sim = true;
else
sim = false;
Property config;
config.fromConfigFile(rf.findFile("from").c_str());
Bottle &bGeneral = config.findGroup("general");
if (bGeneral.isNull()) {
cout << "Error: group general is missing!" << endl;
return false;
}
// parsing general config options
Property option;
for (int i = 1; i < bGeneral.size(); i++) {
Bottle *pB = bGeneral.get(i).asList();
if (pB->size() == 2)
option.put(pB->get(0).asString().c_str(), pB->get(1));
else {
cout << "Error: invalid option!" << endl;
return false;
}
}
option.put("local", name.c_str());
option.put("grasp_model_type",
rf.find("grasp_model_type").asString().c_str());
option.put("grasp_model_file", rf.findFile("grasp_model_file").c_str());
option.put("hand_sequences_file",
rf.findFile("hand_sequences_file").c_str());
printf("%s\n", option.toString().c_str());
// parsing arm dependent config options
Bottle &bArm = config.findGroup("arm_dependent");
graspOrien.resize(4);
graspDisp.resize(4);
dOffs.resize(3);
dLift.resize(3);
home_x.resize(3);
getArmDependentOptions(bArm, graspOrien, graspDisp, dOffs, dLift, home_x);
option.put("part", "right_arm");
printf("Options for right arm \n %s\n", option.toString().c_str());
cout << "***** Instantiating primitives for right_arm" << endl;
action_right = new ActionPrimitivesLayer2(option);
option.put("part", "left_arm");
printf("Options for left arm \n %s\n", option.toString().c_str());
cout << "***** Instantiating primitives for left_arm"<< endl;
action_left = new ActionPrimitivesLayer2(option);
if (!action_left->isValid()) {
delete action_left;
cout << "Action_left is not valid" << endl;
return false;
}
if (!action_right->isValid()) {
delete action_right;
cout << "Action_right is not valid" << endl;
return false;
}
deque<string> q = action_right->getHandSeqList();
cout << "***** List of available for right hand sequence keys:" << endl;
for (size_t i = 0; i < q.size(); i++)
cout << q[i] << endl;
q = action_left->getHandSeqList();
cout << "***** List of available for left hand sequence keys:" << endl;
for (size_t i = 0; i < q.size(); i++)
cout << q[i] << endl;
return true;
}
virtual bool configure(ResourceFinder &rf)
{
string name=rf.find("name").asString().c_str();
setName(name.c_str());
string partUsed=rf.find("part").asString().c_str();
if ((partUsed!="left_arm") && (partUsed!="right_arm"))
{
cout<<"Invalid part requested!"<<endl;
return false;
}
Property config;
config.fromConfigFile(rf.findFile("from").c_str());
Bottle &bGeneral=config.findGroup("general");
if (bGeneral.isNull())
{
cout<<"Error: group general is missing!"<<endl;
return false;
}
// parsing general config options
Property option;
for (int i=1; i<bGeneral.size(); i++)
{
Bottle *pB=bGeneral.get(i).asList();
if (pB->size()==2)
option.put(pB->get(0).asString().c_str(),pB->get(1));
else
{
cout<<"Error: invalid option!"<<endl;
return false;
}
}
option.put("local",name.c_str());
option.put("part",rf.find("part").asString().c_str());
option.put("grasp_model_type",rf.find("grasp_model_type").asString().c_str());
option.put("grasp_model_file",rf.findFile("grasp_model_file").c_str());
option.put("hand_sequences_file",rf.findFile("hand_sequences_file").c_str());
// parsing arm dependent config options
Bottle &bArm=config.findGroup("arm_dependent");
getArmDependentOptions(bArm,graspOrien,graspDisp,dOffs,dLift,home_x);
cout<<"***** Instantiating primitives for "<<partUsed<<endl;
action=new AFFACTIONPRIMITIVESLAYER(option);
if (!action->isValid())
{
delete action;
return false;
}
deque<string> q=action->getHandSeqList();
cout<<"***** List of available hand sequence keys:"<<endl;
for (size_t i=0; i<q.size(); i++)
cout<<q[i]<<endl;
string fwslash="/";
inPort.open((fwslash+name+"/in").c_str());
rpcPort.open((fwslash+name+"/rpc").c_str());
attach(rpcPort);
openPorts=true;
// check whether the grasp model is calibrated,
// otherwise calibrate it and save the results
Model *model;
action->getGraspModel(model);
if (!model->isCalibrated())
{
Property prop("(finger all)");
model->calibrate(prop);
ofstream fout;
fout.open(option.find("grasp_model_file").asString().c_str());
model->toStream(fout);
fout.close();
}
return true;
}
//Module initialization and configuration
virtual bool configure(ResourceFinder &rf)
{
string slash="/";
string ctrlName;
string robotName;
string partName;
string remoteName;
string localName;
Time::turboBoost();
// get params from the RF
ctrlName=rf.check("local",Value("baseControl")).asString();
robotName=rf.check("robot",Value("cer")).asString();
partName = rf.check("part", Value("mobile_base")).asString();
remoteName=slash+robotName+slash+partName;
localName=slash+ctrlName;
//reads the configuration file
Property ctrl_options;
ConstString configFile=rf.findFile("from");
if (configFile=="") //--from baseCtrl.ini
{
yError("Cannot find .ini configuration file. By default I'm searching for baseCtrl.ini");
return false;
}
else
{
ctrl_options.fromConfigFile(configFile.c_str());
}
ctrl_options.put("remote", remoteName.c_str());
ctrl_options.put("local", localName.c_str());
//check for robotInterface availability
yInfo("Checking for yarpRobotInterface availability");
Port startport;
startport.open (localName + "/yarpRobotInterfaceCheck:rpc");
Bottle cmd; cmd.addString("is_ready");
Bottle response;
int rc_count =0;
int rp_count =0;
int rf_count =0;
double start_time=yarp::os::Time::now();
bool not_yet_connected=true;
bool skip_robot_interface_check = rf.check("skip_robot_interface_check");
if (skip_robot_interface_check)
{
yInfo("skipping yarpRobotInterface check");
}
else
{
do
{
if (not_yet_connected)
{
bool rc = yarp::os::Network::connect (localName + "/yarpRobotInterfaceCheck:rpc","/" + robotName + "/yarprobotinterface");
if (rc == false)
{
yWarning ("Problems trying to connect to %s %d", std::string("/" + robotName + "/yarprobotinterface").c_str(), rc_count ++);
yarp::os::Time::delay (1.0);
continue;
}
else
{
not_yet_connected = false;
yDebug ("Connection established with yarpRobotInterface");
}
}
bool rp = startport.write (cmd, response);
if (rp == false)
{
yWarning ("Problems trying to connect to yarpRobotInterface %d", rp_count ++);
if (yarp::os::Time::now()-start_time>30)
{
yError ("Timeout expired while trying to connect to yarpRobotInterface");
return false;
}
yarp::os::Time::delay (1.0);
continue;
}
else
{
if (response.get(0).asString() != "ok")
{
yWarning ("yarpRobotInterface is not ready yet, retrying... %d", rf_count++);
if (yarp::os::Time::now()-start_time>30)
{
yError ("Timeout expired while waiting for yarpRobotInterface availability");
return false;
}
yarp::os::Time::delay (1.0);
continue;
}
else
{
yInfo ("yarpRobotInterface is ready");
break;
}
}
} while (1);
}
//set the thread rate
int period = rf.check("period",Value(20)).asInt();
yInfo("baseCtrl thread rate: %d ms.",period);
// the motor control thread
bool motors_enabled=true;
if (rf.check("no_motors"))
{
yInfo("'no_motors' option found. Skipping motor control part.");
motors_enabled=false;
}
if (motors_enabled==true)
{
control_thr = new ControlThread(period, rf, ctrl_options);
if (!control_thr->start())
{
delete control_thr;
return false;
}
}
//try to connect to joystickCtrl output
if (rf.check("joystick_connect"))
{
int joystick_trials = 0;
do
{
yarp::os::Time::delay(1.0);
if (yarp::os::Network::connect("/joystickCtrl:o",localName+"/joystick1:i"))
{
yInfo("Joystick has been automatically connected");
break;
}
else
{
yWarning("Unable to find the joystick port, retrying (%d/5)...",joystick_trials);
joystick_trials++;
}
if (joystick_trials>=5)
{
yError("Unable to find the joystick port, giving up");
break;
}
}
while (1);
}
//check for debug mode
if (rf.check("debug"))
{
this->control_thr->enable_debug(true);
}
rpcPort.open((localName+"/rpc").c_str());
attach(rpcPort);
return true;
}
int main(int argc, char *argv[])
{
Network yarp; //initialize network, this goes before everything
if (!yarp.checkNetwork())
{
fprintf(stderr, "Sorry YARP network does not seem to be available, is the yarp server available?\n");
return -1;
}
yarp::os::signal(yarp::os::YARP_SIGINT, sighandler);
yarp::os::signal(yarp::os::YARP_SIGTERM, sighandler);
Time::turboBoost();
//for compatibility with old usage of iCubInterface, the use of the ResourceFinder
//here is merely functional and should NOT be taken as an example
ResourceFinder rf;
rf.setVerbose();
rf.setDefaultConfigFile("iCubInterface.ini");
rf.configure(argc, argv);
ConstString configFile=rf.findFile("config");
ConstString cartRightArm=rf.findFile("cartRightArm");
ConstString cartLeftArm=rf.findFile("cartLeftArm");
std::string filename;
bool remap=false;
if (configFile!="")
{
configFile=rf.findFile("config");
filename=configFile.c_str();
remap=true;
}
else
{
configFile=rf.find("file").asString();
if (configFile!="")
{
const char *conf = yarp::os::getenv("ICUB_ROOT");
filename+=conf;
filename+="/conf/";
filename+=configFile.c_str();
printf("Read robot description from %s\n", filename.c_str());
}
else
{
printf("\n");
printf("Error: iCubInterface was not able to find a valid configuration file ");
printf("(since September 2009 we changed a bit how iCubInterface locates configuratrion files).\n");
printf("== Old possibilities:\n");
printf("--config <CONFIG_FILE> read config file CONFIG_FILE.\n iCubInterface now ");
printf("uses the ResourceFinder class to search for CONFIG_FILE. ");
printf("Make sure you understand how the ResourceFinder works. In particular ");
printf("you most likely need to set the YARP_ROBOT_NAME environment variable ");
printf("to tell the RF to add robots/$YARP_ROBOT_NAME to the search path.\n");
printf("--file <CONFIG_FILE> read config file from $ICUB_ROOT/conf: old style ");
printf("initialization method, obsolete. Still here for compatibility reasons.\n");
printf("== New possibilities:\n");
printf("Place a file called iCubInterface.ini in robots/$YARP_ROBOT_NAME that contains ");
printf("the line \"config icubSafe.ini\" (or anything of your choice), and run iCubInterface ");
printf("without parameters\n.");
printf("== Preventing default behaviors:\n");
printf("Use full path in <CONFIG_FILE> (e.g. --config ./icubSafe.ini).\n");
printf("Use --from: change config file (e.g. --from iCubInterfaceCustom.ini).\n");
return -1;
}
}
// printf("--file <CONFIG_FILE> read robot config file CONFIG_FILE\n");
// printf("Note: this files are searched in $ICUB_ROOT/conf (obsolete)\n");
// printf("--config <CONFIG_FILE> full path to robot config file\n");
// printf("-Examples:\n");
// printf("%s --file icub.ini (obsolete, not standard)\n", argv[0]);
// printf("%s --config $ICUB_ROOT/app/default/conf/icub.ini\n)", argv[0]);
// return -1;
Property robotOptions;
bool ok=robotOptions.fromConfigFile(filename.c_str());
if (!ok)
{
fprintf(stderr, "Sorry could not open %s\n", filename.c_str());
return -1;
}
printf("Config file loaded successfully\n");
std::string quitPortName;
if (robotOptions.check("TERMINATEPORT"))
{
Value &v=robotOptions.findGroup("TERMINATEPORT").find("Name");
quitPortName=std::string(v.toString().c_str());
}
else
quitPortName=std::string("/iCubInterface/quit");
fprintf(stderr, "Quit will listen on %s\n", quitPortName.c_str());
// LATER: must use a specific robot name to make the port unique.
Terminee terminee(quitPortName.c_str());
// Terminee terminee("/iCubInterface/quit");
if (!terminee.isOk()) {
printf("Failed to create proper quit socket\n");
return 1;
}
IRobotInterface *i;
if (remap)
{
i=new RobotInterfaceRemap;
}
else
{
i=new RobotInterface;
}
ri=i; //set pointer to RobotInterface object (used in the handlers above)
ok = i->initialize(filename);
if (!ok)
return 0;
char c = 0;
printf("Driver instantiated and running (silently)\n");
printf("Checking if you have requested instantiation of cartesian controller\n");
bool someCartesian=false;
if (cartRightArm!="")
{
i->initCart(cartRightArm.c_str());
someCartesian=true;
}
if (cartLeftArm!="")
{
i->initCart(cartLeftArm.c_str());
someCartesian=true;
}
if (!someCartesian)
{
printf("Nothing found\n");
}
while (!terminee.mustQuit()&&!terminated)
{
Time::delay(2);
}
printf("Received a quit message\n");
if (someCartesian)
{
i->finiCart();
}
i->detachWrappers();
i->park(); //default behavior is blocking (safer)
i->closeNetworks();
ri=0; //tell signal handler interface is not longer valid (do this before you destroy i ;)
delete i;
i=0;
return 0;
}
void assignTextons(CvMat* tmap, CvMat* grayImg){
//char filterBin_dirLoc[1000];
findWorkDir();
// Run the filter bank on the Gray Image!!
// 1. Read the filter Bank parameters
// 2. convolve the image with the filter to get the response.
char filterBankFileName[1000];
ResourceFinder rf;
rf.setVerbose( true );
rf.setDefaultContext( "activeSeg/conf" );
int argc;
char **argv;
rf.configure( "ICUB_ROOT", 0, 0 );
//string textureBank13 = rf.find("texture_filterbank_13.txt").asString().c_str();
string texture13=rf.findFile("texture_filterbank_13.txt").c_str();
string texture19=rf.findFile("texture_filterbank_19.txt").c_str();
//strcpy(filterBankFileName, filterBin_dirLoc);
//strcat(filterBankFileName,"/texture_filterbank_13.txt");
FILE* fp_13 = fopen(texture13.c_str(),"r");
//strcpy(filterBankFileName, filterBin_dirLoc);
//strcat(filterBankFileName,"/texture_filterbank_19.txt");
FILE* fp_19 = fopen(texture19.c_str(),"r");
if (fp_13 == NULL || fp_19 == NULL){
fprintf(stderr,"either texture_filterbank_19.txt or texture_filterbank_13.txt or both not found\n");
exit(1);
}
//double* tmp1= (double*)malloc(13*13*sizeof(double));
//double* tmp2= (double*)malloc(19*19*sizeof(double));
CvMat* filterBank13x13 = cvCreateMat(13,13,CV_32FC1);
CvMat* filterBank19x19 = cvCreateMat(19,19,CV_32FC1);
CvMat* imgResponse[24];
CvMat* grayImg_pad13x13 = cvCreateMat(grayImg->rows+6,grayImg->cols+6,CV_32FC1);
CvMat* grayImg_pad19x19 = cvCreateMat(grayImg->rows+9,grayImg->cols+9,CV_32FC1);
CvMat* imgResponse_pad13x13 = cvCreateMat(grayImg->rows+6,grayImg->cols+6,CV_32FC1);
CvMat* imgResponse_pad19x19 = cvCreateMat(grayImg->rows+9,grayImg->cols+9,CV_32FC1);
CvMat* tmp_pad13x13 = cvCreateMatHeader(grayImg->rows+6,grayImg->cols+6,CV_32FC1);
CvMat* tmp_pad19x19 = cvCreateMatHeader(grayImg->rows+9,grayImg->cols+9,CV_32FC1);
for(int i=0; i< 24; i++)
imgResponse[i] = cvCreateMat(grayImg->rows,grayImg->cols,CV_32FC1);
cvCopyMakeBorder(grayImg,grayImg_pad13x13,cvPoint(6,6),IPL_BORDER_REPLICATE);
cvCopyMakeBorder(grayImg,grayImg_pad19x19,cvPoint(9,9),IPL_BORDER_REPLICATE);
float tmpVal;
for(int i=0; i< 24; i++){
if (i < 12){
//fread(tmp1,sizeof(double),13*13,fp);
for(int r=0; r < 13; r++){
for(int c=0; c<13; c++){
fscanf(fp_13,"%f",&tmpVal);
cvSetReal2D(filterBank13x13,r,c,tmpVal);
}
}
// step 2: convolve and get the response..
cvFilter2D(grayImg_pad13x13,imgResponse_pad13x13,filterBank13x13,cvPoint(6,6));
cvGetSubRect(imgResponse_pad13x13,tmp_pad13x13,cvRect(6,6,imgResponse[i]->cols,imgResponse[i]->rows));
cvCopy(tmp_pad13x13,imgResponse[i]);
}
else{
//fread(tmp2,sizeof(double),19*19,fp);
for(int r=0; r < 19; r++){
for(int c=0; c<19; c++){
fscanf(fp_19,"%f",&tmpVal);
cvSetReal2D(filterBank19x19,r,c,tmpVal);
}
}
// step 2: convolve and get the response..
cvFilter2D(grayImg_pad19x19,imgResponse_pad19x19, filterBank19x19, cvPoint(9,9));
cvGetSubRect(imgResponse_pad19x19,tmp_pad19x19,cvRect(9,9,imgResponse[i]->cols,imgResponse[i]->rows));
cvCopy(tmp_pad19x19,imgResponse[i]);
}
}
fclose(fp_13);
fclose(fp_19);
cvReleaseMat(&imgResponse_pad13x13);
cvReleaseMat(&imgResponse_pad19x19);
cvReleaseMat(&grayImg_pad13x13);
cvReleaseMat(&grayImg_pad19x19);
cvReleaseMat(&filterBank13x13);
cvReleaseMat(&filterBank19x19);
// Read the textons
string textonsFile=rf.findFile("textons.txt").c_str();
//strcpy(filterBankFileName, filterBin_dirLoc);
//strcat(filterBankFileName,"/textons.txt");
FILE *fp = fopen(textonsFile.c_str(),"r");
//fp = fopen("textons.bin","rb");
//double* tmp3 = (double*)malloc(64*24*sizeof(double));
if (fp == NULL){
fprintf(stderr,"textons.txt not found");
exit(1);
}
//fread(tmp3, sizeof(double),64*24, fp);
CvMat* textons = cvCreateMat(64,24,CV_32FC1);
for(int r=0; r < 64;r++){
for(int c=0; c < 24; c++){
fscanf(fp,"%f",&tmpVal);
cvSetReal2D(textons,r,c,tmpVal);
}
}
fclose(fp);
//free(tmp3);
// find the closest texton for every pixel !!
CvMat* colFrmTextonMat = cvCreateMatHeader(64,1,CV_32FC1);
CvMat* totSum = cvCreateMat(64,1,CV_32FC1);
CvMat* tmp = cvCreateMat(64,1,CV_32FC1);
for(int r=0; r<grayImg->rows; r++){
for(int c=0; c<grayImg->cols; c++){
cvSetZero(totSum);
for(int i=0; i<24; i++){
//find the distance of the pixel response from all other
cvGetCol(textons,colFrmTextonMat, i);
cvSubS(colFrmTextonMat,cvGet2D(imgResponse[i],r,c),tmp);
cvPow(tmp,tmp,2.0);
cvAdd(totSum,tmp,totSum);
}
// find the texton with minimum distance
CvPoint minLoc, maxLoc;
double minVal, maxVal;
cvMinMaxLoc(totSum,&minVal,&maxVal,&minLoc,&maxLoc);
cvSetReal2D(tmap,r,c,(double)minLoc.y);
}
}
//free matrices
for(int i=0; i< 24; i++)
cvReleaseMat(&imgResponse[i]);
cvReleaseMat(&totSum);
cvReleaseMat(&tmp);
cvReleaseMat(&textons);
}
bool MotorFrictionIdentificationModule::configure(ResourceFinder &rf)
{
//--------------------------PARAMETER HELPER--------------------------
paramHelper = new ParamHelperServer(motorFrictionIdentificationParamDescr, PARAM_ID_SIZE,
motorFrictionIdentificationCommandDescr, COMMAND_ID_SIZE);
paramHelper->linkParam(PARAM_ID_MODULE_NAME, &moduleName);
paramHelper->linkParam(PARAM_ID_CTRL_PERIOD, &threadPeriod);
paramHelper->linkParam(PARAM_ID_ROBOT_NAME, &robotName);
paramHelper->registerCommandCallback(COMMAND_ID_HELP, this);
paramHelper->registerCommandCallback(COMMAND_ID_QUIT, this);
// Read parameters from configuration file (or command line)
Bottle initMsg;
paramHelper->initializeParams(rf, initMsg);
printBottle(initMsg);
///< PARAMETER HELPER CLIENT (FOR JOINT TORQUE CONTROL)
const ParamProxyInterface* torqueCtrlParams[5];
torqueCtrlParams[0] = jointTorqueControl::jointTorqueControlParamDescr[4];
torqueCtrlParams[1] = jointTorqueControl::jointTorqueControlParamDescr[5];
torqueCtrlParams[2] = jointTorqueControl::jointTorqueControlParamDescr[6];
torqueCtrlParams[3] = jointTorqueControl::jointTorqueControlParamDescr[7];
torqueCtrlParams[4] = jointTorqueControl::jointTorqueControlParamDescr[8];
YARP_ASSERT(torqueCtrlParams[0]->id==jointTorqueControl::PARAM_ID_KT);
torqueController = new ParamHelperClient(torqueCtrlParams, 5); //jointTorqueControl::PARAM_ID_SIZE);
// do not initialize paramHelperClient because jointTorqueControl module may not be running
// Open ports for communicating with other modules
if(!paramHelper->init(moduleName))
{
fprintf(stderr, "Error while initializing parameter helper. Closing module.\n");
return false;
}
rpcPort.open(("/"+moduleName+"/rpc").c_str());
setName(moduleName.c_str());
attach(rpcPort);
//--------------------------WHOLE BODY INTERFACE--------------------------
yarp::os::Property yarpWbiOptions;
//Get wbi options from the canonical file
if( !rf.check("wbi_conf_file") )
{
fprintf(stderr,"[ERR] motorFrictionIdentificationModule: impossible to open motorFrictionIdentificationModule: wbi_conf_file option missing");
return false;
}
if( !rf.check("robot") )
{
fprintf(stderr,"[ERR] motorFrictionIdentificationModule: impossible to open motorFrictionIdentificationModule: robot option missing");
return false;
}
std::string wbiConfFile = rf.findFile("wbi_conf_file");
yarpWbiOptions.fromConfigFile(wbiConfFile);
yarpWbiOptions.put("robot",rf.find("robot").asString());
//List of joints used in the dynamic model of the robot
IDList RobotDynamicModelJoints;
std::string RobotDynamicModelJointsListName = "ICUB_MAIN_JOINTS";
if( !loadIdListFromConfig(RobotDynamicModelJointsListName,yarpWbiOptions,RobotDynamicModelJoints) )
{
fprintf(stderr, "[ERR] motorFrictionIdentificationModule: impossible to load wbiId joint list with name %s\n",RobotDynamicModelJointsListName.c_str());
return false;
}
///< read the parameter "joint list" from configuration file to configure the WBI
jointNamesList.resize(paramHelper->getParamProxy(PARAM_ID_JOINT_NAMES_LIST)->size);
paramHelper->getParamProxy(PARAM_ID_JOINT_NAMES_LIST)->linkToVariable(jointNamesList.data());
///< link the parameter "joint names" to the variable jointNames
jointNames.resize(jointNamesList.size());
paramHelper->getParamProxy(PARAM_ID_JOINT_NAMES)->linkToVariable(jointNames.data(), jointNamesList.size());
///< add all the specified joints
bool ok = true;
ID wbi_id;
for(int i=0; ok && i<jointNamesList.size(); i++)
{
wbi_id = jointNamesList[i];
ok = robotInterface->addJoint(wbi_id);
jointNames[i] = jointNamesList[i];
}
if(!ok || !robotInterface->init())
{
fprintf(stderr, "Error while initializing whole body interface. Closing module\n");
return false;
}
fprintf(stderr, "After initialize interface\n");
//--------------------------CTRL THREAD--------------------------
identificationThread = new MotorFrictionIdentificationThread(moduleName, robotName, threadPeriod, paramHelper, robotInterface, torqueController, rf);
if(!identificationThread->start())
{
fprintf(stderr, "Error while initializing motorFrictionIdentification control thread. Closing module.\n");
return false;
}
fprintf(stderr,"MotorFrictionIdentification control started\n");
return true;
}
bool Eye2world::configure(ResourceFinder &rf) {
if (!ThreadedRFModule::configure(rf)) {
return false;
}
ConstString str;
str = "Adds a specific value to the z coordinate of the transformation to the projection plane.";
addModuleOption(new Option("heightOffset", str, Option::NUMERIC, Value(0.0)));
str
= "This value is added to the z coordinate of the transformation to the projection plane but is again subtracted from the result afterwards.";
addModuleOption(new Option("motor2eye", str, Option::NUMERIC, Value(0.0)));
str
= "A scale with is applied to the results. This might be used to compensate calibration issues.";
addModuleOption(new Option("scale", str, Option::NUMERIC, Value(1.0)));
Value* v = Value::makeList("1.0 2 3.3");
addModuleOption(new Option("vector", "Test vector", Option::VECTOR, *v));
delete v;
ConstString robotPort = prefix + robotName;
rf.setDefault("eyeCalibration", "iCubEyes.ini");
ConstString calibrationFilename = rf.findFile("eyeCalibration" /*,
"Name of the configuration file of the iCub's eye calibration (string)"*/);
rf.setDefault("tableConfiguration", "table.ini");
ConstString tableConfiguration = rf.findFile("tableConfiguration" /*,
"Name of the configuration file of the experiments table (string)"*/);
str
= "Input port providing 2D coordinates as Bottle([ConstString:cam,] double:x, double:y[, double:height])";
str = prefix + getName(rf.check("in", Value("/in"), str).asString());
dataPorts.add(id.Input_Coordinates2d, str, new BufferedPort<Bottle> );
str = prefix + getName(rf.check("headState", Value(robotPort + "/head/state:i"),
"Input port providing the head's position").asString());
dataPorts.add(id.Input_HeadPosition, str, new BufferedPort<Bottle> );
str = prefix + getName(rf.check("torsoState", Value(robotPort + "/torso/state:i"),
"Input port providing the torso's position").asString());
dataPorts.add(id.Input_TorsoPosition, str, new BufferedPort<Bottle> );
str = prefix + getName(rf.check("out", Value("/out"),
"Output port providing 3D coordinates as Bottle(double:x, double:y, double:z)").asString());
dataPorts.add(id.Output_Coordinates3d, str, new BufferedPort<Bottle> );
headV2=rf.check("headV2");
Property config;
if (!config.fromConfigFile(calibrationFilename)) {
return false;
}
rightEyeCalibration.fromString(config.findGroup("CAMERA_CALIBRATION_RIGHT").toString());
leftEyeCalibration.fromString(config.findGroup("CAMERA_CALIBRATION_LEFT").toString());
cameras["right"] = &rightEyeCalibration;
cameras["left"] = &leftEyeCalibration;
Property tableConfig;
if (!tableConfig.fromConfigFile(tableConfiguration)) {
cerr << "Unable to read table configuration. Assuming 0 vector as 3D offset." << endl;
}
tabletopPosition.fromString(tableConfig.findGroup("POSITION").toString());
vector<unsigned int> errorLog;
if (!dataPorts.open(&errorLog)) {
for (unsigned int i = 0; i < errorLog.size(); i++) {
cout << getName() << ": unable to open port " << dataPorts.getName(i) << endl;
}
return false; // unable to open; let RFModule know so that it won't run
}
return startThread();
}
bool saliencyBlobFinderModule::configure(ResourceFinder &rf) {
/* Process all parameters from both command-line and .ini file */
if(rf.check("help")) {
printf("HELP \n");
printf("====== \n");
printf("--name : changes the rootname of the module ports \n");
printf("--robot : changes the name of the robot where the module interfaces to \n");
printf("--name : rootname for all the connection of the module \n");
printf("--camerasContext : context where camera parameters are stored \n");
printf("--camerasFile : file of parameters of the camera in the context \n");
printf(" \n");
printf("press CTRL-C to stop... \n");
return true;
}
/* get the module name which will form the stem of all module port names */
moduleName = rf.check("name",
Value("/blobFinder/icub/left_cam"),
"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 thread rate which will define the period of the processing thread
*/
threadRate = rf.check("ratethread",
Value(33),
"processing ratethread (int)").asInt();
cout << "Module started with the parameter ratethread: " << threadRate << endl;
/*
* gets the minBounding area for blob neighbours definition
*/
minBoundingArea = rf.check("minBoundingArea",
Value(225),
"minBoundingArea (int)").asInt();
cout << "Module started with min bounding area " << minBoundingArea << endl;
if (!cmdPort.open(getName())) {
cout << getName() << ": Unable to open port " << endl;
return false;
}
attach(cmdPort); // attach to port
/*
if (rf.check("config")) {
configFile=rf.findFile(rf.find("config").asString().c_str());
if (configFile=="") {
return false;
}
}
else {
configFile.clear();
}
*/
if (rf.check("camerasFile")) {
if (rf.check("camerasContext")) {
printf("found a new context %s \n", rf.find("camerasContext").asString().c_str());
// rf.find("camerasContext").asString().c_str()
rf.setDefaultContext("cameraCalibration/conf");
}
printf("got the cameraContext %s \n", rf.getContext().c_str());
camerasFile=rf.findFile(rf.find("camerasFile").asString().c_str());
if (camerasFile==""){
return false;
}
else {
printf("found the camerasFile %s \n", camerasFile.c_str());
}
}
else {
camerasFile.clear();
}
printf("configFile: %s \n", camerasFile.c_str());
//initialization of the main thread
blobFinder = new blobFinderThread(threadRate,camerasFile);
blobFinder->setName(this->getName().c_str());
blobFinder->setMinBoundingArea(minBoundingArea);
blobFinder->start();
cout << "waiting for connection of the input port" << endl;
return true;
}
virtual bool configure(ResourceFinder &rf)
{
string slash = "/";
string ctrlName;
string robotName;
// string remoteName;
string localName;
Time::turboBoost();
// get params from the RF
ctrlName = rf.check("local", Value("robotJoystickControl")).asString();
robotName = rf.check("robot", Value("cer")).asString();
localName = "/" + ctrlName;
//reads the configuration file
Property ctrl_options;
ConstString configFile = rf.findFile("from");
if (configFile == "") //--from torsoJoystickControl.ini
{
yWarning("Cannot find .ini configuration file. By default I'm searching for torsoJoystickControl.ini");
//return false;
}
else
{
ctrl_options.fromConfigFile(configFile.c_str());
}
ctrl_options.put("robot", robotName.c_str());
ctrl_options.put("local", localName.c_str());
//check for robotInterface availablity
yInfo("Checking for robotInterface availability");
Port startport;
startport.open(localName+"/robotInterfaceCheck:rpc");
Bottle cmd; cmd.addString("is_ready");
Bottle response;
int rc_count = 0;
int rp_count = 0;
int rf_count = 0;
double start_time = yarp::os::Time::now();
bool not_yet_connected = true;
bool skip_robot_interface_check = rf.check("skip_robot_interface_check");
if (skip_robot_interface_check)
{
yInfo("skipping robotInterface check");
}
else
{
do
{
if (not_yet_connected)
{
bool rc = yarp::os::Network::connect(localName + "/robotInterfaceCheck:rpc", "/" + robotName + "/robotInterface");
if (rc == false)
{
yWarning("Problems trying to connect to RobotInterface %d", rc_count++);
yarp::os::Time::delay(1.0);
continue;
}
else
{
not_yet_connected = false;
yDebug("Connection established with robotInterface");
}
}
bool rp = startport.write(cmd, response);
if (rp == false)
{
yWarning("Problems trying to connect to RobotInterface %d", rp_count++);
if (yarp::os::Time::now() - start_time > 30)
{
yError("Timeout expired while trying to connect to robotInterface");
return false;
}
yarp::os::Time::delay(1.0);
continue;
}
else
{
if (response.get(0).asString() != "ok")
{
yWarning("RobotInterface is not ready yet, retrying... %d", rf_count++);
if (yarp::os::Time::now() - start_time > 30)
{
yError("Timeout expired while waiting for robotInterface availability");
return false;
}
yarp::os::Time::delay(1.0);
continue;
}
else
{
yInfo("RobotInterface is ready");
break;
}
}
} while (1);
}
//set the thread rate
int rate = rf.check("rate",Value(10)).asInt();
yInfo("baseCtrl thread rate: %d ms.",rate);
//start the control thread
control_thr = new ControlThread(rate, rf, ctrl_options);
if (!control_thr->start())
{
delete control_thr;
return false;
}
//check for debug mode
if (rf.check("no_motors"))
{
this->control_thr->motors_enabled = false;
yInfo() << "Motors disabled";
}
rpcPort.open((localName+"/rpc").c_str());
attach(rpcPort);
return true;
}
bool wholeBodyDynamicsModule::configure(ResourceFinder &rf)
{
if( rf.check("robot") ) {
robotName = rf.find("robot").asString();
} else {
std::cerr << "wholeBodyDynamicsModule::configure failed: robot parameter not found. Closing module." << std::endl;
return false;
}
if( rf.check("name") ) {
moduleName = rf.find("name").asString();
setName(moduleName.c_str());
} else {
std::cerr << "wholeBodyDynamicsModule::configure failed: name parameter not found. Closing module." << std::endl;
return false;
}
//Loading thread period
if( rf.check("period") && rf.find("period").isInt() )
{
period = rf.find("period").asInt();
}
// If period is not specified, check also the legacy "rate" option
if( !rf.check("period") && rf.check("rate") && rf.find("rate").isInt() )
{
period = rf.find("rate").asInt();
}
bool fixed_base = false;
bool fixed_base_calibration = false;
std::string fixed_link;
std::string fixed_link_calibration;
if( rf.check("assume_fixed") )
{
fixed_link = rf.find("assume_fixed").asString().c_str();
if( fixed_link != "root_link" &&
fixed_link != "l_sole" &&
fixed_link != "r_sole" &&
fixed_link != "r_foot_dh_frame" &&
fixed_link != "l_foot_dh_frame" )
{
yError() << "assume_fixed option found, but disabled because " << fixed_link << " is not a recognized fixed_link ";
return false;
} else {
yInfo() << "assume_fixed option found, using " << fixed_link << " as fixed link as a kinematic root instead of the imu.";
fixed_base = true;
fixed_base_calibration = true;
fixed_link_calibration = fixed_link;
// \todo TODO workaround for heidelberg
if( fixed_link == "l_sole" )
{
fixed_link = fixed_link_calibration = "l_foot_dh_frame";
}
if( fixed_link == "r_sole" )
{
fixed_link = fixed_link_calibration = "r_foot_dh_frame";
}
}
}
else
{
}
//--------------------------RPC PORT--------------------------------------------
attach(rpcPort);
std::string rpcPortName= "/";
rpcPortName+= getName();
rpcPortName += "/rpc:i";
if (!rpcPort.open(rpcPortName.c_str())) {
std::cerr << getName() << ": Unable to open port " << rpcPortName << std::endl;
return false;
}
//--------------------------WHOLE BODY STATES INTERFACE--------------------------
yarp::os::Property yarpWbiOptions;
//Get wbi options from the canonical file
if( !rf.check("wbi_conf_file") )
{
fprintf(stderr,"[ERR] wholeBodyDynamicsThread: impossible to open wholeBodyInterface: wbi_conf_file option missing");
return false;
}
std::string wbiConfFile = rf.findFile("wbi_conf_file");
yarpWbiOptions.fromConfigFile(wbiConfFile);
//Overwrite the parameters in the wbi_conf_file with stuff from the wbd options file
yarpWbiOptions.fromString(rf.toString(),false);
//List of joints used in the dynamic model of the robot
IDList RobotDynamicModelJoints;
std::string RobotDynamicModelJointsListName = rf.check("torque_estimation_joint_list",
yarp::os::Value("ROBOT_DYNAMIC_MODEL_JOINTS"),
"Name of the list of joint used for torque estimation").asString().c_str();
if( !loadIdListFromConfig(RobotDynamicModelJointsListName,rf,RobotDynamicModelJoints) )
{
if( !loadIdListFromConfig(RobotDynamicModelJointsListName,yarpWbiOptions,RobotDynamicModelJoints) )
{
fprintf(stderr, "[ERR] wholeBodyDynamicsModule: impossible to load wbiId joint list with name %s\n",RobotDynamicModelJointsListName.c_str());
return false;
}
}
//Add to the options some wbd specific stuff
if( fixed_base )
{
yarpWbiOptions.put("fixed_base",fixed_link);
}
if( rf.check("assume_fixed_from_odometry") )
{
yarpWbiOptions.put("assume_fixed_from_odometry","dummy");
}
if( rf.check("IDYNTREE_SKINDYNLIB_LINKS") )
{
yarp::os::Property & prop = yarpWbiOptions.addGroup("IDYNTREE_SKINDYNLIB_LINKS");
prop.fromString(rf.findGroup("IDYNTREE_SKINDYNLIB_LINKS").tail().toString());
}
else
{
fprintf(stderr, "[ERR] wholeBodyDynamicsModule: impossible to load IDYNTREE_SKINDYNLIB_LINKS group, exiting");
}
if( rf.check("WBD_SUBTREES") )
{
yarp::os::Property & prop = yarpWbiOptions.addGroup("WBD_SUBTREES");
prop.fromString(rf.findGroup("WBD_SUBTREES").tail().toString());
}
else
{
fprintf(stderr, "[ERR] wholeBodyDynamicsModule: impossible to load WBD_SUBTREES group, exiting");
}
if( rf.check("WBD_OUTPUT_TORQUE_PORTS") )
{
yarp::os::Property & prop = yarpWbiOptions.addGroup("WBD_OUTPUT_TORQUE_PORTS");
prop.fromString(rf.findGroup("WBD_OUTPUT_TORQUE_PORTS").tail().toString());
}
else
{
fprintf(stderr, "[ERR] wholeBodyDynamicsModule: impossible to load WBD_OUTPUT_TORQUE_PORTS group, exiting");
}
if( rf.check("WBD_OUTPUT_EXTERNAL_WRENCH_PORTS") )
{
yarp::os::Property & prop = yarpWbiOptions.addGroup("WBD_OUTPUT_EXTERNAL_WRENCH_PORTS");
prop.fromString(rf.findGroup("WBD_OUTPUT_EXTERNAL_WRENCH_PORTS").tail().toString());
}
if( rf.check("calibration_support_link") )
{
yarpWbiOptions.put("calibration_support_link",rf.find("calibration_support_link").asString());
}
else
{
yarpWbiOptions.put("calibration_support_link","root_link");
}
sensors = new yarpWholeBodySensors(moduleName.c_str(), yarpWbiOptions);
sensors->addSensors(wbi::SENSOR_ENCODER,RobotDynamicModelJoints);
//estimationInterface->addEstimates(wbi::ESTIMATE_JOINT_VEL,RobotDynamicModelJoints);
//estimationInterface->addEstimates(wbi::ESTIMATE_JOINT_ACC,RobotDynamicModelJoints);
//List of 6-axis Force-Torque sensors in the robot
IDList RobotFTSensors;
std::string RobotFTSensorsListName = "ROBOT_MAIN_FTS";
if( !loadIdListFromConfig(RobotFTSensorsListName,yarpWbiOptions,RobotFTSensors) )
{
yError("wholeBodyDynamicsTree: impossible to load wbiId list with name %s\n",RobotFTSensorsListName.c_str());
}
sensors->addSensors(wbi::SENSOR_FORCE_TORQUE,RobotFTSensors);
//List of IMUs sensors in the robot
IDList RobotIMUSensors;
std::string RobotIMUSensorsListName = "ROBOT_MAIN_IMUS";
if( !loadIdListFromConfig(RobotIMUSensorsListName,yarpWbiOptions,RobotIMUSensors) )
{
yError("wholeBodyDynamicsTree: impossible to load wbiId list with name %s\n",RobotFTSensorsListName.c_str());
}
sensors->addSensors(wbi::SENSOR_IMU,RobotIMUSensors);
if(!sensors->init())
{
yError() << getName() << ": Error while initializing whole body estimator interface.Closing module";
return false;
}
//--------------------------WHOLE BODY DYNAMICS THREAD--------------------------
wbdThread = new wholeBodyDynamicsThread(moduleName,
robotName,
period,
sensors,
yarpWbiOptions,
fixed_base_calibration,
fixed_link_calibration,
rf.check("assume_fixed_from_odometry"));
if(!wbdThread->start())
{
yError() << getName()
<< ": Error while initializing whole body estimator thread."
<< "Closing module";
return false;
}
yInfo() << "wholeBodyDynamicsThread started";
return true;
}
bool configure(ResourceFinder &rf)
{
bool noLegs=rf.check("no_legs");
Property optTorso("(device remote_controlboard)");
Property optArmR("(device remote_controlboard)");
Property optArmL("(device remote_controlboard)");
Property optLegR("(device remote_controlboard)");
Property optLegL("(device remote_controlboard)");
string robot=rf.find("robot").asString().c_str();
optTorso.put("remote",("/"+robot+"/torso").c_str());
optArmR.put("remote",("/"+robot+"/right_arm").c_str());
optArmL.put("remote",("/"+robot+"/left_arm").c_str());
optLegR.put("remote",("/"+robot+"/right_leg").c_str());
optLegL.put("remote",("/"+robot+"/left_leg").c_str());
string local=rf.find("local").asString().c_str();
optTorso.put("local",("/"+local+"/torso").c_str());
optArmR.put("local",("/"+local+"/right_arm").c_str());
optArmL.put("local",("/"+local+"/left_arm").c_str());
optLegR.put("local",("/"+local+"/right_leg").c_str());
optLegL.put("local",("/"+local+"/left_leg").c_str());
optTorso.put("writeStrict","on");
optArmR.put("writeStrict","on");
optArmL.put("writeStrict","on");
optLegR.put("writeStrict","on");
optLegL.put("writeStrict","on");
if (!torso.open(optTorso) || !armR.open(optArmR) || !armL.open(optArmL))
{
cout<<"Device drivers not available!"<<endl;
close();
return false;
}
if (!noLegs)
{
if (!legR.open(optLegR) || !legL.open(optLegL))
{
cout<<"Device drivers not available!"<<endl;
close();
return false;
}
}
PolyDriverList listArmR, listArmL, listLegR, listLegL;
listArmR.push(&torso,"torso");
listArmR.push(&armR,"right_arm");
listArmL.push(&torso,"torso");
listArmL.push(&armL,"left_arm");
listLegR.push(&legR,"right_leg");
listLegL.push(&legL,"left_leg");
Property optServerArmR("(device cartesiancontrollerserver)");
Property optServerArmL("(device cartesiancontrollerserver)");
Property optServerLegR("(device cartesiancontrollerserver)");
Property optServerLegL("(device cartesiancontrollerserver)");
optServerArmR.fromConfigFile(rf.findFile("right_arm_file"),false);
optServerArmL.fromConfigFile(rf.findFile("left_arm_file"),false);
optServerLegR.fromConfigFile(rf.findFile("right_leg_file"),false);
optServerLegL.fromConfigFile(rf.findFile("left_leg_file"),false);
if (!serverArmR.open(optServerArmR) || !serverArmL.open(optServerArmL))
{
close();
return false;
}
if (!noLegs)
{
if (!serverLegR.open(optServerLegR) || !serverLegL.open(optServerLegL))
{
close();
return false;
}
}
IMultipleWrapper *wrapperArmR, *wrapperArmL, *wrapperLegR, *wrapperLegL;
serverArmR.view(wrapperArmR);
serverArmL.view(wrapperArmL);
serverLegR.view(wrapperLegR);
serverLegL.view(wrapperLegL);
if (!wrapperArmR->attachAll(listArmR) || !wrapperArmL->attachAll(listArmL))
{
close();
return false;
}
if (!noLegs)
{
if (!wrapperLegR->attachAll(listLegR) || !wrapperLegL->attachAll(listLegL))
{
close();
return false;
}
}
return true;
}
bool speechInteraction::configure(ResourceFinder &rf)
{
moduleName = rf.check("name", Value("speechInteraction"), "module name (string)").asString();
setName(moduleName.c_str());
Property config;
config.fromConfigFile(rf.findFile("from").c_str());
Bottle &nGeneral = config.findGroup("number_of_vocabs");
nVocabs = nGeneral.find("nvocabs").asInt();
Bottle &speechGeneral = config.findGroup("speech_engine");
speechType = speechGeneral.find("engine").asInt();
cout << "Engine type: " << speechType << endl;
Bottle &inputGeneral = config.findGroup("input_vocabs");
string findVocab = "vocab_";
ostringstream convert;
cout << "INPUT VOCABS" << endl;
for( int i = 0; i < nVocabs; i++ )
{
convert << (i+1);
findVocab = findVocab + convert.str();
inputVocabs.push_back(inputGeneral.find(findVocab).asString().c_str());
cout << findVocab << ": " << inputGeneral.find(findVocab).asString().c_str() << endl;
findVocab = "vocab_";
convert.str("");
}
Bottle &outputGeneral = config.findGroup("output_vocabs");
cout << "OUTPUT VOCABS" << endl;
for( int i = 0; i < nVocabs; i++ )
{
convert << (i+1);
findVocab = findVocab + convert.str();
outputVocabs.push_back(outputGeneral.find(findVocab).asString().c_str());
cout << findVocab << ": " << outputGeneral.find(findVocab).asString().c_str() << endl;
findVocab = "vocab_";
convert.str("");
}
Bottle &portsGeneral = config.findGroup("ports");
inputPortName = portsGeneral.find("input_port").asString().c_str();
outputPortName = portsGeneral.find("output_port").asString().c_str();
triggerBehaviourPortName = portsGeneral.find("behaviour_port").asString().c_str();
inputOpen = inputPort.open(inputPortName.c_str());
outputOpen = outputPort.open(outputPortName.c_str());
behaviourPortOpen = triggerBehaviourPort.open(triggerBehaviourPortName.c_str());
if(!outputOpen || !inputOpen || !behaviourPortOpen)
{
cout << "Could not open ports. Exiting" << endl;
return false;
}
if( speechType == 1 )
{
cout << "DEBUG!!!!" << endl;
//moduleName = rf.check("name", Value("speechInteraction"), "module name (string)").asString();
//setName(moduleName.c_str());
GrammarAskNamePerson = rf.findFileByName("GrammarAskNamePerson.xml");
yInfo() << moduleName << " : finding configuration files...";
cout << "Grammar file: " << GrammarAskNamePerson;
//Create an iCub Client and check that all dependencies are here before starting
//bool isRFVerbose = false;
iCub = new ICubClient(moduleName,"speechInteraction","client.ini",true);
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);
if (!iCub->getRecogClient())
{
yInfo() << " WARNING SPEECH RECOGNIZER NOT CONNECTED";
}
}
return true;
}
bool configure(ResourceFinder &rf)
{
name = "virtualContactGeneration";
robot = "icubSim";
threadPeriod = 100; //period of the virtContactGenThread in ms
verbosity = 0;
type = "random"; //selection of the
SkinPart partOfSkin;
//******************************************************
//********************** CONFIGS ***********************
//******************* GENERAL GROUP ******************
Bottle &bGeneral=rf.findGroup("general");
bGeneral.setMonitor(rf.getMonitor());
//******************* NAME ******************
if (bGeneral.check("name"))
{
name = bGeneral.find("name").asString();
yInfo("Module name set to %s", name.c_str());
}
else yInfo("Module name set to default, i.e. %s", name.c_str());
setName(name.c_str());
//******************* ROBOT ******************
if (bGeneral.check("robot"))
{
robot = bGeneral.find("robot").asString();
yInfo("Robot is: %s", robot.c_str());
}
else yInfo("Could not find robot option in the config file; using %s as default",robot.c_str());
//****************** rate ******************
if (bGeneral.check("rate"))
{
threadPeriod = bGeneral.find("rate").asInt();
yInfo("virtContactGenThread rateThread working at %i ms.",threadPeriod);
}
else yInfo("Could not find rate in the config file; using %i ms as default period",threadPeriod);
//******************* VERBOSE ******************
if (bGeneral.check("verbosity"))
{
verbosity = bGeneral.find("verbosity").asInt();
yInfo("verbosity set to %i", verbosity);
}
else yInfo("Could not find verbosity option in the config file; using %i as default",verbosity);
//******************* TYPE ******************
if (bGeneral.check("type"))
{
type = bGeneral.find("type").asString();
yInfo("Type is: %s", type.c_str());
}
else yInfo("Could not find type option in the config file; using %s as default",type.c_str());
//*************** ACTIVE SKIN PARTS GROUP ****************
Bottle &bSkinParts=rf.findGroup("skin_parts");
if (!bSkinParts.isNull())
{
bSkinParts.setMonitor(rf.getMonitor());
if (bSkinParts.check("SKIN_LEFT_HAND"))
{
if(bSkinParts.find("SKIN_LEFT_HAND").asString()=="on")
{
partOfSkin = SKIN_LEFT_HAND;
activeSkinPartsNamesVector.push_back(partOfSkin);
yInfo("Adding SKIN_LEFT_HAND to active skin parts.");
}
}
else yInfo("Could not find [skin_parts] SKIN_LEFT_HAND option in the config file; set to default, i.e. off");
if (bSkinParts.check("SKIN_LEFT_FOREARM"))
{
if(bSkinParts.find("SKIN_LEFT_FOREARM").asString()=="on")
{
partOfSkin = SKIN_LEFT_FOREARM;
activeSkinPartsNamesVector.push_back(partOfSkin);
yInfo("Adding SKIN_LEFT_FOREARM to active skin parts.");
}
}
else yInfo("Could not find [skin_parts] SKIN_LEFT_FOREARM option in the config file; set to default, i.e. off");
if (bSkinParts.check("SKIN_LEFT_UPPER_ARM"))
{
if(bSkinParts.find("SKIN_LEFT_UPPER_ARM").asString()=="on")
{
partOfSkin = SKIN_LEFT_UPPER_ARM;
activeSkinPartsNamesVector.push_back(partOfSkin);
yInfo("Adding SKIN_LEFT_UPPER_ARM to active skin parts.");
}
}
else yInfo("Could not find [skin_parts] SKIN_LEFT_UPPER_ARM option in the config file; set to default, i.e. off");
if (bSkinParts.check("SKIN_RIGHT_HAND"))
{
if(bSkinParts.find("SKIN_RIGHT_HAND").asString()=="on")
{
partOfSkin = SKIN_RIGHT_HAND;
activeSkinPartsNamesVector.push_back(partOfSkin);
yInfo("Adding SKIN_RIGHT_HAND to active skin parts.");
}
}
else yInfo("Could not find [skin_parts] SKIN_RIGHT_HAND option in the config file; set to default, i.e. off");
if (bSkinParts.check("SKIN_RIGHT_FOREARM"))
{
if(bSkinParts.find("SKIN_RIGHT_FOREARM").asString()=="on")
{
partOfSkin = SKIN_RIGHT_FOREARM;
activeSkinPartsNamesVector.push_back(partOfSkin);
yInfo("Adding SKIN_RIGHT_FOREARM to active skin parts.");
}
}
else yInfo("Could not find [skin_parts] SKIN_RIGHT_FOREARM option in the config file; set to default, i.e. off");
if (bSkinParts.check("SKIN_RIGHT_UPPER_ARM"))
{
if(bSkinParts.find("SKIN_RIGHT_UPPER_ARM").asString()=="on")
{
partOfSkin = SKIN_RIGHT_UPPER_ARM;
activeSkinPartsNamesVector.push_back(partOfSkin);
yInfo("Adding SKIN_RIGHT_UPPER_ARM to active skin parts.");
}
}
else yInfo("Could not find [skin_parts] SKIN_RIGHT_UPPER_ARM option in the config file; set to default, i.e. off");
}
else{ //bSkinParts.isNull()
yInfo("Could not find [skin_parts] group in the config file; set all to default, i.e. off");
}
//*************** SKIN PARTS FILENAMES FROM SKIN MANAGER INI FILES ****************
ResourceFinder skinRF;
int partNum;
skinRF.setVerbose(false);
skinRF.setDefaultContext("skinGui"); //overridden by --context parameter
skinRF.setDefaultConfigFile("skinManAll.ini"); //overridden by --from parameter
skinRF.setVerbose(true);
skinRF.configure(0,NULL);
Bottle &skinEventsConf = skinRF.findGroup("SKIN_EVENTS");
if(!skinEventsConf.isNull())
{
yInfo("SKIN_EVENTS section found\n");
if(skinEventsConf.check("skinParts"))
{
Bottle* skinPartList = skinEventsConf.find("skinParts").asList();
partNum=skinPartList->size();
}
if(skinEventsConf.check("taxelPositionFiles"))
{
Bottle *taxelPosFiles = skinEventsConf.find("taxelPositionFiles").asList();
for(int i=0;i<partNum;i++) // all of the skinparts
{
string taxelPosFile = taxelPosFiles->get(i).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
if(verbosity>0) yInfo("[skin_event] filePath [%i in bottle] %s; setting under %s in skinPartsPositionsFilePath.\n",i,filePath.c_str(),SkinPart_s[i+1].c_str());
skinPartsPositionsFilePaths[(SkinPart)(i+1)] = filePath; //! Importantly, this is relying on the fact that the skin parts are in the
//right order in the .ini file, matching with SkinPart enum, and starting with 1 for left hand
}
}
}
else
{
yError(" No skin's configuration files found.");
return 0;
}
}
//******************************************************
//*********************** THREAD **********************
virtContactGenThrd = new virtContactGenerationThread(threadPeriod,name,robot,verbosity,type,activeSkinPartsNamesVector,skinPartsPositionsFilePaths);
if (!virtContactGenThrd -> start())
{
delete virtContactGenThrd;
virtContactGenThrd = 0;
yError("virtContactGenThrd wasn't instantiated!!");
return false;
}
yInfo("virtContactGenThrd instantiated...");
if (rf.check("autoconnect"))
{
if (Network::connect(("/"+name+"/virtualContacts:o").c_str(),"/doubleTouch/contacts:i"))
{
yInfo("Autoconnection to doubleTouch port, i.e. /doubleTouch/contacts:i, successful!");
}
}
return true;
}
virtual bool configure(ResourceFinder &rf)
{
string slash="/";
string ctrlName;
string robotName;
string partName;
string remoteName;
string localName;
Time::turboBoost();
// get params from the RF
ctrlName=rf.check("local",Value("fakeMobileBaseTest")).asString();
robotName=rf.check("robot",Value("fakeRobot")).asString();
partName = rf.check("part", Value("mobile_base")).asString();
bool holonomic = rf.check("holonomic");
if (holonomic) yInfo() << "Robot is holonomic";
else yInfo() << "Robot is not holonomic";
remoteName=slash+robotName+slash+partName;
localName=slash+ctrlName;
//reads the configuration file
Property ctrl_options;
ConstString configFile=rf.findFile("from");
if (configFile=="") //--from fakeMobileBaseTest.ini
{
yWarning("Cannot find .ini configuration file. By default I'm searching for fakeMobileBaseTest.ini");
// return false;
}
else
{
ctrl_options.fromConfigFile(configFile.c_str());
}
ctrl_options.put("remote", remoteName.c_str());
ctrl_options.put("local", localName.c_str());
int rc_count =0;
int rp_count =0;
int rf_count =0;
double start_time=yarp::os::Time::now();
bool not_yet_connected=true;
//set the thread rate
int period = rf.check("period",Value(20)).asInt();
yInfo("fakeMobileBaseTest thread rate: %d ms.",period);
//GENERAL options
yarp::os::Bottle general_options;
if (ctrl_options.check("GENERAL"))
{
general_options = ctrl_options.findGroup("GENERAL");
}
else
{
yError() << "Missing general_options section";
return false;
}
//initialize ROS
bool useRos = general_options.check("use_ROS", Value(false), "enable ROS communications").asBool();
if(useRos)
{
if (ctrl_options.check("ROS_GENERAL"))
{
string rosNodeName;
yarp::os::Bottle r_group = ctrl_options.findGroup("ROS_GENERAL");
if (r_group.check("node_name") == false)
{
yError() << "Missing node_name parameter"; return false;
}
rosNodeName = r_group.find("node_name").asString();
rosNode = new yarp::os::Node(rosNodeName);
yarp::os::Time::delay(0.1);
}
else
{
yError() << "[ROS_GENERAL] group is missing from configuration file. ROS communication will not be initialized";
}
}
//creates the input
input = new Input();
if (input->open(rf, ctrl_options) ==false)
{
yError() << "Module failed to launch";
return false;
}
//creates the controller
control = new Controller();
if (control->init(holonomic) == false)
{
yError() << "Module failed to launch";
return false;
}
if (ctrl_options.check("ODOMETRY_ERROR"))
{
yarp::os::Bottle error_group = ctrl_options.findGroup("ODOMETRY_ERROR");
if (error_group.check("x_gain") == false) { yError() << "Missing x_gain parameter"; return false; }
if (error_group.check("y_gain") == false) { yError() << "Missing y_gain parameter"; return false; }
if (error_group.check("t_gain") == false) { yError() << "Missing t_gain parameter"; return false; }
double odometry_error_x_gain = error_group.find("x_gain").asDouble();
double odometry_error_y_gain = error_group.find("y_gain").asDouble();
double odometry_error_t_gain = error_group.find("t_gain").asDouble();
control->set_odometry_error(odometry_error_x_gain,odometry_error_y_gain,odometry_error_t_gain);
}
//try to connect to joystickCtrl output
if (rf.check("joystick_connect"))
{
int joystick_trials = 0;
do
{
yarp::os::Time::delay(1.0);
if (yarp::os::Network::connect("/joystickCtrl:o", localName + "/joystick:i"))
{
yInfo("Joystick has been automatically connected");
break;
}
else
{
yWarning("Unable to find the joystick port, retrying (%d/5)...",joystick_trials);
joystick_trials++;
}
if (joystick_trials>=5)
{
yError("Unable to find the joystick port, giving up");
break;
}
}
while (1);
}
rpcPort.open((localName+"/rpc").c_str());
attach(rpcPort);
m_command.m_command_lin_dir = 0;
m_command.m_command_lin_vel = 0;
m_command.m_command_ang_vel = 0;
m_command.m_command_time = 0;
return true;
}
