bool swTeleop::SWIcubArm::init( yarp::os::ResourceFinder &oRf, bool bLeftArm)
{
if(m_bInitialized)
{
std::cerr << "Icub Arm is already initialized. " << std::endl;
return true;
}
if(bLeftArm)
{
m_sArm = "left";
}
else
{
m_sArm = "right";
}
// gets the module name which will form the stem of all module port names
m_sModuleName = oRf.check("name", yarp::os::Value("teleoperation_iCub"), "Teleoperation/iCub Module name (string)").asString();
m_sRobotName = oRf.check("robot",yarp::os::Value("icubSim"), "Robot name (string)").asString();
// robot parts to control
m_bArmHandActivated = oRf.check(std::string(m_sArm + "ArmHandActivated").c_str(), yarp::os::Value(m_bArmHandActivatedDefault), std::string(m_sArm + " Arm/hand activated (int)").c_str()).asInt() != 0;
m_bFingersActivated = oRf.check(std::string(m_sArm + "FingersActivated").c_str(), yarp::os::Value(m_bFingersActivatedDefault), std::string(m_sArm + " Fingers activated (int)").c_str()).asInt() != 0;
m_i32RateVelocityControl = oRf.check("armsRateVelocityControl", yarp::os::Value(m_i32RateVelocityControlDefault), "Arms rate velocity control (int)").asInt();
if(!m_bArmHandActivated && !m_bFingersActivated)
{
std::cout << m_sArm + " arm/hand and " + m_sArm +" fingers not activated, icub " + m_sArm + " arm initialization aborted. " << std::endl;
return (m_bInitialized = false);
}
// min / max values for iCub Torso joints
for(uint ii = 0; ii < m_vArmJointVelocityAcceleration.size(); ++ii)
{
std::ostringstream l_os;
l_os << ii;
std::string l_sMinJoint(m_sArm + "ArmMinValueJoint" + l_os.str());
std::string l_sMaxJoint(m_sArm + "ArmMaxValueJoint" + l_os.str());
std::string l_sArmResetPosition(m_sArm + "ArmResetPosition" + l_os.str());
std::string l_sArmJointVelocityAcceleration(m_sArm + "ArmJointVelocityAcceleration" + l_os.str());
std::string l_sArmJointVelocityK(m_sArm + "ArmJointVelocityK" + l_os.str());
std::string l_sArmJointPositionAcceleration(m_sArm + "ArmJointPositionAcceleration" + l_os.str());
std::string l_sArmJointPositionSpeed(m_sArm + "ArmJointPositionSpeed" + l_os.str());
std::string l_sMinJointInfo(m_sArm + " arm minimum joint" + l_os.str() + " Value (double)");
std::string l_sMaxJointInfo(m_sArm + " arm maximum joint" + l_os.str() + " Value (double)");
std::string l_sArmResetPositionInfo(m_sArm + " arm reset position " + l_os.str() + " Value (double)");
std::string l_sArmJointVelocityAccelerationInfo(m_sArm + " arm joint velocity acceleration " + l_os.str() + " Value (double)");
std::string l_sArmJointVelocityKInfo(m_sArm + " arm joint velocity K coeff"+ l_os.str() + " Value (double)");
std::string l_sArmJointPositionAccelerationInfo(m_sArm + " arm joint position acceleration " + l_os.str() + " Value (double)");
std::string l_sArmJointPositionSpeedInfo(m_sArm + " arm joint position speed " + l_os.str() + " Value (double)");
m_vArmMinJoint[ii] = oRf.check(l_sMinJoint.c_str(), m_vArmMinJointDefault[ii], l_sMinJointInfo.c_str()).asDouble();
m_vArmMaxJoint[ii] = oRf.check(l_sMaxJoint.c_str(), m_vArmMaxJointDefault[ii], l_sMaxJointInfo.c_str()).asDouble();
m_vArmResetPosition[ii] = oRf.check(l_sArmResetPosition.c_str(), m_vArmResetPositionDefault[ii], l_sArmResetPositionInfo.c_str()).asDouble();
m_vArmJointVelocityAcceleration[ii]= oRf.check(l_sArmJointVelocityAcceleration.c_str(), m_vArmJointVelocityAccelerationDefault[ii], l_sArmJointVelocityAccelerationInfo.c_str()).asDouble();
m_vArmJointPositionAcceleration[ii]= oRf.check(l_sArmJointPositionAcceleration.c_str(), m_vArmJointPositionAccelerationDefault[ii], l_sArmJointPositionAccelerationInfo.c_str()).asDouble();
m_vArmJointPositionSpeed[ii] = oRf.check(l_sArmJointPositionSpeed.c_str(), m_vArmJointPositionSpeedDefault[ii], l_sArmJointPositionSpeedInfo.c_str()).asDouble();
m_vArmJointVelocityK[ii] = oRf.check(l_sArmJointVelocityK.c_str(), m_vArmJointVelocityKDefault[ii], l_sArmJointVelocityKInfo.c_str()).asDouble();
}
// miscellaneous
m_i32TimeoutArmReset = oRf.check(std::string(m_sArm + "ArmTimeoutReset").c_str(), yarp::os::Value(m_i32TimeoutArmResetDefault), std::string(m_sArm + " arm timeout reset iCub (int)").c_str()).asInt();
// set polydriver options
m_oArmOptions.put("robot", m_sRobotName.c_str());
m_oArmOptions.put("device", "remote_controlboard");
m_oArmOptions.put("local", ("/local/" + m_sRobotName + "/" + m_sArm + "_arm").c_str());
m_oArmOptions.put("name", ("/" + m_sRobotName + "/" + m_sArm + "_arm").c_str());
m_oArmOptions.put("remote", ("/" + m_sRobotName + "/" + m_sArm + "_arm").c_str());
// init polydriver
m_oRobotArm.open(m_oArmOptions);
if(!m_oRobotArm.isValid())
{
std::cerr << std::endl <<"-ERROR: " << m_sArm << " robotArm is not valid, escape arm initialization. " << std::endl <<std::endl;
return (m_bInitialized=false);
}
// initializing controllers
if (!m_oRobotArm.view(m_pIArmVelocity) || !m_oRobotArm.view(m_pIArmPosition) || !m_oRobotArm.view(m_pIArmEncoders))
{
std::cerr << std::endl << "-ERROR: " << m_sArm << " while getting required robot Arm interfaces." << std::endl <<std::endl;
m_oRobotArm.close();
return (m_bInitialized=false);
}
// init ports
m_sHandTrackerPortName = "/teleoperation/" + m_sRobotName + "/" + m_sArm + "_arm/hand";
m_sHandFingersTrackerPortName = "/teleoperation/" + m_sRobotName + "/" + m_sArm + "_arm/hand_fingers";
// open ports
bool l_bPortOpeningSuccess = true;
if(m_bArmHandActivated || m_bFingersActivated)
{
l_bPortOpeningSuccess = m_oHandFingersTrackerPort.open(m_sHandFingersTrackerPortName.c_str());
if(l_bPortOpeningSuccess)
l_bPortOpeningSuccess = m_oHandTrackerPort.open(m_sHandTrackerPortName.c_str());
}
if(!l_bPortOpeningSuccess)
{
std::cerr << std::endl <<"-ERROR: Unable to open ports." << std::endl <<std::endl;
m_oRobotArm.close();
return (m_bInitialized=false);
}
// retrieve Torso number of joints
m_pIArmPosition->getAxes(&m_i32ArmJointsNb);
// set accelerations/speeds
for(int ii = 0; ii < m_i32ArmJointsNb; ++ii)
{
m_pIArmPosition->setRefAcceleration(ii, m_vArmJointPositionAcceleration[ii]);
m_pIArmPosition->setRefSpeed(ii, m_vArmJointPositionSpeed[ii]);
m_pIArmVelocity->setRefAcceleration(ii, m_vArmJointVelocityAcceleration[ii]);
}
// init controller
m_pVelocityController = new swTeleop::SWArmVelocityController(m_pIArmEncoders, m_pIArmVelocity, m_vArmJointVelocityK, m_i32RateVelocityControl);
m_pVelocityController->enable(m_bArmHandActivated, m_bFingersActivated);
// display parameters
std::cout << std::endl << std::endl;
displayDebug(m_sArm + std::string(" arm/hand activated"), m_bArmHandActivated);
displayDebug(m_sArm + std::string(" fingers activated"), m_bFingersActivated);
displayDebug(std::string("Gaze activated"), m_i32TimeoutArmReset);
displayDebug(std::string("Rate velocity control"), m_i32RateVelocityControl);
std::cout << std::endl;
displayVectorDebug(m_sArm + std::string(" arm min joint : "), m_vArmMinJoint);
displayVectorDebug(m_sArm + std::string(" arm max joint : "), m_vArmMaxJoint);
displayVectorDebug(m_sArm + std::string(" arm reset position joint : "), m_vArmResetPosition);
displayVectorDebug(m_sArm + std::string(" arm joint velocity acceleration: "), m_vArmJointVelocityAcceleration);
displayVectorDebug(m_sArm + std::string(" arm joint position acceleration: "), m_vArmJointPositionAcceleration);
displayVectorDebug(m_sArm + std::string(" arm joint position speed : "), m_vArmJointPositionSpeed);
displayVectorDebug(m_sArm + std::string(" arm joint velocity : "), m_vArmJointVelocityK);
std::cout << std::endl << std::endl;
return (m_bIsRunning=m_bInitialized=true);
}
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 faceTrackerModule::configure(yarp::os::ResourceFinder &rf) {
moduleName = rf.check("name",
Value("faceTracker"),
"module name (string)").asString();
setName(moduleName.c_str());
opcName = rf.check("opcName",
Value("OPC"),
"Opc name (string)").asString();
string xmlPath = rf.findFileByName("haarcascade_frontalface_default.xml");
// Create an iCub Client and check that all dependencies are here befor starting
opc = new OPCClient(moduleName.c_str());
opc->connect(opcName);
icub = NULL;
handlerPortName = "/";
handlerPortName += getName() + "/rpc";
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
// ==================================================================
// image port open
imagePortLeft.open("/facetracking/image/left/in"); // give the left port a name
// ==================================================================
// robot
// ==================================================================
while(!Network::connect("/icub/camcalib/left/out", "/facetracking/image/left/in"))
{
Time::delay(3);
cout << "try to connect left camera, please wait ..." << endl;
}
Property options;
options.put("device", "remote_controlboard");
options.put("local", "/tutorial/motor/client");
options.put("remote", "/icub/head");
robotHead = new PolyDriver(options);
if(!robotHead->isValid())
{
cout << "Cannot connect to the robot head" << endl;
return false;
}
robotHead->view(pos);
robotHead->view(vel);
robotHead->view(enc);
if(pos==NULL || vel==NULL || enc==NULL)
{
cout << "Cannot get interface to robot head" << endl;
robotHead->close();
return false;
}
jnts = 0;
pos->getAxes(&jnts);
setpoints.resize(jnts);
cur_encoders.resize(jnts);
prev_encoders.resize(jnts);
/* prepare command */
for(int i=0;i<jnts;i++)
{
setpoints[i] = 0;
}
// ==================================================================
//// create a opencv window
cv::namedWindow("cvImage_Left",CV_WINDOW_NORMAL);
// ==================================================================
// face detection configuration
face_classifier_left.load(xmlPath);
attach(handlerPort); // attach to port
// ==================================================================
// Parameters
counter = 0;
x_buf = 0;
y_buf = 0;
mode = 0; // 0: going to a set position, 1: face searching, 2: face tracking, 3: face stuck,
setpos_counter = 0;
panning_counter = 0;
stuck_counter = 0;
tracking_counter = 0;
// ==================================================================
// random motion
tilt_target = 0;
pan_target = 0;
seed = 10000;
srand(seed);
pan_max = 80;
tilt_max = 20;
cvIplImageLeft = NULL;
return true ;
}
bool tutorialModule::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("/tutorial"),
"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();
}
/* create the thread and pass pointers to the module parameters */
rThread = new tutorialRatethread(robotName, configFile);
rThread->setName(getName().c_str());
//rThread->setInputPortName(inputPortName.c_str());
/* now start the thread to do the work */
rThread->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 SimToolLoaderModule::configure(yarp::os::ResourceFinder &rf) {
Bottle table;
Bottle temp;
string objectName = "obj";
/* module name */
moduleName = rf.check("name", Value("simtoolloader"),
"Module name (string)").asString();
setName(moduleName.c_str());
/* Hand used */
hand=rf.find("hand").asString().c_str();
if ((hand!="left") && (hand!="right"))
hand="right";
/* port names */
simToolLoaderSimOutputPortName = "/";
simToolLoaderSimOutputPortName += getName( rf.check("simToolLoaderSimOutputPort",
Value("/sim:rpc"),
"Loader output port(string)")
.asString() );
simToolLoaderCmdInputPortName = "/";
simToolLoaderCmdInputPortName += getName( rf.check("simToolLoaderCmdInputPort",
Value("/cmd:i"),
"Loader input port(string)")
.asString() );
/* open ports */
if (!simToolLoaderSimOutputPort.open(
simToolLoaderSimOutputPortName.c_str())) {
cout << getName() << ": unable to open port"
<< simToolLoaderSimOutputPortName << endl;
return false;
}
if (!simToolLoaderCmdInputPort.open(
simToolLoaderCmdInputPortName.c_str())) {
cout << getName() << ": unable to open port"
<< simToolLoaderCmdInputPortName << endl;
return false;
}
/* Rate thread period */
threadPeriod = rf.check("threadPeriod", Value(0.5),
"Control rate thread period key value(double) in seconds ").asDouble();
/* Read Table Configuration */
table = rf.findGroup("table");
/* Read the Objects configurations */
vector<Bottle> object;
cout << "Loading objects to buffer" << endl;
bool noMoreModels = false;
int n =0;
while (!noMoreModels){ // read until there are no more objects.
stringstream s;
s.str("");
s << objectName << n;
string objNameNum = s.str();
temp = rf.findGroup("objects").findGroup(objNameNum);
if(!temp.isNull()) {
cout << "object" << n << ", id: " << objNameNum;
cout << ", model: " << temp.get(2).asString() << endl;
object.push_back(temp);
temp.clear();
n++;
}else {
noMoreModels = true;
}
}
numberObjs = n;
cout << "Loaded " << object.size() << " objects " << endl;
/* Create the control rate thread */
ctrlThread = new CtrlThread(&simToolLoaderSimOutputPort,
&simToolLoaderCmdInputPort,
threadPeriod,
table, object,hand);
/* Starts the thread */
if (!ctrlThread->start()) {
delete ctrlThread;
return false;
}
return true;
}
bool skinManager::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_DEFAULT.c_str()), "module name (string)").asString();
robotName = rf.check("robot", Value(ROBOT_NAME_DEFAULT.c_str()), "name of the robot (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 some other values from the configuration file */
int period = (int)rf.check("period", Value(PERIOD_DEFAULT),
"Period of the thread in ms (positive int)").asInt();
float minBaseline = (float)rf.check("minBaseline", Value(MIN_BASELINE_DEFAULT),
"If the baseline reaches this value then, if allowed, a calibration is executed (float in [0,255])").asDouble();
float compGain = (float)rf.check("compensationGain", Value(COMPENSATION_GAIN_DEFAULT),
"Gain of the compensation algorithm (float)").asDouble();
float contCompGain = (float)rf.check("contactCompensationGain", Value(CONTACT_COMPENSATION_GAIN_DEFAULT),
"Gain of the compensation algorithm during contact (float)").asDouble();
int addThreshold = (int)rf.check("addThreshold", Value(ADD_THRESHOLD_DEFAULT),
"Value added to all the touch thresholds (positive int)").asInt();
bool zeroUpRawData = rf.check("zeroUpRawData", ZERO_UP_RAW_DATA_DEFAULT,
"if true the raw data are considered from zero up, otherwise from 255 down (bool)").asBool();
bool smoothFilter = rf.check("smoothFilter", SMOOTH_FILTER_DEFAULT,
"if true then the smoothing filter is active (bool)").asBool();
float smoothFactor = (float) rf.check("smoothFactor", Value(SMOOTH_FACTOR_DEFAULT),
"Determine the smoothing intensity (float in [0,1])").asDouble();
bool binarization = rf.check("binarization", BINARIZATION_DEFAULT,
"if true then the binarization is active (bool)").asBool();
/*
* 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
*/
string handlerPortName = "/";
handlerPortName += getName(rf.check("handlerPort", Value(RPC_PORT_DEFAULT.c_str())).asString());
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
attach(handlerPort); // attach to port
handlerPort.setRpcMode(true);
/* create the thread and pass pointers to the module parameters */
myThread = new CompensationThread(moduleName, &rf, robotName, compGain, contCompGain, addThreshold, minBaseline,
zeroUpRawData, period, binarization, smoothFilter, smoothFactor);
/* now start the thread to do the work */
return myThread->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 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 BehaviorManager::configure(yarp::os::ResourceFinder &rf)
{
moduleName = rf.check("name",Value("BehaviorManager")).asString();
setName(moduleName.c_str());
yInfo()<<moduleName<<": finding configuration files...";//<<endl;
period = rf.check("period",Value(1.0)).asDouble();
Bottle grp = rf.findGroup("BEHAVIORS");
behaviorList = *grp.find("behaviors").asList();
rpc_in_port.open("/" + moduleName + "/trigger:i");
yInfo() << "RPC_IN : " << rpc_in_port.getName();
for (int i = 0; i<behaviorList.size(); i++)
{
behavior_name = behaviorList.get(i).asString();
if (behavior_name == "tagging") {
behaviors.push_back(new Tagging(&mut, rf, "tagging"));
} else if (behavior_name == "pointing") {
behaviors.push_back(new Pointing(&mut, rf, "pointing"));
} else if (behavior_name == "dummy") {
behaviors.push_back(new Dummy(&mut, rf, "dummy"));
} else if (behavior_name == "dummy2") {
behaviors.push_back(new Dummy(&mut, rf, "dummy2"));
} else if (behavior_name == "reactions") {
behaviors.push_back(new Reactions(&mut, rf, "reactions"));
} else if (behavior_name == "followingOrder") {
behaviors.push_back(new FollowingOrder(&mut, rf, "followingOrder"));
} else if (behavior_name == "narrate") {
behaviors.push_back(new Narrate(&mut, rf, "narrate"));
} else if (behavior_name == "recognitionOrder") {
behaviors.push_back(new recognitionOrder(&mut, rf, "recognitionOrder"));
// other behaviors here
} else {
yDebug() << "Behavior " + behavior_name + " not implemented";
return false;
}
}
//Create an iCub Client and check that all dependencies are here before starting
bool isRFVerbose = false;
iCub = new ICubClient(moduleName, "behaviorManager","client.ini",isRFVerbose);
if (!iCub->connect())
{
yInfo() << "iCubClient : Some dependencies are not running...";
Time::delay(1.0);
}
while (!Network::connect("/ears/behavior:o", rpc_in_port.getName())) {
yWarning() << "Ears is not reachable";
yarp::os::Time::delay(0.5);
}
// id = 0;
for(auto& beh : behaviors) {
beh->configure();
beh->openPorts(moduleName);
beh->iCub = iCub;
if (beh->from_sensation_port_name != "None") {
while (!Network::connect(beh->from_sensation_port_name, beh->sensation_port_in.getName())) {
yInfo()<<"Connecting "<< beh->from_sensation_port_name << " to " << beh->sensation_port_in.getName();// <<endl;
yarp::os::Time::delay(0.5);
}
}
if (beh->external_port_name != "None") {
while (!Network::connect(beh->rpc_out_port.getName(), beh->external_port_name)) {
yInfo()<<"Connecting "<< beh->rpc_out_port.getName() << " to " << beh->external_port_name;// <<endl;
yarp::os::Time::delay(0.5);
}
}
}
attach(rpc_in_port);
yInfo("Init done");
return true;
}
bool EndogenousSalience::configure(yarp::os::ResourceFinder &rf)
{
/*
* Process all parameters from
* - command-line
* - endogenousSalience.ini file (or whatever file is specified by the --from argument)
*/
/* get the module name which will form the stem of all module port names */
moduleName = rf.check("name",
Value("endogenousSalience"),
"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 name of the input and output ports, automatically prefixing the module name by using getName() */
cartesianInputPortName = "/";
cartesianInputPortName += getName(
rf.check("cartesianImageInPort",
Value("/cartesianImage:i"),
"Input image port (string)").asString()
);
logpolarInputPortName = "/";
logpolarInputPortName += getName(
rf.check("logpolarImageInPort",
Value("/logpolarImage:i"),
"Exemplar image port (string)").asString()
);
salienceOutputPortName = "/";
salienceOutputPortName += getName(
rf.check("salienceImageOutPort",
Value("/salienceImage:o"),
"Left output image port (string)").asString()
);
hueTolerance = rf.check("hueTolerance",
Value(10),
"Tolerance for hue value (int)").asInt();
saturationTolerance = rf.check("saturationTolerance",
Value(10),
"Tolerance for saturation value (int)").asInt();
hueBins = rf.check("hueBins",
Value(32),
"Number of hue bins in HS histogram (int)").asInt();
saturationBins = rf.check("saturationBins",
Value(32),
"Number of saturation bins in HS histogram (int)").asInt();
filterRadius = rf.check("filterRadius",
Value(10),
"Radius of the morphological opening filter (int)").asInt();
if (debug) {
printf("endogenousSalience: module name is %s\n",moduleName.c_str());
printf("endogenousSalience: robot name is %s\n",robotName.c_str());
printf("endogenousSalience: hue and saturation tolerances are %d %d\n",hueTolerance,saturationTolerance);
printf("endogenousSalience: hue and saturation bins are %d %d\n",hueBins,saturationBins);
printf("endogenousSalience: port names are\n%s\n%s\n%s\n\n",cartesianInputPortName.c_str(),
logpolarInputPortName.c_str(),
salienceOutputPortName.c_str());
}
/* do all initialization here */
/* open ports */
if (!cartesianImageIn.open(cartesianInputPortName.c_str())) {
cout << getName() << ": unable to open port " << cartesianInputPortName << endl;
return false; // unable to open; let RFModule know so that it won't run
}
if (!logpolarImageIn.open(logpolarInputPortName.c_str())) {
cout << getName() << ": unable to open port " << logpolarInputPortName << endl;
return false; // unable to open; let RFModule know so that it won't run
}
if (!salienceImageOut.open(salienceOutputPortName.c_str())) {
cout << getName() << ": unable to open port " << salienceOutputPortName << 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 */
endogenousSalienceThread = new EndogenousSalienceThread(&cartesianImageIn,
&logpolarImageIn,
&salienceImageOut,
&hueTolerance,
&saturationTolerance,
&hueBins,
&saturationBins,
&filterRadius);
/* now start the thread to do the work */
endogenousSalienceThread->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 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 fileFeederModule::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("/fileFeeder"),
"module name (string)").asString();
/*
* before continuing, set the module name before getting any other parameters,
* specifically the port names which are dependent on the module name
*/
setName(moduleName.c_str());
/*
* get the robot name which will form the stem of the robot ports names
* and append the specific part and device required
*/
robotName = rf.check("robot",
Value("icub"),
"Robot name (string)").asString();
robotPortName = "/" + robotName + "/head";
/*
* attach a port of the same name as the module (prefixed with a /) to the module
* so that messages received from the port are redirected to the respond method
*/
handlerPortName = "";
handlerPortName += getName(); // use getName() rather than a literal
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
attach(handlerPort); // attach to port
ffThread=new fileFeederThread();
ffThread->setName(getName().c_str());
ffThread->start();
/* get the filename start which will form the name of the read images */
filenameStart = rf.check("filenameStart",
Value("."),
"module name (string)").asString();
ffThread->setStartFilename(filenameStart);
/* get the filename end which will form the name of the read images */
filenameEnd = rf.check("filenameEnd",
Value(").jpg"),
"module name (string)").asString();
ffThread->setLastFilename(filenameEnd);
/* get the number of images that are going to be read */
numberImages = rf.check("numberImages",
Value(100),
"module name (int)").asInt();
ffThread->setNumberOfImages(numberImages);
ffThread->setStartFilename(filenameStart);
return true ; // let the RFModule know everything went well
// so that it will then run the module
}
bool TrainModule::configure(yarp::os::ResourceFinder& opt) {
/* Implementation note:
* Calling open() in the base class (i.e. PredictModule) is cumbersome due
* to different ordering and dynamic binding (e.g. it calls
* registerAllPorts()) and because we do bother with an incoming model port.
*/
// read for the general specifiers:
yarp::os::Value* val;
std::string machineName;
// cache resource finder
this->setResourceFinder(&opt);
// check for help request
if(opt.check("help")) {
this->printOptions();
return false;
}
// check for algorithm listing request
if(opt.check("list")) {
this->printMachineList();
return false;
}
// check for port specifier: portSuffix
if(opt.check("port", val)) {
this->portPrefix = val->asString().c_str();
}
// check for filename to load machine from
if(opt.check("load", val)) {
this->getMachinePortable().readFromFile(val->asString().c_str());
} else{
// not loading anything, require machine name
if(opt.check("machine", val)) {
machineName = val->asString().c_str();
} else {
this->printOptions("No machine type specified");
return false;
}
// construct new machine
this->getMachinePortable().setWrapped(machineName);
// send configuration options to the machine
this->getMachine().configure(opt);
}
// add replier for incoming data (prediction requests)
this->predict_inout.setReplier(this->predictProcessor);
// add processor for incoming data (training samples)
this->train_in.useCallback(trainProcessor);
// register ports before connecting
this->registerAllPorts();
// and finally load command file
if(opt.check("commands", val)) {
this->loadCommandFile(val->asString().c_str());
}
// attach to the incoming command port and terminal
this->attach(cmd_in);
this->attachTerminal();
return true;
}
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 learnPrimitive::configure(yarp::os::ResourceFinder &rf)
{
string moduleName = rf.check("name", Value("learnPrimitive")).asString().c_str();
setName(moduleName.c_str());
GrammarYesNo = rf.findFileByName(rf.check("GrammarYesNo", Value("nodeYesNo.xml")).toString());
GrammarNameAction = rf.findFileByName(rf.check("GrammarNameAction", Value("GrammarNameAction.xml")).toString());
GrammarTypeAction = rf.findFileByName(rf.check("GrammarTypeAction", Value("GrammarTypeAction.xml")).toString());
yInfo() << "findFileByName " << rf.findFileByName("learnPrimitive.ini") ;
pathToIniFile = rf.findFileByName("learnPrimitive.ini") ;
cout << moduleName << ": finding configuration files..." << endl;
period = rf.check("period", Value(0.1)).asDouble();
//bool bEveryThingisGood = true;
//Create an iCub Client and check that all dependencies are here before starting
bool isRFVerbose = true;
iCub = new ICubClient(moduleName, "learnPrimitive", "client.ini", isRFVerbose);
iCub->opc->isVerbose &= true;
string robot = rf.check("robot", Value("icubSim")).asString().c_str();
string arm = rf.check("arm", Value("left_arm")).asString().c_str();
portToArm.open(("/" + moduleName + "/toArm:rpc").c_str());
string portRobotArmName = "/" + robot + "/" + arm + "/rpc:i";
yInfo() << "================> port controlling the arm : " << portRobotArmName;
if (!Network::connect(portToArm.getName().c_str(),portRobotArmName))
{
yWarning() << "WARNING PORT TO CONTROL ARM (" << portRobotArmName << ") IS NOT CONNECTED";
}
if (!iCub->connect())
{
cout << "iCubClient : Some dependencies are not running..." << endl;
Time::delay(1.0);
}
//rpc port
rpcPort.open(("/" + moduleName + "/rpc").c_str());
attach(rpcPort);
if (!iCub->getRecogClient())
{
yWarning() << "WARNING SPEECH RECOGNIZER NOT CONNECTED";
}
if (!iCub->getABMClient())
{
yWarning() << "WARNING ABM NOT CONNECTED";
}
updateProtoAction(rf);
updatePrimitive(rf);
updateAction(rf);
yInfo() << "\n \n" << "----------------------------------------------" << "\n \n" << moduleName << " ready ! \n \n ";
iCub->say("learn Primitive is ready", false);
return true;
}
bool lfCollectorModule::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("/logpolarFrameCollector"),
"module name (string)").asString();
/*
* before continuing, set the module name before getting any other parameters,
* specifically the port names which are dependent on the module name
*/
setName(moduleName.c_str());
/*
* get the robot name which will form the stem of the robot ports names
* and append the specific part and device required
*/
robotName = rf.check("robot",
Value("icub"),
"Robot name (string)").asString();
robotPortName = "/" + robotName + "/head";\
/*
* attach a port of the same name as the module (prefixed with a /) to the module
* so that messages received from the port are redirected to the respond method
*/
handlerPortName = "";
handlerPortName += getName(); // use getName() rather than a literal
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
attach(handlerPort); // attach to port
// --------------------------------------------
lfThread=new lfCollectorThread();
lfThread->setName(getName().c_str());
/*
* set the period between two successive synchronisations between viewer and events
*/
synchPeriod = rf.check("synchPeriod",
Value(10000),
"synchronisation period (int)").asInt();
lfThread->setSynchPeriod(synchPeriod);
/*
* set the retinaSize (considering squared retina)
*/
retinalSize = rf.check("retinalSize",
Value(128),
"retinalSize (int)").asInt();
lfThread->setRetinalSize(retinalSize);
/* checking whether the module synchronizes with single camera or stereo camera
*/
if( rf.check("stereo")) {
lfThread->setStereo(true);
}
else {
lfThread->setStereo(false);
}
/* checking whether the module synchronizes with single camera or stereo camera
*/
if( rf.check("em")) {
lfThread->setEM(true);
}
else {
lfThread->setEM(false);
}
/**
* checking whether the viewer represent log-polar information
*/
if( rf.check("logpolar")) {
lfThread->setLogPolar(true);
}
else {
lfThread->setLogPolar(false);
}
lfThread->start();
return true ; // let the RFModule know everything went well
// so that it will then run the module
}
bool cfCollectorModule::configure(yarp::os::ResourceFinder &rf) {
/* Process all parameters from both command-line and .ini file */
if(rf.check("help")) {
printf("HELP \n");
printf("=======");
printf("--name (string) : specifies the rootname\n");
printf("--robot (string) : indicates the name of the robot to connect to\n");
printf("--retinalSize (int) : defines the dimension of the retina (input)\n");
printf("--responseGradient (int) : the increment for any single event in the register\n");
printf("--sychPeriod (int) : period for synchronization of the variable lastTimestamp\n");
printf("--windowSize (int) : size of the window where events are collected \n ");
printf("--stereo : if present both left and right events are represented \n ");
printf("--bottleHanlder : the user select to send events only through bottle port esclusively \n");
printf("--verbose : enable debug savings of events in files");
printf("\n press CTRL-C to continue \n");
return true;
}
printf("initialization of the main thread \n");
/* get the module name which will form the stem of all module port names */
moduleName = rf.check("name",
Value("/cartesianFrameCollector"),
"module name (string)").asString();
/*
* before continuing, set the module name before getting any other parameters,
* specifically the port names which are dependent on the module name
*/
setName(moduleName.c_str());
/*
* get the robot name which will form the stem of the robot ports names
* and append the specific part and device required
*/
robotName = rf.check("robot",
Value("icub"),
"Robot name (string)").asString();
robotPortName = "/" + robotName + "/head";\
/*
* attach a port of the same name as the module (prefixed with a /) to the module
* so that messages received from the port are redirected to the respond method
*/
handlerPortName = "";
handlerPortName += getName(); // use getName() rather than a literal
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
printf("attaching handler port2 \n");
attach(handlerPort); // attach to port
// --------------------------------------------
printf("starting cfCollector Thread \n");
cfThread=new cfCollectorThread();
cfThread->setName(getName().c_str());
printf("name of the cfThread correctly set \n");
/*
* set the period between two successive synchronisations between viewer and events
*/
synchPeriod = rf.check("synchPeriod",
Value(10000),
"synchronisation period (int)").asInt();
cfThread->setSynchPeriod(synchPeriod);
/*
* set the windowSize (dimension of temporal window collected events)
*/
windowSize = rf.check("windowSize",
Value(10),
"windowSize (int)").asInt();
printf("looking for the windowSize; found value %d \n", windowSize);
cfThread->setWindowSize(windowSize);
/*
* set the retinaSize (considering squared retina)
*/
printf("looking for the retinalSize \n");
retinalSize = rf.check("retinalSize",
Value(128),
"retinalSize (int)").asInt();
cfThread->setRetinalSize(retinalSize);
/*
* set the retinaSize (considering squared retina)
*/
printf("looking for the responseGradient \n");
responseGradient = rf.check("responseGradient",
Value(127),
"responseGradient (int)").asInt();
cfThread->setResponseGradient(responseGradient);
/*
*checking whether the module synchronizes with single camera or stereo camera
*/
if( rf.check("stereo")) {
cfThread->setStereo(true);
}
else {
cfThread->setStereo(false);
}
/*
*checking whether the user wants exclusively to send events as bottles
*/
if( rf.check("bottleHandler")) {
printf("set the bottleHandler flag true \n");
cfThread->setBottleHandler(true);
}
else {
printf("set the bottleHandler flag false \n");
cfThread->setBottleHandler(false);
}
/*
*checking whether the user wants exclusively to send events as bottles
*/
if( rf.check("verbose")) {
printf("set the verbose mode for all the components \n");
cfThread->setVerbose(true);
}
else {
printf("verbose mode deactivated \n");
cfThread->setVerbose(false);
}
/*
*set option for mapping three states into 3baseline graylevels
*/
if( rf.check("tristate")) {
cfThread->setTristate(true);
}
else {
cfThread->setTristate(false);
}
/*
*checking whether the module synchronizes with single camera or stereo camera
*/
printf("Looking for asvMode ... \n");
if( rf.check("asvMode")) {
printf("setting asvMode = true, dvsMode = false \n");
cfThread->setASVMode(true);
cfThread->setDVSMode(false);
}
else {
printf("setting asvMode = false, dvsMode = false \n");
cfThread->setASVMode(false);
}
/* checking whether the module synchronizes with single camera or stereo camera
*/
if( rf.check("dvsMode")) {
cfThread->setDVSMode(true);
cfThread->setASVMode(false);
}
else {
cfThread->setDVSMode(false);
}
/**
* checking whether the viewer represent log-polar information
*/
if( rf.check("logpolar")) {
cfThread->setLogPolar(true);
}
else {
cfThread->setLogPolar(false);
}
cfThread->start();
return true ; // let the RFModule know everything went well
// so that it will then run the module
}
bool rd::RobotDevastation::configure(yarp::os::ResourceFinder &rf)
{
//-- Show help
//printf("--------------------------------------------------------------\n");
if (rf.check("help")) {
printf("RobotDevastation options:\n");
printf("\t--help (this help)\t--from [file.ini]\t--context [path]\n");
printf("\t--id integer\n");
printf("\t--name string\n");
printf("\t--team integer\n");
printf("\t--robotName string\n");
// Do not exit: let last layer exit so we get help from the complete chain.
}
printf("RobotDevastation using no additional special options.\n");
//-- Get player data
//-----------------------------------------------------------------------------
if( ! rf.check("id") )
{
RD_ERROR("No id!\n");
return false;
}
RD_INFO("id: %d\n",rf.find("id").asInt());
if( ! rf.check("name") )
{
RD_ERROR("No name!\n");
return false;
}
RD_INFO("name: %s\n",rf.find("name").asString().c_str());
if( ! rf.check("team") )
{
RD_ERROR("No team!\n");
return false;
}
RD_INFO("team: %d\n",rf.find("team").asInt());
if( ! rf.check("robotName") )
{
RD_ERROR("No robotName!\n");
return false;
}
RD_INFO("robotName: %s\n",rf.find("robotName").asString().c_str());
//-- Init mentalMap
mentalMap = RdMentalMap::getMentalMap();
mentalMap->configure( rf.find("id").asInt() );
std::vector< RdPlayer > players;
players.push_back( RdPlayer(rf.find("id").asInt(),std::string(rf.find("name").asString()),100,100,rf.find("team").asInt(),0) );
mentalMap->updatePlayers(players);
mentalMap->addWeapon(RdWeapon("Default gun", 10, 5));
//-- Init input manager
RdSDLInputManager::RegisterManager();
inputManager = RdInputManager::getInputManager("SDL");
inputManager->addInputEventListener(this);
if (!inputManager->start() )
{
RD_ERROR("Could not init inputManager!\n");
return false;
}
//-- Init sound
if( ! initSound( rf ) )
return false;
if( ! rf.check("noMusic") )
audioManager->play("bso", -1);
//-- Init robot
if( rf.check("mockupRobotManager") ) {
robotManager = new RdMockupRobotManager(rf.find("robotName").asString());
} else {
robotManager = new RdYarpRobotManager(rf.find("robotName").asString());
}
if( ! robotManager->connect() ) {
RD_ERROR("Could not connect to robotName \"%s\"!\n",rf.find("robotName").asString().c_str());
RD_ERROR("Use syntax: robotDevastation --robotName %s\n",rf.find("robotName").asString().c_str());
return false;
}
//-- Init network manager
RdYarpNetworkManager::RegisterManager();
networkManager = RdYarpNetworkManager::getNetworkManager(RdYarpNetworkManager::id);
networkManager->addNetworkEventListener(mentalMap);
mentalMap->addMentalMapEventListener((RdYarpNetworkManager *)networkManager);
networkManager->login(mentalMap->getMyself());
//-- Init image manager
if( rf.check("mockupImageManager") ) {
RdMockupImageManager::RegisterManager();
imageManager = RdImageManager::getImageManager(RdMockupImageManager::id);
} else {
RdYarpImageManager::RegisterManager();
imageManager = RdImageManager::getImageManager(RdYarpImageManager::id);
}
//-- Add the image processing listener to the image manager
imageManager->addImageEventListener(&processorImageEventListener);
//-- Configure the camera port
std::ostringstream remoteCameraPortName; //-- Default looks like "/0/rd1/img:o"
remoteCameraPortName << "/";
remoteCameraPortName << rf.find("robotName").asString();
remoteCameraPortName << "/img:o";
imageManager->configure("remote_img_port", remoteCameraPortName.str() );
std::ostringstream localCameraPortName; //-- Default should look like "/0/robot/img:i"
localCameraPortName << "/";
localCameraPortName << rf.find("id").asInt();
localCameraPortName << "/robot/img:i";
imageManager->configure("local_img_port", localCameraPortName.str() ); //-- Name given by me
if( ! imageManager->start() )
return false;
//-- Init output thread
rateThreadOutput.init(rf);
return true;
}
bool CamCalibModule::configure(yarp::os::ResourceFinder &rf)
{
ConstString str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
setName(str.c_str()); // modulePortName
double maxDelay = rf.check("maxDelay", Value(0.010), "Max delay between image and encoders").asDouble();
// pass configuration over to bottle
Bottle botConfig(rf.toString().c_str());
botConfig.setMonitor(rf.getMonitor());
// Load from configuration group ([<group_name>]), if group option present
Value *valGroup; // check assigns pointer to reference
if(botConfig.check("group", valGroup, "Configuration group to load module options from (string).")) {
strGroup = valGroup->asString().c_str();
// is group a valid bottle?
if (botConfig.check(strGroup.c_str())){
Bottle &group=botConfig.findGroup(strGroup.c_str(),string("Loading configuration from group " + strGroup).c_str());
botConfig.fromString(group.toString());
} else {
yError() << "Group " << strGroup << " not found.";
return false;
}
} else {
yError ("There seem to be an error loading parameters (group section missing), stopping module");
return false;
}
string calibToolName = botConfig.check("projection",
Value("pinhole"),
"Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
_calibTool = CalibToolFactories::getPool().get(calibToolName.c_str());
if (_calibTool!=NULL) {
bool ok = _calibTool->open(botConfig);
if (!ok) {
delete _calibTool;
_calibTool = NULL;
return false;
}
}
if (yarp::os::Network::exists(getName("/in"))) {
yWarning() << "port " << getName("/in") << " already in use";
}
if (yarp::os::Network::exists(getName("/out"))) {
yWarning() << "port " << getName("/out") << " already in use";
}
if (yarp::os::Network::exists(getName("/conf"))) {
yWarning() << "port " << getName("/conf") << " already in use";
}
_prtImgIn.setSaturation(rf.check("saturation",Value(1.0)).asDouble());
_prtImgIn.open(getName("/in"));
_prtImgIn.setPointers(&_prtImgOut,_calibTool);
_prtImgIn.setVerbose(rf.check("verbose"));
_prtImgIn.setLeftEye((strGroup == "CAMERA_CALIBRATION_LEFT") ? true : false);
_prtImgIn.setMaxDelay(maxDelay);
_prtImgIn.setUseIMU(rf.check("useIMU"));
_prtImgIn.setUseIMU(rf.check("useTorso"));
_prtImgIn.setUseIMU(rf.check("useEyes"));
_prtImgIn.useCallback();
_prtImgOut.open(getName("/out"));
_configPort.open(getName("/conf"));
_prtTEncsIn.open(getName("/torso_encs/in"));
_prtTEncsIn._prtImgIn = &_prtImgIn;
// _prtTEncsIn.setStrict();
_prtTEncsIn.useCallback();
_prtHEncsIn.open(getName("/head_encs/in"));
_prtHEncsIn._prtImgIn = &_prtImgIn;
// _prtHEncsIn.setStrict();
_prtHEncsIn.useCallback();
_prtImuIn.open(getName("/imu/in"));
_prtImuIn._prtImgIn = &_prtImgIn;
// _prtImuIn.setStrict();
_prtImuIn.useCallback();
attach(_configPort);
fflush(stdout);
_prtImgIn.rpyPort.open(getName("/rpy"));
return true;
}
bool CamCalibModule::configure(yarp::os::ResourceFinder &rf)
{
ConstString str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
verboseExecTime = rf.check("verboseExecTime");
if (rf.check("w_align")) align=ALIGN_WIDTH;
else if (rf.check("h_align")) align=ALIGN_HEIGHT;
if (rf.check("fps"))
{
requested_fps=rf.find("fps").asDouble();
yInfo() << "Module will run at " << requested_fps;
}
else
{
yInfo() << "Module will run at max fps";
}
setName(str.c_str()); // modulePortName
Bottle botLeftConfig(rf.toString().c_str());
Bottle botRightConfig(rf.toString().c_str());
botLeftConfig.setMonitor(rf.getMonitor());
botRightConfig.setMonitor(rf.getMonitor());
string strLeftGroup = "CAMERA_CALIBRATION_LEFT";
if (botLeftConfig.check(strLeftGroup.c_str()))
{
Bottle &group=botLeftConfig.findGroup(strLeftGroup.c_str(),string("Loading configuration from group " + strLeftGroup).c_str());
botLeftConfig.fromString(group.toString());
}
else
{
yError() << "Group " << strLeftGroup << " not found.";
return false;
}
string strRightGroup = "CAMERA_CALIBRATION_RIGHT";
if (botRightConfig.check(strRightGroup.c_str()))
{
Bottle &group=botRightConfig.findGroup(strRightGroup.c_str(),string("Loading configuration from group " + strRightGroup).c_str());
botRightConfig.fromString(group.toString());
}
else
{
yError() << "Group " << strRightGroup << " not found.";
return false;
}
string calibToolLeftName = botLeftConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
string calibToolRightName = botRightConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
calibToolLeft = CalibToolFactories::getPool().get(calibToolLeftName.c_str());
if (calibToolLeft!=NULL)
{
bool ok = calibToolLeft->open(botLeftConfig);
if (!ok)
{
delete calibToolLeft;
calibToolLeft = NULL;
return false;
}
}
calibToolRight = CalibToolFactories::getPool().get(calibToolRightName.c_str());
if (calibToolRight!=NULL)
{
bool ok = calibToolRight->open(botRightConfig);
if (!ok)
{
delete calibToolRight;
calibToolRight = NULL;
return false;
}
}
if(rf.check("dual"))
{
dualImage_mode = true;
yInfo() << "Dual mode activate!!";
}
if(dualImage_mode)
{
leftImage = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;
rightImage = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;
// open a single port with name /dual:i
if (yarp::os::Network::exists(getName("/dual:i")))
{
yWarning() << "port " << getName("/dual:i") << " already in use";
}
if(! imageInLeft.open(getName("/dual:i")) )
return false;
imageInLeft.setStrict(false);
}
else
{
if (yarp::os::Network::exists(getName("/left:i")))
{
yWarning() << "port " << getName("/left:i") << " already in use";
}
if (yarp::os::Network::exists(getName("/right:i")))
{
yWarning() << "port " << getName("/right:i") << " already in use";
}
imageInLeft.open(getName("/left:i"));
imageInRight.open(getName("/right:i"));
imageInLeft.setStrict(false);
imageInRight.setStrict(false);
}
if (yarp::os::Network::exists(getName("/out")))
{
yWarning() << "port " << getName("/out") << " already in use";
}
if (yarp::os::Network::exists(getName("/conf")))
{
yWarning() << "port " << getName("/conf") << " already in use";
}
imageOut.open(getName("/out:o"));
configPort.open(getName("/conf"));
attach(configPort);
return true;
}
bool objectGeneratorSim::configure(yarp::os::ResourceFinder &rf)
{
Matrix iH;
iH = zeros(4,4);
iH(0,1)=-1;
iH(1,2)=1;
iH(1,3)=0.5976;
iH(2,0)=-1;
iH(2,3)=-0.026;
iH(3,3)=1;
H=SE3inv(iH);
elapsedCycles = 0;
listSize[0]=0;
listSize[1]=0;
listSize[2]=0;
string moduleName = rf.check("name", Value("objectGeneratorSim")).asString().c_str();
//setName(moduleName.c_str());
string moduleOutput = rf.check("output", Value("/obstacles:o")).asString().c_str();
string moduleOutputTarget = rf.check("output", Value("/reachingTarget:o")).asString().c_str();
string moduleInput = "/pos:i";
bool bEveryThingisGood = true;
string port2icubsim = "/" + moduleName + "/sim:o";
if (!portSim.open(port2icubsim.c_str())) {
cout << getName() << ": Unable to open port " << port2icubsim << endl;
bEveryThingisGood = false;
}
// create an output port
std::string port2output = "/" + moduleName + moduleOutput;
if (!portOutput.open(port2output.c_str())) {
cout << getName() << ": Unable to open port " << port2output << endl;
bEveryThingisGood = false;
}
std::string port2outputT = "/" + moduleName + moduleOutputTarget;
if (!portOutputTarget.open(port2outputT.c_str())) {
cout << getName() << ": Unable to open port " << port2outputT << endl;
bEveryThingisGood = false;
}
std::string port2input = "/" + moduleName + moduleInput;
if (!portInput.open(port2input.c_str())) {
cout << getName() << ": Unable to open port " << port2input << endl;
bEveryThingisGood = false;
}
std::string port2world = "/icubSim/world";
while (!Network::connect(port2icubsim, port2world.c_str()))
{
std::cout << "Trying to get input from "<< port2icubsim << "..." << std::endl;
yarp::os::Time::delay(1.0);
}
cout << moduleName << ": finding configuration files..." << endl;
rpc.open(("/" + moduleName + "/rpc").c_str());
attach(rpc);
Bottle cmd;
cmd.clear();
cmd.addString("world");
cmd.addString("del");
cmd.addString("all");
portSim.write(cmd);
yarp::os::Time::delay(1);
spamTable();
yarp::os::Time::delay(3);
yarp::os::Bottle pos1;
pos1.clear();
pos1.addDouble(-0.01);
pos1.addDouble(0.61);
pos1.addDouble(0.36);
objectGeneratorSim::createObject(pos1, true);
yarp::os::Time::delay(3);
pos1.clear();
pos1.addDouble(-0.07);
pos1.addDouble(0.61);
pos1.addDouble(0.29);
objectGeneratorSim::createObject(pos1);
yarp::os::Time::delay(3);
cout<<"Configuration done."<<endl;
return bEveryThingisGood;
}
bool LogPolarTransform::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("logpolarTransform"),
"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 name of the input and output ports, automatically prefixing the module name by using getName() */
inputPortName = "/";
inputPortName += getName(
rf.check("imageInputPort",
Value("/image:i"),
"Input image port (string)").asString()
);
outputPortName = "/";
outputPortName += getName(
rf.check("imageOutputPort",
Value("/image:o"),
"Output image port (string)").asString()
);
/* get the direction of the transform */
transformDirection = rf.check("direction",
Value("CARTESIAN2LOGPOLAR"),
"Key value (int)").asString();
cout << "Configuration of logpolar " << transformDirection << endl;
if (transformDirection == "CARTESIAN2LOGPOLAR") {
direction = CARTESIAN2LOGPOLAR;
}
else {
direction = LOGPOLAR2CARTESIAN;
}
/* get the number of angles */
numberOfAngles = rf.check("angles",
Value(252),
"Key value (int)").asInt();
/* get the number of rings */
numberOfRings = rf.check("rings",
Value(152),
"Key value (int)").asInt();
/* get the size of the X dimension */
xSize = rf.check("xsize",
Value(320),
"Key value (int)").asInt();
/* get the size of the Y dimension */
ySize = rf.check("ysize",
Value(240),
"Key value (int)").asInt();
/* get the overlap ratio */
overlap = rf.check("overlap",
Value(1.0),
"Key value (int)").asDouble();
/* do all initialization here */
/* open ports */
if (!imageIn.open(inputPortName.c_str())) {
cout << getName() << ": unable to open port " << inputPortName << endl;
return false; // unable to open; let RFModule know so that it won't run
}
if (!imageOut.open(outputPortName.c_str())) {
cout << getName() << ": unable to open port " << outputPortName << 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
//attachTerminal(); // attach to terminal
/* create the thread and pass pointers to the module parameters */
logPolarTransformThread = new LogPolarTransformThread(&imageIn, &imageOut,
&direction,
&xSize, &ySize,
&numberOfAngles, &numberOfRings,
&overlap);
/* now start the thread to do the work */
logPolarTransformThread->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 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 SalienceModule::configure(yarp::os::ResourceFinder &rf){
ConstString str = rf.check("name", Value("/salience"), "module name (string)").asString();
setName(str.c_str()); // modulePortName
attachTerminal();
// framerate
_intFPS = rf.check("fps", Value(20), "Try to achieve this number of frames per second (int).").asInt();
_intPrintFPSAfterNumFrames = rf.check("fpsOutputFrequency", Value(20), "Print the achieved framerate after this number of frames (int).").asInt();
_dblTPF = 1.0f/((float)_intFPS);
_intFPSAchieved = 0;
_intFC = 0;
_dblTPFAchieved = 0.0;
_dblStartTime = 0.0;
numBlurPasses = rf.check("numBlurPasses",
Value(0),
"Blur the output map numBlurPasses times with a gaussian 3x3 kernel (int).").asInt();
drawSaliencePeak = rf.check("drawSaliencePeak",
Value(1),
"Draw a crosshair at salience peak onto the output visualization image (int [0|1]).").asInt()!=0;
thresholdSalience = rf.check("thresholdSalience",
Value(0.0),
"Set salience map values < threshold to zero (double).").asDouble();
activateIOR = rf.check("activateInhibitionOfReturn",
Value(0),
"Use IOR (int [0|1]).").asInt()!=0;
if (activateIOR){
ior.open(rf);
}
filter = NULL;
ConstString filterName = rf.check("filter",
Value(""),
"filter to use (string [group|intensity|color|directional|motion|emd|ruddy|face])").asString();
if (filterName=="") {
printf("*** Please specify a filter, e.g. --filter motion\n");
vector<string> names = SalienceFactories::getPool().getNames();
printf("*** Filters available: ");
for (unsigned int i=0; i<names.size(); i++) {
printf("%s ", names[i].c_str());
}
printf("\n");
return false;
}
filter = SalienceFactories::getPool().get(filterName);
if (filter!=NULL) {
bool ok = filter->open(rf);
if (!ok) {
delete filter;
filter = NULL;
return false;
}
}
imgPort.open(getName("/view"));
peakPort.open(getName("/peak")); //For streaming saliency peak coordinates (Alex, 31/05/08)
filteredPort.open(getName("/map"));
configPort.open(getName("/conf"));
attach(configPort);
oldSizeX = -1;
oldSizeY = -1;
needInit = true;
fflush(stdout);
return true;
}
MainWindow::MainWindow(yarp::os::ResourceFinder &rf, QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow),
loadingWidget(this)
{
ui->setupUi(this);
setWindowTitle(APP_NAME);
utilities = nullptr;
pressed = false;
initThread = nullptr;
moduleName = QString("%1").arg(rf.check("name", Value("yarpdataplayer"), "module name (string)").asString().c_str());
if (rf.check("withExtraTimeCol")){
withExtraTimeCol = true;
column = rf.check("withExtraTimeCol",Value(1)).asInt32();
if (column < 1 || column > 2 ){
column = 1;
}
LOG( "Selected timestamp column to check is %d \n", column);
} else {
withExtraTimeCol = false;
column = 0;
}
add_prefix = rf.check("add_prefix");
createUtilities();
subDirCnt = 0;
utilities = nullptr;
setWindowTitle(moduleName);
setupActions();
setupSignals();
QString port = QString("/%1/rpc:i").arg(moduleName);
rpcPort.open( port.toLatin1().data() );
::signal(SIGINT, sighandler);
::signal(SIGTERM, sighandler);
attach(rpcPort);
quitFromCmd = false;
connect(ui->mainWidget,SIGNAL(itemDoubleClicked(QTreeWidgetItem*,int)),this,SLOT(onItemDoubleClicked(QTreeWidgetItem*,int)));
connect(this,SIGNAL(internalLoad(QString)),this,SLOT(onInternalLoad(QString)),Qt::QueuedConnection);
connect(this,SIGNAL(internalPlay()),this,SLOT(onInternalPlay()),Qt::BlockingQueuedConnection);
connect(this,SIGNAL(internalPause()),this,SLOT(onInternalPause()),Qt::BlockingQueuedConnection);
connect(this,SIGNAL(internalStop()),this,SLOT(onInternalStop()),Qt::BlockingQueuedConnection);
connect(this,SIGNAL(internalStep(Bottle*)),this,SLOT(onInternalStep(Bottle*)),Qt::BlockingQueuedConnection);
connect(this,SIGNAL(internalSetFrame(std::string,int)),this,SLOT(onInternalSetFrame(std::string,int)),Qt::BlockingQueuedConnection);
connect(this,SIGNAL(internalGetFrame(std::string, int*)),this,SLOT(onInternalGetFrame(std::string,int*)),Qt::BlockingQueuedConnection);
connect(this,SIGNAL(internalQuit()),this,SLOT(onInternalQuit()),Qt::QueuedConnection);
QShortcut *openShortcut = new QShortcut(QKeySequence("Ctrl+O"), parent);
QObject::connect(openShortcut, SIGNAL(activated()), this, SLOT(onInternalLoad(QString)));
QShortcut *closeShortcut = new QShortcut(QKeySequence("Ctrl+Q"), parent);
QObject::connect(closeShortcut, SIGNAL(activated()), this, SLOT(onInternalQuit()));
}
bool GBSegmModule::configure (yarp::os::ResourceFinder &rf)
{
if (rf.check("help","if present, display usage message")) {
printf("Call with --from configfile.ini\n");
return false;
}
if (rf.check("name"))
setName(rf.find("name").asString().c_str());
else setName("GBSeg");
//override defaults if specified - TODO: range checking
if(rf.check("sigma")) sigma = (float)rf.find("sigma").asDouble();
if(rf.check("k")) k = (float)rf.find("k").asDouble();
if(rf.check("minRegion")) min_size = rf.find("minRegion").asInt();
std::string slash="/";
_imgPort.open((slash + getName("/rawImg:i").c_str()).c_str());
_configPort.open((slash + getName("/conf").c_str()).c_str());
_viewPort.open((slash +getName("/viewImg:o").c_str()).c_str());
attach(_configPort);
//read an image to get the dimensions
ImageOf<PixelRgb> *yrpImgIn;
yrpImgIn = _imgPort.read();
if (yrpImgIn == NULL) // this is the case if module is requested to quit while waiting for image
return true;
input=new image<rgb>(yrpImgIn->width(), yrpImgIn->height(), true);
//
// //THIS IS NOT REQUIRED - INPUT IMAGES ARE ALWAYS RGB
// //IF DIM == 3 THEN THE PROCESSING CLASS CONVERTS TO LUV
// //IF DIM == 1 THEN THE PROCESSING CLASS ONLY USES THE FIRST CHANNEL
// //WE USE HUE CHANNEL, SO MUST CONVERT FROM RGB TO HSV
// //check compatibility of image depth
// /*if (yrpImgIn->getPixelSize() != dim_)
// {
// cout << endl << "Incompatible image depth" << endl;
// return false;
// }*/
//
// //override internal image dimension if necessary
// if( width_ > orig_width_ )
// width_ = orig_width_;
// if( height_ > orig_height_ )
// height_ = orig_height_;
//
// //allocate memory for image buffers and get the pointers
//
// inputImage.resize(width_, height_); inputImage_ = inputImage.getRawImage();
// inputHsv.resize(width_, height_); inputHsv_ = inputHsv.getRawImage();
// inputHue.resize(width_, height_); inputHue_ = inputHue.getRawImage();
// filtImage.resize(width_, height_); filtImage_ = filtImage.getRawImage();
// segmImage.resize(width_, height_); segmImage_ = segmImage.getRawImage();
// gradMap.resize(width_, height_); gradMap_ = (float*)gradMap.getRawImage();
// confMap.resize(width_, height_); confMap_ = (float*)confMap.getRawImage();
// weightMap.resize(width_, height_); weightMap_ = (float*)weightMap.getRawImage();
// labelImage.resize(width_, height_);
// labelView.resize(width_, height_);
return true;
}
bool FrontalEyeField::configure(yarp::os::ResourceFinder &rf) {
//Get conf parameters
moduleName = rf.check("name",Value("frontalEyeField")).asString();
setName(moduleName.c_str());
tau = rf.check("tau", Value(2.0)).asDouble();
retinaW = rf.check("retinaW", Value(3)).asInt();
retinaH = rf.check("retinaH", Value(3)).asInt();
string nameSourcePrefix = rf.check("nameSplitterPrefix", Value("/v1Retina/in_")).asString();
string nameSourceSuffix = rf.check("nameSplitterSuffix", Value("/error:o")).asString();
cameraUsed = (rf.check("camera", Value("left")).asString() == "right");
//string nameSourcePrefix = rf.check("nameSplitterPrefix", Value("/retina/")).asString();
//string nameSourceSuffix = rf.check("nameSplitterSuffix", Value("/retina/error:o")).asString();
//----------------------------//
//Configure the egocentric error cvz
stringstream configEgocentricError;
configEgocentricError
<< "type" << '\t' << cvz::core::TYPE_MMCM << endl
<< "name" << '\t' << "egocentricError" << endl
<< "width" << '\t' << 10 << endl
<< "height" << '\t' << 10 << endl
<< "layers" << '\t' << 5 << endl
<< "sigmaFactor" << '\t' << 0.75 << endl
<< "learningRate" << '\t' << 0.01 << endl << endl;
//Add the proprioception
configEgocentricError
<< "[modality_0]" << endl
<< "name" << '\t' << "fixationPoint" << endl
<< "type" << '\t' << "yarpVector" << endl
<< "size" << '\t' << 3 << endl
<< "minBounds" << '\t' << "(-2.0 -1.0 -1.0)" << endl
<< "maxBounds" << '\t' << "(-1.0 1.0 1.0)" << endl
<< "autoconnect" << '\t' << "/iKinGazeCtrl/x:o" << endl << endl;
//Add the v1Retina
configEgocentricError
<< "[modality_1]" << endl
<< "name" << '\t' << "v2Error" << endl
<< "type" << '\t' << "yarpVector" << endl
<< "size" << '\t' << 1 << endl
//<< "isBlocking" << endl
<< "autoconnect" << '\t' << "/v2/v1Retina/error:o"<<endl;
Property prop;
prop.fromConfig(configEgocentricError.str().c_str());
mmcmErrorPrediction = new cvz::core::ThreadedCvz(prop, 100);
mmcmErrorPrediction->start();
//cvz::core::CvzBuilder::allocate(&mmcmErrorPrediction, cvz::core::TYPE_MMCM);
//mmcmErrorPrediction->configure(prop);
//----------------------------//
//Open the gaze controller
gazePortName = "/";
gazePortName += getName() + "/gaze";
Property option;
option.put("device", "gazecontrollerclient");
option.put("remote", "/iKinGazeCtrl");
option.put("local", gazePortName.c_str());
igaze = NULL;
if (clientGazeCtrl.open(option)) {
clientGazeCtrl.view(igaze);
}
else
{
cout << "Invalid gaze polydriver" << endl;
return false;
}
igaze->storeContext(&store_context_id);
double neckTrajTime = rf.check("neckTrajTime",
Value(0.75)).asDouble();
igaze->setNeckTrajTime(neckTrajTime);
igaze->bindNeckPitch(-15.0, 10.0);
igaze->bindNeckRoll(-10.0, 10.0);
igaze->bindNeckYaw(-20.0, 20.0);
igaze->setSaccadesStatus(false);
igaze->blockEyes(0.0);
//Open the retina input ports
retinaInput.resize(retinaW);
errorMap.resize(retinaW);
for (int x = 0; x < retinaW; x++)
{
retinaInput[x].resize(retinaH);
errorMap[x].resize(retinaH);
for (int y = 0; y < retinaH; y++)
{
stringstream ss;
ss << "/" << moduleName << "/error/" << x << "_" << y << ":i";
retinaInput[x][y] = new BufferedPort<Bottle>();
retinaInput[x][y]->open(ss.str().c_str());
}
}
//Wait for the connection
connectErrorInput(nameSourcePrefix, nameSourceSuffix);
timeNextSaccade = Time::now() + tau;
return true;
}
bool PasarModule::configure(yarp::os::ResourceFinder &rf) {
std::string opcName;
std::string gazePortName;
std::string handlerPortName;
std::string saliencyPortName;
string moduleName = rf.check("name", Value("pasar")).asString().c_str();
setName(moduleName.c_str());
// moduleName = rf.check("name",
// Value("pasar")).asString();
// setName(moduleName.c_str());
//Parameters
pTopDownAppearanceBurst = rf.check("parameterTopDownAppearanceBurst",
Value(0.5)).asDouble();
pTopDownDisappearanceBurst = rf.check("parameterTopDownDisappearanceBurst",
Value(0.5)).asDouble();
pTopDownAccelerationCoef = rf.check("parameterTopDownAccelerationCoef",
Value(0.1)).asDouble();
//pLeakyIntegrationA = rf.check("parameterLeakyIntegrationA",
// Value(0.9)).asDouble();
pTopDownInhibitionReturn = rf.check("parameterInhibitionReturn",
Value(0.05)).asDouble();
pExponentialDecrease = rf.check("ExponentialDecrease",
Value(0.9)).asDouble();
pTopDownWaving = rf.check("pTopDownWaving",
Value(0.2)).asDouble();
dBurstOfPointing = rf.check("pBurstOfPointing",
Value(0.2)).asDouble();
//check for decrease
if (pExponentialDecrease >= 1 || pExponentialDecrease <= 0.0) pExponentialDecrease = 0.95;
presentRightHand.first = false;
presentRightHand.second = false;
presentLeftHand.first = false;
presentLeftHand.first = false;
rightHandt1 = Vector(3, 0.0);
rightHandt2 = Vector(3, 0.0);
leftHandt1 = Vector(3, 0.0);
leftHandt2 = Vector(3, 0.0);
thresholdMovementAccel = rf.check("thresholdMovementAccel",
Value(0.02)).asDouble();
thresholdWaving = rf.check("thresholdWaving",
Value(0.02)).asDouble();
thresholdSaliency = rf.check("thresholdSaliency",
Value(0.005)).asDouble();
isControllingMotors = rf.check("motorControl",
Value(0)).asInt() == 1;
//Ports
opcName=rf.check("opc",Value("OPC")).asString().c_str();
opc = new OPCClient(moduleName);
while (!opc->connect(opcName))
{
cout<<"Waiting connection to OPC..."<<endl;
Time::delay(1.0);
}
opc->checkout();
icub = opc->addOrRetrieveEntity<Agent>("icub");
if (!saliencyInput.open(("/" + moduleName + "/saliency:i").c_str())) {
cout << getName() << ": Unable to open port saliency:i" << endl;
return false;
}
if (!Network::connect("/agentDetector/skeleton:o", ("/" + moduleName + "/skeleton:i").c_str()))
{
isSkeletonIn = false;
}
else
{
yInfo() << " is connected to skeleton";
isSkeletonIn = true;
}
isPointing = rf.find("isPointing").asInt() == 1;
isWaving = rf.find("isWaving").asInt() == 1;
yInfo() << " pointing: " << isPointing;
yInfo() << " waving: " << isWaving;
if (!saliencyOutput.open(("/" + moduleName + "/saliency:o").c_str())) {
cout << getName() << ": Unable to open port saliency:o" << endl;
return false;
}
if (!handlerPort.open(("/" + moduleName + "/rpc").c_str())) {
cout << getName() << ": Unable to open port rpc" << endl;
return false;
}
attach(handlerPort); // attach to port
trackedObject = "";
presentObjectsLastStep.clear();
return true;
}
bool PasarModule::configure(yarp::os::ResourceFinder &rf) {
std::string opcName;
std::string gazePortName;
std::string handlerPortName;
std::string saliencyPortName;
moduleName = rf.check("name",
Value("pasar"),
"module name (string)").asString();
setName(moduleName.c_str());
//Parameters
pTopDownAppearanceBurst = rf.check("parameterTopDownAppearanceBurst",
Value(0.5)).asDouble();
pTopDownDisappearanceBurst = rf.check("parameterTopDownDisappearanceBurst",
Value(0.5)).asDouble();
pTopDownAccelerationCoef = rf.check("parameterTopDownAccelerationCoef",
Value(0.1)).asDouble();
//pLeakyIntegrationA = rf.check("parameterLeakyIntegrationA",
// Value(0.9)).asDouble();
pTopDownInhibitionReturn = rf.check("parameterInhibitionReturn",
Value(0.05)).asDouble();
pExponentialDecrease = rf.check("ExponentialDecrease",
Value(0.9)).asDouble();
//check for decrease
if (pExponentialDecrease >=1) pExponentialDecrease = 0.95;
thresholdMovementAccel = rf.check("thresholdMovementAccel",
Value(0.0)).asDouble();
thresholdSaliency = rf.check("thresholdSaliency",
Value(0.005)).asDouble();
isControllingMotors = rf.check("motorControl",
Value(0)).asInt() == 1;
//Ports
opcName = rf.check("opcName",
Value("OPC"),
"Opc name (string)").asString();
opc = new OPCClient(moduleName.c_str());
opc->connect(opcName);
if (!opc->isConnected())
if (!opc->connect("OPC"))
return false;
opc->checkout();
icub = opc->addAgent("icub");
saliencyPortName = "/";
saliencyPortName += getName() + "/saliency:i";
if (!saliencyInput.open(saliencyPortName.c_str())) {
cout << getName() << ": Unable to open port " << saliencyPortName << endl;
return false;
}
saliencyPortName = "/";
saliencyPortName += getName() + "/saliency:o";
if (!saliencyOutput.open(saliencyPortName.c_str())) {
cout << getName() << ": Unable to open port " << saliencyPortName << endl;
return false;
}
handlerPortName = "/";
handlerPortName += getName() + "/rpc";
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
return false;
}
gazePortName = "/";
gazePortName += getName() + "/gaze";
Property option;
option.put("device","gazecontrollerclient");
option.put("remote","/iKinGazeCtrl");
option.put("local", gazePortName.c_str());
if (isControllingMotors)
{
igaze=NULL;
if (clientGazeCtrl.open(option)) {
clientGazeCtrl.view(igaze);
}
else
{
cout<<"Invalid gaze polydriver"<<endl;
return false;
}
igaze->storeContext(&store_context_id);
double neckTrajTime = rf.check("neckTrajTime", Value(0.75)).asDouble();
igaze->setNeckTrajTime(neckTrajTime);
}
attach(handlerPort); // attach to port
trackedObject = "";
presentObjectsLastStep.clear();
return true ;
}
bool swTeleop::SWIcubTorso::init( yarp::os::ResourceFinder &oRf)
{
if(m_bInitialized)
{
std::cerr << "Icub Torso is already initialized. " << std::endl;
return true;
}
// gets the module name which will form the stem of all module port names
m_sModuleName = oRf.check("name", Value("teleoperation_iCub"), "Teleoperation/iCub Module name (string)").asString();
m_sRobotName = oRf.check("robot",Value("icubSim"), "Robot name (string)").asString();
m_i32RateVelocityControl = oRf.check("torsoRateVelocityControl", Value(m_i32RateVelocityControlDefault), "Torso rate velocity control (int)").asInt();
// robot parts to control
m_bTorsoActivated = oRf.check("torsoActivated", Value(m_bTorsoActivatedDefault), "Torso activated (int)").asInt() != 0;
if(!m_bTorsoActivated)
{
std::cout << "Torso not activated, icub torso initialization aborted. " << std::endl;
return (m_bInitialized=false);
}
// min / max values for iCub Torso joints
for(uint ii = 0; ii < m_vTorsoJointVelocityAcceleration.size(); ++ii)
{
std::ostringstream l_os;
l_os << ii;
std::string l_sMinJoint("torsoMinValueJoint" + l_os.str());
std::string l_sMaxJoint("torsoMaxValueJoint" + l_os.str());
std::string l_sTorsoJointVelocityAcceleration("torsoJointVelocityAcceleration" + l_os.str());
std::string l_sTorsoJointVelocityK("torsoJointVelocityK" + l_os.str());
std::string l_sTorsoJointPositionAcceleration("torsoJointPositionAcceleration" + l_os.str());
std::string l_sTorsoJointPositionSpeed("torsoJointPositionSpeed" + l_os.str());
std::string l_sTorsoResetPosition("torsoResetPosition" + l_os.str());
std::string l_sMinJointInfo("torso minimum joint" + l_os.str() + " Value (double)");
std::string l_sMaxJointInfo("torso maximum joint" + l_os.str() + " Value (double)");
std::string l_sTorsoJointVelocityAccelerationInfo("torso joint velocity acceleration " + l_os.str() + " Value (double)");
std::string l_sTorsoJointVelocityKInfo("torso joint velocity K coeff"+ l_os.str() + " Value (double)");
std::string l_sTorsoJointPositionAccelerationInfo("torso joint position acceleration " + l_os.str() + " Value (double)");
std::string l_sTorsoJointPositionSpeedInfo("torso joint position speed " + l_os.str() + " Value (double)");
std::string l_sTorsoResetPositionInfo("torso reset position " + l_os.str() + " Value (double)");
m_vTorsoMinJoint[ii] = oRf.check(l_sMinJoint.c_str(), m_vTorsoMinJointDefault[ii], l_sMinJointInfo.c_str()).asDouble();
m_vTorsoMaxJoint[ii] = oRf.check(l_sMaxJoint.c_str(), m_vTorsoMaxJointDefault[ii], l_sMaxJointInfo.c_str()).asDouble();
m_vTorsoResetPosition[ii] = oRf.check(l_sTorsoResetPosition.c_str(), m_vTorsoResetPositionDefault[ii], l_sTorsoResetPositionInfo.c_str()).asDouble();
m_vTorsoJointVelocityAcceleration[ii] = oRf.check(l_sTorsoJointVelocityAcceleration.c_str(), m_vTorsoJointVelocityAccelerationDefault[ii], l_sTorsoJointVelocityAccelerationInfo.c_str()).asDouble();
m_vTorsoJointPositionAcceleration[ii] = oRf.check(l_sTorsoJointPositionAcceleration.c_str(), m_vTorsoJointPositionAccelerationDefault[ii], l_sTorsoJointPositionAccelerationInfo.c_str()).asDouble();
m_vTorsoJointPositionSpeed[ii] = oRf.check(l_sTorsoJointPositionSpeed.c_str(), m_vTorsoJointPositionSpeedDefault[ii], l_sTorsoJointPositionSpeedInfo.c_str()).asDouble();
m_vTorsoJointVelocityK[ii] = oRf.check(l_sTorsoJointVelocityK.c_str(), m_vTorsoJointVelocityKDefault[ii], l_sTorsoJointVelocityKInfo.c_str()).asDouble();
}
// miscellaneous
m_i32TimeoutTorsoReset = oRf.check("torsoTimeoutReset", Value(m_i32TimeoutTorsoResetDefault), "torso timeout reset iCub (int)").asInt();
// set polydriver options
m_oTorsoOptions.put("robot", m_sRobotName.c_str());
m_oTorsoOptions.put("device", "remote_controlboard");
m_oTorsoOptions.put("local", ("/local/" + m_sRobotName + "/torso").c_str());
m_oTorsoOptions.put("name", ("/" + m_sRobotName + "/torso").c_str());
m_oTorsoOptions.put("remote", ("/" + m_sRobotName + "/torso").c_str());
// init polydriver
m_oRobotTorso.open(m_oTorsoOptions);
if(!m_oRobotTorso.isValid())
{
std::cerr << "-ERROR: robotTorso is not valid, escape torso initialization. " << std::endl;
return (m_bInitialized=false);
}
// initializing controllers
if (!m_oRobotTorso.view(m_pITorsoVelocity) || !m_oRobotTorso.view(m_pITorsoPosition) || !m_oRobotTorso.view(m_pITorsoEncoders) ||!m_oRobotTorso.view(m_pITorsoControlMode))
{
std::cerr << std::endl << "-ERROR: while getting required robot Torso interfaces." << std::endl << std::endl;
m_oRobotTorso.close();
return (m_bInitialized=false);
}
// init ports
m_sTorsoTrackerPortName = "/teleoperation/" + m_sRobotName + "/torso";
// open ports
bool l_bPortOpeningSuccess = true;
if(m_bTorsoActivated)
{
l_bPortOpeningSuccess = m_oTorsoTrackerPort.open(m_sTorsoTrackerPortName.c_str());
}
if(!l_bPortOpeningSuccess)
{
std::cerr << std::endl << "-ERROR: Unable to open ports." << std::endl << std::endl;
m_oRobotTorso.close();
return (m_bInitialized=false);
}
// retrieve Torso number of joints
m_pITorsoPosition->getAxes(&m_i32TorsoJointsNb);
// set accelerations/speeds
for(int ii = 0; ii < m_i32TorsoJointsNb; ++ii)
{
m_pITorsoPosition->setRefAcceleration(ii, m_vTorsoJointPositionAcceleration[ii]);
m_pITorsoPosition->setRefSpeed(ii, m_vTorsoJointPositionSpeed[ii]);
m_pITorsoVelocity->setRefAcceleration(ii, m_vTorsoJointVelocityAcceleration[ii]);
}
// init controller
m_pVelocityController = new swTeleop::SWTorsoVelocityController(m_pITorsoEncoders, m_pITorsoVelocity, m_pITorsoControlMode, m_vTorsoJointVelocityK, m_i32RateVelocityControl);
m_pVelocityController->enableTorso(m_bTorsoActivated);
// display parameters
std::cout << std::endl << std::endl;
displayDebug(std::string("Torso activated"), m_bTorsoActivated);
displayDebug(std::string("Timeout torso reset"), m_i32TimeoutTorsoReset);
displayDebug(std::string("Rate velocity control"), m_i32RateVelocityControl);
std::cout << std::endl;
displayVectorDebug(std::string("Torso min joint : "), m_vTorsoMinJoint);
displayVectorDebug(std::string("Torso max joint : "), m_vTorsoMaxJoint);
displayVectorDebug(std::string("Torso reset position joint : "), m_vTorsoResetPosition);
displayVectorDebug(std::string("Torso joint velocity acceleration: "), m_vTorsoJointVelocityAcceleration);
displayVectorDebug(std::string("Torso joint position acceleration: "), m_vTorsoJointPositionAcceleration);
displayVectorDebug(std::string("Torso joint position speed : "), m_vTorsoJointPositionSpeed);
displayVectorDebug(std::string("Torso head joint velocity K : "), m_vTorsoJointVelocityK);
std::cout << std::endl << std::endl;
return (m_bIsRunning=m_bInitialized=true);
}