bool actionClass::openFile2(std::string filename, yarp::os::ResourceFinder &rf)
{
bool ret = true;
FILE* data_file1 = 0;
FILE* data_file2 = 0;
FILE* data_file3 = 0;
string filename_left = filename + "_left" + ".txt";
string filename_right = filename + "_right" + ".txt";
string filename_torso = filename + "_torso" + ".txt";
filename_left = rf.findFile(filename_left);
filename_right = rf.findFile(filename_right);
filename_torso = rf.findFile(filename_torso);
fprintf(stderr, "||| File found for left leg: %s\n", filename_left.c_str());
fprintf(stderr, "||| File found for right leg: %s\n", filename_right.c_str());
fprintf(stderr, "||| File found for torso: %s\n", filename_torso.c_str());
data_file1 = fopen(filename_left.c_str(),"r");
data_file2 = fopen(filename_right.c_str(),"r");
data_file3 = fopen(filename_torso.c_str(),"r");
if (data_file1 != NULL && data_file2 != NULL && data_file3 != NULL)
{
char* bb1 = 0;
char* bb2 = 0;
char* bb3 = 0;
int line =0;
do
{
char trajectory_line1[1024];
char trajectory_line2[1024];
char trajectory_line3[1024];
bb1 = fgets (trajectory_line1, 1024, data_file1);
bb2 = fgets (trajectory_line2, 1024, data_file2);
bb3 = fgets (trajectory_line3, 1024, data_file3);
if (bb1 == 0 || bb2 == 0 || bb3 == 0) break;
if(!parseCommandLine2(trajectory_line1, trajectory_line2, trajectory_line3, line++))
{
printf ("error parsing file, line %d\n", line);
ret = false;
break;
};
}
while (1);
fclose (data_file1);
fclose (data_file2);
fclose (data_file3);
}
else
{
//file not opened
ret = false;
}
return ret;
}
bool RobotInterface::Module::configure(yarp::os::ResourceFinder &rf)
{
if (!rf.check("config")) {
yFatal() << "Missing \"config\" argument";
}
const yarp::os::ConstString &filename = rf.findFile("config");
yTrace() << "Reading robot config file" << filename;
RobotInterface::XMLReader reader;
mPriv->robot = reader.getRobot(filename.c_str());
// yDebug() << mPriv->robot;
// User can use YARP_PORT_PREFIX environment variable to override
// the default name, so we don't care of handling the --name
// argument
setName(mPriv->robot.portprefix().c_str());
// Enter startup phase
if (!mPriv->robot.enterPhase(RobotInterface::ActionPhaseStartup)) {
yError() << "Error in startup phase... see previous messages for more info";
return false;
}
return true;
}
bool AudioPowerMapModule::configure(yarp::os::ResourceFinder &rf) {
yInfo("Configuring the module");
// get the module name which will form the stem of all module port names
moduleName = rf.check("name", Value("/AudioPowerMap"), "module name (string)").asString();
// before continuing, set the module name before getting any other parameters,
// specifically the port names which are dependent on the module name
setName(moduleName.c_str());
// get the robot name which will form the stem of the robot ports names
// and append the specific part and device required
robotName = rf.check("robot", Value("icub"), "Robot name (string)").asString();
robotPortName = "/" + robotName + "/head";
inputPortName = rf.check("inputPortName", Value(":i"), "Input port name (string)").asString();
// attach a port of the same name as the module (prefixed with a /) to the module
// so that messages received from the port are redirected to the respond method
handlerPortName = "";
handlerPortName += getName();
if (!handlerPort.open(handlerPortName.c_str())) {
std::cout << getName() << ": Unable to open port " << handlerPortName << std::endl;
return false;
}
// attach to port
attach(handlerPort);
if (rf.check("config")) {
configFile=rf.findFile(rf.find("config").asString().c_str());
if (configFile=="") {
return false;
}
}
else {
configFile.clear();
}
// create the thread and pass pointers to the module parameters
apr = new AudioPowerMapRatethread(robotName, configFile, rf);
apr->setName(moduleName);
// now start the thread to do the work
// this calls threadInit() and it if returns true, it then calls run()
bool ret = apr->start();
// let the RFModule know everything went well
// so that it will then run the module
return ret;
}
bool 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;
}
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 eventDrivenModule::configure(yarp::os::ResourceFinder &rf) {
/* Process all parameters from both command-line and .ini file */
/* get the module name which will form the stem of all module port names */
moduleName = rf.check("name",
Value("/eventDriven"),
"module name (string)").asString();
/*
* before continuing, set the module name before getting any other parameters,
* specifically the port names which are dependent on the module name
*/
setName(moduleName.c_str());
/*
* get the robot name which will form the stem of the robot ports names
* and append the specific part and device required
*/
robotName = rf.check("robot",
Value("icub"),
"Robot name (string)").asString();
robotPortName = "/" + robotName + "/head";
inputPortName = rf.check("inputPortName",
Value("/AER:i"),
"Input port name (string)").asString();
/*
* attach a port of the same name as the module (prefixed with a /) to the module
* so that messages received from the port are redirected to the respond method
*/
handlerPortName = "";
handlerPortName += getName(); // use getName() rather than a literal
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
attach(handlerPort); // attach to port
if (rf.check("config")) {
configFile=rf.findFile(rf.find("config").asString().c_str());
if (configFile=="") {
return false;
}
}
else {
configFile.clear();
}
/* create the thread and pass pointers to the module parameters */
edThread = new eventDrivenThread(robotName, configFile);
edThread->setName(getName().c_str());
edThread->setInputPortName(inputPortName.c_str());
/* now start the thread to do the work */
edThread->start(); // this calls threadInit() and it if returns true, it then calls run()
return true ; // let the RFModule know everything went well
// so that it will then run the module
}
bool 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 asvGrabberModule::configure(yarp::os::ResourceFinder &rf) {
/* Process all parameters from both command-line and .ini file */
/* get the module name which will form the stem of all module port names */
moduleName = rf.check("name",
Value("/asvGrabber"),
"module name (string)").asString();
/*
* before continuing, set the module name before getting any other parameters,
* specifically the port names which are dependent on the module name
*/
setName(moduleName.c_str());
/*
* get the robot name which will form the stem of the robot ports names
* and append the specific part and device required
*/
robotName = rf.check("robot",
Value("icub"),
"Robot name (string)").asString();
robotPortName = "/" + robotName + "/head";
/*
* get the device name which will be used to read events
*/
deviceName = rf.check("deviceName",
Value("/dev/aerfx2_0"),
"Device name (string)").asString();
devicePortName = deviceName ;
printf("trying to connect to the device %s \n",devicePortName.c_str());
/*
* attach a port of the same name as the module (prefixed with a /) to the module
* so that messages received from the port are redirected to the respond method
*/
handlerPortName = "";
handlerPortName += getName(); // use getName() rather than a literal
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
attach(handlerPort); // attach to port
bool _save = false;
std::string deviceNum = "0";
/*
* get the file name of binaries when the biases are read from this file
*/
binaryName = rf.check("file",
Value("none"),
"filename of the binary (string)").asString();
printf("trying to read %s for biases \n",binaryName.c_str());
binaryNameComplete = rf.findFile(binaryName.c_str());
/*
* get the file name of binaries when the biases are read from this file
*/
dumpNameComplete = "";
dumpName = rf.check("dumpFile",
Value("none"),
"filename of the binary (string)").asString();
printf("trying to save events in %s \n",dumpName.c_str());
dumpNameComplete = rf.findFile(dumpName.c_str());
dumpPathDirectory = rf.getContextPath();
string pathDirectory = dumpPathDirectory.substr(0, 52);
pathDirectory += "/";
printf("saving directory %s \n",dumpPathDirectory.c_str());
if(!strcmp(binaryName.c_str(),"none")) {
printf("not reading from binary \n");
D2Y=new asvGrabberThread(devicePortName, false,binaryName);
//D2Y->setFromBinary(false);
}
else {
printf("reading from binary \n");
//D2Y->setFromBinary(true);
D2Y=new asvGrabberThread(devicePortName, true, binaryNameComplete);
//D2Y->setBinaryFile(f);
}
printf("dumpEventSet \n");
if (strcmp(dumpNameComplete.c_str(),"" )) {
printf("set dumping event true \n");
D2Y->setDumpEvent(true);
D2Y->setDumpFile(dumpNameComplete);
D2Y->setWorkingDirectory(pathDirectory);
}
else {
printf("set dumping event false \n");
D2Y->setDumpEvent(false);
}
printf("starting the processor \n");
D2Y->start();
return true ; // let the RFModule know everything went well
// so that it will then run the module
}
bool TorqueBalancingReferencesGenerator::configure (yarp::os::ResourceFinder &rf)
{
using namespace yarp::os;
using namespace yarp::sig;
using namespace Eigen;
if (!rf.check("wbi_config_file", "Checking wbi configuration file")) {
std::cout << "No WBI configuration file found.\n";
return false;
}
Property wbiProperties;
if (!wbiProperties.fromConfigFile(rf.findFile("wbi_config_file"))) {
std::cout << "Not possible to load WBI properties from file.\n";
return false;
}
wbiProperties.fromString(rf.toString(), false);
//retrieve the joint list
std::string wbiList = rf.find("wbi_joint_list").asString();
wbi::IDList iCubMainJoints;
if (!yarpWbi::loadIdListFromConfig(wbiList, wbiProperties, iCubMainJoints)) {
std::cout << "Cannot find joint list\n";
return false;
}
size_t actuatedDOFs = iCubMainJoints.size();
//create an instance of wbi
m_robot = new yarpWbi::yarpWholeBodyInterface("torqueBalancingReferencesGenerator", wbiProperties);
if (!m_robot) {
std::cout << "Could not create wbi object.\n";
return false;
}
m_robot->addJoints(iCubMainJoints);
if (!m_robot->init()) {
std::cout << "Could not initialize wbi object.\n";
return false;
}
robotName = rf.check("robot", Value("icubSim"), "Looking for robot name").asString();
// numberOfPostures = rf.check("numberOfPostures", Value(0), "Looking for numberOfPostures").asInt();
directionOfOscillation.resize(3,0.0);
frequencyOfOscillation = 0;
amplitudeOfOscillation = 0;
if (!loadReferences(rf,postures,comTimeAndSetPoints,actuatedDOFs,changePostural, changeComWithSetPoints,amplitudeOfOscillation,frequencyOfOscillation, directionOfOscillation ))
{
return false;
}
loadReferences(rf,postures,comTimeAndSetPoints,actuatedDOFs,changePostural, changeComWithSetPoints,amplitudeOfOscillation,frequencyOfOscillation, directionOfOscillation);
for(int i=0; i < postures.size(); i++ )
{
std::cerr << "[INFO] time_" << i << " = " << postures[i].time << std::endl;
std::cerr << "[INFO] posture_" << i << " = " << postures[i].qDes.toString()<< std::endl;
}
for(int i=0; i < comTimeAndSetPoints.size(); i++ )
{
std::cerr << "[INFO] time_" << i << " = " << comTimeAndSetPoints[i].time << std::endl;
std::cerr << "[INFO] com_" << i << " = " << comTimeAndSetPoints[i].comDes.toString()<< std::endl;
}
//
std::cout << "[INFO] Number of DOFs: " << actuatedDOFs << std::endl;
std::cerr << "[INFO] changePostural: " << changePostural << std::endl;
std::cerr << "[INFO] changeComWithSetPoints: " << changeComWithSetPoints << std::endl;
std::cerr << "[INFO] amplitudeOfOscillation: " << amplitudeOfOscillation << std::endl;
std::cout << "[INFO] frequencyOfOscillation " << frequencyOfOscillation << std::endl;
std::cerr << "[INFO] directionOfOscillation: " << directionOfOscillation.toString() << std::endl;
q0.resize(actuatedDOFs, 0.0);
com0.resize(3, 0.0);
comDes.resize(3, 0.0);
DcomDes.resize(3, 0.0);
DDcomDes.resize(3, 0.0);
m_robot->getEstimates(wbi::ESTIMATE_JOINT_POS, q0.data());
double world2BaseFrameSerialization[16];
double rotoTranslationVector[7];
wbi::Frame world2BaseFrame;
m_robot->getEstimates(wbi::ESTIMATE_BASE_POS, world2BaseFrameSerialization);
wbi::frameFromSerialization(world2BaseFrameSerialization, world2BaseFrame);
m_robot->forwardKinematics(q0.data(), world2BaseFrame, wbi::wholeBodyInterface::COM_LINK_ID, rotoTranslationVector);
com0[0] = rotoTranslationVector[0];
com0[1] = rotoTranslationVector[1];
com0[2] = rotoTranslationVector[2];
timeoutBeforeStreamingRefs = rf.check("timeoutBeforeStreamingRefs", Value(20), "Looking for robot name").asDouble();
period = rf.check("period", Value(0.01), "Looking for module period").asDouble();
std::cout << "timeoutBeforeStreamingRefs: " << timeoutBeforeStreamingRefs << "\n";
std::string group_name = "PORTS_INFO";
if( !rf.check(group_name) )
{
std::cerr << "[ERR] no PORTS_INFO group found" << std::endl;
return true;
}
else
{
yarp::os::Bottle group_bot = rf.findGroup(group_name);
if( !group_bot.check("portNameForStreamingQdes") ||
!group_bot.check("portNameForStreamingComDes") )
{
std::cerr << "[ERR] portNameForStreamingQdes or portNameForStreamingComDes not found in config file" << std::endl;
return false;
}
//Check coupling configuration is well formed
std::string portNameForStreamingComDes = group_bot.find("portNameForStreamingComDes").asString();
std::cout << "portNameForStreamingComDes: " << portNameForStreamingComDes << "\n";
std::string portNameForStreamingQdes = group_bot.find("portNameForStreamingQdes").asString();
std::cout << "portNameForStreamingQdes: " << portNameForStreamingQdes << "\n";
portForStreamingComDes.open(portNameForStreamingComDes);
portForStreamingQdes.open(portNameForStreamingQdes);
t0 = yarp::os::Time::now();
return true;
}
}
bool targetFinderModule::configure(yarp::os::ResourceFinder &rf) {
/* Process all parameters from both command-line and .ini file */
/* get the module name which will form the stem of all module port names */
moduleName = rf.check("name",
Value("/targetFinder"),
"module name (string)").asString();
/*
* before continuing, set the module name before getting any other parameters,
* specifically the port names which are dependent on the module name
*/
setName(moduleName.c_str());
/*
* get the robot name which will form the stem of the robot ports names
* and append the specific part and device required
*/
robotName = rf.check("robot",
Value("icub"),
"Robot name (string)").asString();
robotPortName = "/" + robotName + "/head";
configName = rf.check("config",
Value("icubEyes.ini"),
"Config file for intrinsic parameters (string)").asString();
printf("configFile: %s \n", configName.c_str());
if (strcmp(configName.c_str(),"")) {
printf("looking for the config file \n");
configFile=rf.findFile(configName.c_str());
printf("config file %s \n", configFile.c_str());
if (configFile=="") {
printf("ERROR: file not found");
return false;
}
}
else {
configFile.clear();
}
/*
* set the operating mode which correspond as well with the file map saved in conf
*/
mapName = rf.check("mode",
Value("intensity"),
"file map name (string)").asString();
mapName += ".txt";
mapNameComplete = rf.findFile(mapName.c_str());
/*
* attach a port of the same name as the module (prefixed with a /) to the module
* so that messages received from the port are redirected to the respond method
*/
handlerPortName = "";
handlerPortName += getName(); // use getName() rather than a literal
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
attach(handlerPort); // attach to port
tf = new targetFinderThread();
tf->setMapURL(mapNameComplete);
tf->setRobotName(robotName);
tf->setConfigFile(configFile);
tf->setName(getName().c_str());
tf->start();
return true ; // let the RFModule know everything went well
// so that it will then run the module
}
bool ImageSource::configure(yarp::os::ResourceFinder &rf)
{
debug = true;
/* Process all parameters from both command-line and .ini file */
/* get the module name which will form the stem of all module port names */
moduleName = rf.check("name",
Value("imageSource"),
"module name (string)").asString();
/*
* before continuing, set the module name before getting any other parameters,
* specifically the port names which are dependent on the module name
*/
setName(moduleName.c_str());
/* now, get the rest of the parameters */
/*
* get the imageFilename
*/
imageFilename = rf.check("imageFile",
Value("image.bmp"),
"image filename (string)").asString();
imageFilename = (rf.findFile(imageFilename.c_str())).c_str();
/* get the complete name of the image output port */
outputPortName = rf.check("outputPort",
Value("/image:o"),
"Output image port (string)").asString();
/* get the complete name of the gaze output port */
gazePortName = rf.check("gazePort",
Value("/gaze:o"),
"Output gaze port (string)").asString();
/* get the complete name of the encoder state output port */
encoderPortName = rf.check("encoderPort",
Value("/icub/head/state:o"),
"Output encoder port (string)").asString();
/* get the frequency, width, height, standard deviation, horizontalViewAngle, and verticalViewAngle values */
frequency = rf.check("frequency",
Value(10),
"frequency key value (int)").asInt();
width = rf.check("width",
Value(320),
"output width key value (int)").asInt();
height = rf.check("height",
Value(240),
"output height key value (int)").asInt();
noiseLevel = rf.check("noise",
Value(20),
"noise level key value (int)").asInt();
window = rf.check("window",
Value(0),
"window flag key value (int)").asInt();
random = rf.check("random",
Value(0),
"random flag key value (int)").asInt();
xShift = rf.check("xShift",
Value(5),
"horizontal shift key value (int)").asInt();
yShift = rf.check("yShift",
Value(5),
"vertical shift key value (int)").asInt();
horizontalViewAngle = rf.check("horizontalViewAngle",
Value(120.0),
"horizontal field of view angle key value (double)").asDouble();
verticalViewAngle = rf.check("verticalViewAngle",
Value(90.0),
"vertical field of view angle key value (double)").asDouble();
if (debug) {
cout << "imageSource::configure: image file name " << imageFilename << endl;
cout << "imageSource::configure: output port name " << outputPortName << endl;
cout << "imageSource::configure: gaze port name " << gazePortName << endl;
cout << "imageSource::configure: encoder port name " << encoderPortName << endl;
cout << "imageSource::configure: frequency " << frequency << endl;
cout << "imageSource::configure: width " << width << endl;
cout << "imageSource::configure: height " << height << endl;
cout << "imageSource::configure: noise level " << noiseLevel << endl;
cout << "imageSource::configure: window flag " << window << endl;
cout << "imageSource::configure: random flag " << random << endl;
cout << "imageSource::configure: x shift " << xShift << endl;
cout << "imageSource::configure: y shift " << yShift << endl;
cout << "imageSource::configure: horizontal FoV " << horizontalViewAngle << endl;
cout << "imageSource::configure: vertical FoV " << verticalViewAngle << endl;
}
/* do all initialization here */
/* open ports */
if (!imageOut.open(outputPortName.c_str())) {
cout << "imageSource::configure" << ": unable to open port " << outputPortName << endl;
return false; // unable to open; let RFModule know so that it won't run
}
if (!gazeOut.open(gazePortName.c_str())) {
cout << "imageSource::configure" << ": unable to open port " << gazePortName << endl;
return false; // unable to open; let RFModule know so that it won't run
}
if (!encoderOut.open(encoderPortName.c_str())) {
cout << "imageSource::configure" << ": unable to open port " << encoderPortName << endl;
return false; // unable to open; let RFModule know so that it won't run
}
/*
* attach a port of the same name as the module (prefixed with a /) to the module
* so that messages received from the port are redirected to the respond method
*/
handlerPortName = "/";
handlerPortName += getName(); // use getName() rather than a literal
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
attach(handlerPort); // attach to port
/* create the thread and pass pointers to the module parameters */
//cout << "imageSource::configure: calling Thread constructor" << endl;
imageSourceThread = new ImageSourceThread(&imageOut, &gazeOut, &encoderOut, &imageFilename,
(int)(1000 / frequency), &width, &height, &noiseLevel, &window, &random,
&xShift, &yShift,
&horizontalViewAngle, &verticalViewAngle);
//cout << "imageSource::configure: returning from Thread constructor" << endl;
/* now start the thread to do the work */
imageSourceThread->start(); // this calls threadInit() and it if returns true, it then calls run()
return true; // let the RFModule know everything went well
// so that it will then run the module
}
bool ObserverModule::configure(yarp::os::ResourceFinder &rf) {
/* Process all parameters from both command-line and .ini file */
/* get the module name which will form the stem of all module port names */
moduleName = rf.check("name",
Value(""),
"module name (string)").asString();
/*
* before continuing, set the module name before getting any other parameters,
* specifically the port names which are dependent on the module name
*/
// setName(moduleName.c_str());
/*
* get the robot name which will form the stem of the robot ports names
* and append the specific part and device required
*/
robotName = rf.check("robot",
Value("icub"),
"Robot name (string)").asString();
//robotPortName = "/" + robotName + "/head";
inputPortName = rf.check("inputPortName",
Value(":i"),
"Input port name (string)").asString();
/*
* attach a port of the same name as the module (prefixed with a /) to the module
* so that messages received from the port are redirected to the respond method
*/
handlerPortName = "";
handlerPortName += getName(); // use getName() rather than a literal
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
attach(handlerPort); // attach to port
if (rf.check("config")) {
configFile=rf.findFile(rf.find("config").asString().c_str());
if (configFile=="") {
return false;
}
}
else {
configFile.clear();
}
bool returnval = true;
//create the thread and configure it using the ResourceFinder
_effectorThread = new EffectorThread();
returnval &= _effectorThread->configure(rf);
_affordanceAccess = new darwin::observer::AffordanceAccessImpl();
if (returnval) {
returnval &= _affordanceAccess->configure(rf);
}
_attentionAccess = new darwin::observer::AttentionAccessImpl();
if (returnval) {
returnval &= _attentionAccess->configure(rf);
}
_workspace = new darwin::observer::WorkspaceCalculationsImpl();
if (returnval) {
returnval &= _workspace->configure(rf);
}
/* create the thread and pass pointers to the module parameters */
rThread = new ObserverThread(robotName, configFile);
rThread->setName(getName().c_str());
rThread->setEffectorAccess(_effectorThread);
rThread->setAffordanceAccess(_affordanceAccess);
rThread->setAttentionAccess(_attentionAccess);
rThread->setWorkspaceAccess(_workspace);
const string pt = rf.findPath("observerConfig.ini");
unsigned pos = pt.find("observerConfig.ini");
pathPrefix = pt.substr(0,pos);
printf("observer configuraion file in %s \n", pathPrefix.c_str());
//////////////////////// find file paths
///////////input files
rThread->setPath(pathPrefix);
// rThread->setColorPath(colormapFile);
// rThread->setModelPath(modelFile);
//=======================================================================
//
//rThread->setInputPortName(inputPortName.c_str());
if (returnval) {
returnval &= _effectorThread->start();
}
/* now start the thread to do the work */
if (returnval) {
returnval &= rThread->start(); // this calls threadInit() and it if returns true, it then calls run()
}
return returnval; // let the RFModule know everything went well
// so that it will then run the module
}
bool demoModule::configure(yarp::os::ResourceFinder &rf) {
/*
* PLEASE remove useless comments when writing actual code. If needed then use Doxygen comments and tags.
*/
/* Process all parameters from both command-line and .ini file */
/* get the module name which will form the stem of all module port names */
moduleName = rf.check("name",
Value("demoModule"),
"module name (string)").asString();
/*
* before continuing, set the module name before getting any other parameters,
* specifically the port names which are dependent on the module name
*/
setName(moduleName.c_str());
/* now, get the rest of the parameters */
/*
* get the robot name which will form the stem of the robot ports names
* and append the specific part and device required
*/
robotName = rf.check("robot",
Value("icub"),
"Robot name (string)").asString();
robotPortName = "/" + robotName + "/head";
/*
* get the cameraConfig file and read the required parameter values cx, cy
* in both the groups [CAMERA_CALIBRATION_LEFT] and [CAMERA_CALIBRATION_RIGHT]
*/
cameraConfigFilename = rf.check("cameraConfig",
Value("icubEyes.ini"),
"camera configuration filename (string)").asString();
cameraConfigFilename = (rf.findFile(cameraConfigFilename.c_str())).c_str();
Property cameraProperties;
if (cameraProperties.fromConfigFile(cameraConfigFilename.c_str()) == false) {
cout << "myModule: unable to read camera configuration file" << cameraConfigFilename;
return 0;
}
else {
cxLeft = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_LEFT").check("cx", Value(160.0), "cx left").asDouble();
cyLeft = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_LEFT").check("cy", Value(120.0), "cy left").asDouble();
cxRight = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_RIGHT").check("cx", Value(160.0), "cx right").asDouble();
cyRight = (float) cameraProperties.findGroup("CAMERA_CALIBRATION_RIGHT").check("cy", Value(120.0), "cy right").asDouble();
}
/* get the name of the input and output ports, automatically prefixing the module name by using getName() */
inputPortName = "/";
inputPortName += getName(
rf.check("myInputPort",
Value("/demoModule/image:i"),
"Input image port (string)").asString()
);
outputPortName = "/";
outputPortName += getName(
rf.check("myOutputPort",
Value("/demoModule/image:o"),
"Output image port (string)").asString()
);
/* get the threshold value */
thresholdValue = rf.check("threshold",
Value(8),
"Key value (int)").asInt();
/* do all initialization here */
/*
* attach a port of the same name as the module (prefixed with a /) to the module
* so that messages received from the port are redirected to the respond method
*/
handlerPortName = "/";
handlerPortName += getName(); // use getName() rather than a literal
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
attach(handlerPort); // attach to port
/* create the thread and pass pointers to the module parameters */
dThread = new demoThread(&thresholdValue);
/* now start the thread to do the work */
dThread->start(); // this calls threadInit() and it if returns true, it then calls run()
return true ; // let the RFModule know everything went well
// so that it will then run the module
}
bool wingsTranslatorModule::configure(yarp::os::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("--config : changes the eye config file e.g. icubEyes.ini");
printf("--robot : changes the name of the robot where the module interfaces to \n");
printf("--tableHeight : changes the reference height of the plane for homography");
printf("--kinWingsLeft : look for the kinematic constraint of the camera ");
printf("--kinWingsRight: look for the kinematic constraint of the camera ");
printf("press CTRL-C to continue.. \n");
return true;
}
/* get the module name which will form the stem of all module port names */
moduleName = rf.check("name",
Value("/wingsTranslator"),
"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";
/* setting the table height for homography */
tableHeight = rf.check("tableHeight",
Value(0.12),
"sets the plane z-axis height for homography (double)").asDouble();
printf("tableHeight: %f \n", tableHeight);
/* eyes config file for projection into the image plane */
configName = rf.check("config",
Value("icubEyes.ini"),
"Config file for intrinsic parameters (string)").asString();
printf("configFile: %s \n", configName.c_str());
if (strcmp(configName.c_str(),"")) {
printf("looking for the config file \n");
configFile=rf.findFile(configName.c_str());
printf("config file %s \n", configFile.c_str());
if (configFile=="") {
printf("file not found; the program will proceed with standard values \n");
return false;
}
}
else {
configFile.clear();
}
/*******************************************************************************************************/
isOnWings = true;
printf("trying to read the kinematic chain for the left \n");
wingsLeftName = rf.check("kinWingLeft",
Value("null"), //wingsKinematic.ini"
"Config file for kinematics left wing (string)").asString();
printf("wingLeftName: %s \n", wingsLeftName.c_str());
if (strcmp(wingsLeftName.c_str(),"null")) {
printf("looking for the wingsLeft file \n");
wingsLeftFile=rf.findFile(wingsLeftName.c_str());
printf("wings left file %s \n", wingsLeftFile.c_str());
if (wingsLeftFile=="") {
printf("ERROR: file not found; the program will proceed with standard values \n");
isOnWings = false;
//return false;
}
}
else {
printf("left : setting isOnWings false because not found \n");
isOnWings = false;
wingsLeftFile.clear();
}
/******************************************************************************************************/
printf("trying to read the kinematic chain for the right \n");
wingsRightName = rf.check("kinWingRight",
Value("null"), //wingsKinematic.ini"
"Config file for kinematics right wing (string)").asString();
printf("wingRightName: %s \n", wingsRightName.c_str());
if (strcmp(wingsRightName.c_str(),"null")) {
printf("looking for the wingsRight file \n");
wingsRightFile=rf.findFile(wingsRightName.c_str());
printf("wings right file %s \n", wingsRightFile.c_str());
if (wingsRightFile=="") {
printf("ERROR: file kinematic right eye not found; the program will proceed with standard values \n");
isOnWings = false;
//return false;
}
}
else {
printf("left : setting isOnWings false because not found \n");
isOnWings = false;
wingsRightFile.clear();
}
/*
* attach a port of the same name as the module (prefixed with a /) to the module
* so that messages received from the port are redirected to the respond method
*/
handlerPortName = "";
handlerPortName += getName(); // use getName() rather than a literal
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
attach(handlerPort); // attach to port
tf = new wingsTranslatorThread();
tf->setMapURL(mapNameComplete);
tf->setRobotName(robotName);
tf->setConfigFile(configFile);
if(isOnWings) {
printf("setting the isOnWings TRUE in the module \n");
tf->setIsOnWings(true);
tf->setWingsLeftFile(wingsLeftFile);
tf->setWingsRightFile(wingsRightFile);
}
else {
printf("setting the isOnWings FALSE in the module \n");
tf->setIsOnWings(false);
}
tf->setTableHeight(tableHeight);
tf->setName(getName().c_str());
tf->start();
return true ; // let the RFModule know everything went well
// so that it will then run the module
}
bool efExtractorModule::configure(yarp::os::ResourceFinder &rf) {
/* Process all parameters from both command-line and .ini file */
if(rf.check("help")) {
printf("Help \n");
printf("==== \n");
printf("--name : name of the module \n");
printf("--mode : (intensity) mapping to be used \n");
printf("--bottleHanlder : the user select to send events only through bottle port esclusively \n");
printf("--verbose : saves relevant information in files \n");
printf("--plotLatency : saves relevant information about latency \n");
printf("press CTRL-C to continue... \n");
return true;
}
/* get the module name which will form the stem of all module port names */
moduleName = rf.check("name",
Value("/eventFeatureExtractor"),
"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";
/*
* set the operating mode which correspond as well with the file map saved in conf
*/
mapName = rf.check("mode",
Value("intensity"),
"file map name (string)").asString();
mapName += ".txt";
mapNameComplete = rf.findFile(mapName.c_str());
/*
* attach a port of the same name as the module (prefixed with a /) to the module
* so that messages received from the port are redirected to the respond method
*/
handlerPortName = "";
handlerPortName += getName(); // use getName() rather than a literal
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
attach(handlerPort); // attach to port
efeThread = new efExtractorThread();
efeThread->setMapURL(mapNameComplete);
efeThread->setName(getName().c_str());
/*
*checking whether the user wants exclusively to send events as bottles
*/
if( rf.check("bottleHandler")) {
printf("--------------------------->set the bottleHandler flag true \n");
efeThread->setBottleHandler(true);
}
else {
printf("--------------------------->set the bottleHandler flag false \n");
efeThread->setBottleHandler(false);
}
if(rf.check("verbose")) {
efeThread->setVERBOSE(true);
}
else {
efeThread->setVERBOSE(false);
}
if(rf.check("plotLatency")) {
efeThread->setPlotLatency(true);
}
else {
efeThread->setPlotLatency(false);
}
if(!efeThread->start()) {
return false;
}
return true ; // let the RFModule know everything went well
// so that it will then run the module
}
