void utManagerThread::threadRelease()
{
yDebug("Deleting target from the iCubGui..\n");
deleteGuiTarget();
yDebug("Closing ports..\n");
closePort(motionCUTBlobs);
printMessage(1," motionCUTBlobs successfully closed!\n");
}
int utManagerThread::run_with_dispBlobber()
{
int kalState = -1;
switch (stateFlag)
{
case 0:
yDebug("Looking for motion...\n");
timeNow = yarp::os::Time::now();
oldMcutPoss.clear();
stateFlag++;
deleteGuiTarget();
break;
case 1:
// state #01: check the motionCUT and see if there's something interesting.
// if so, step up.
if (processMotion())
{
yDebug("Initializing tracker...\n");
timeNow = yarp::os::Time::now();
stateFlag++;
}
break;
case 2:
// state #02: read data from the dispBlobber and retrieve the 3D point of the center of the nearest blob
// with that, initialize the kalman filter, and then step up.
if (getPointFromDispBlobber())
{
yDebug("Initializing Kalman filter...\n");
kalThrd -> setKalmanState(KALMAN_INIT);
kalThrd -> kalmanInit(dispBlobberPos);
stateFlag++;
}
break;
case 3:
printMessage(2,"Reading from tracker and SFM...\n");
if (getPointFromDispBlobber())
{
kalThrd -> setKalmanInput(dispBlobberPos);
}
kalThrd -> getKalmanState(kalState);
sendGuiTarget();
if (kalState == KALMAN_STOPPED)
{
yInfo("For some reasons, the kalman filters stopped. Going back to initial state.\n");
stateFlag = 0;
}
break;
default:
yError("utManagerThread should never be here!!!\tState: %d\n",stateFlag);
Time::delay(1);
break;
}
return kalState;
}
bool AllostaticController::updateAllostatic()
{
DriveOutCZ activeDrive = chooseDrive();
yInfo() << "Drive " + activeDrive.name + " chosen";
if (activeDrive.name == "None") {
yInfo() << "No drive out of CZ." ;
return true;
}
else
{
yInfo() << "Drive " + activeDrive.name + " out of CZ." ;
}
if (allostaticDrives[activeDrive.name].active) {
yInfo() << "Trigerring " + activeDrive.name;
// record event in ABM
if (iCub->getABMClient()->Connect()) {
yDebug() << "ABM connected and receiving record.";
string drive_level;
if (to_string(activeDrive.level) == "0"){
drive_level = "under";
}
else{
drive_level="over";
}
string predicate = "goes_" + drive_level;
yDebug() << "Predicate set.";
std::list<std::pair<std::string, std::string> > lArgument;
lArgument.push_back(std::pair<std::string, std::string>(predicate, "predicate"));
lArgument.push_back(std::pair<std::string, std::string>(activeDrive.name, "agent"));
lArgument.push_back(std::pair<std::string, std::string>(drive_level, "object"));
iCub->getABMClient()->sendActivity("action",
activeDrive.name,
"drives", // expl: "pasar", "drives"...
lArgument,
true);
yInfo() << activeDrive.name + " has been recorded in the ABM";
}
else{
yDebug() << "ABM not connected; no recording of the trigger.";
}
allostaticDrives[activeDrive.name].triggerBehavior(activeDrive.level);
}
else {
yInfo() << "Drive " + activeDrive.name + " is not active";
}
return true;
}
void vtWThread::threadRelease()
{
printMessage(0,"Deleting target from the iCubGui..\n");
deleteGuiEvents();
yDebug("Closing gaze controller..");
Vector ang(3,0.0);
igaze -> lookAtAbsAngles(ang);
igaze -> restoreContext(contextGaze);
igaze -> stopControl();
ddG.close();
yDebug("Closing estimators..");
delete linEst_optFlow;
linEst_optFlow = NULL;
delete linEst_pf3dTracker;
linEst_pf3dTracker = NULL;
delete linEst_doubleTouch;
linEst_doubleTouch = NULL;
delete linEst_fgtTracker;
linEst_fgtTracker = NULL;
yDebug("Closing ports..");
optFlowPort.interrupt();
optFlowPort.close();
yTrace("optFlowPort successfully closed!");
pf3dTrackerPort.interrupt();
pf3dTrackerPort.close();
yTrace("pf3dTrackerPort successfully closed!");
doubleTouchPort.interrupt();
doubleTouchPort.close();
yTrace("doubleTouchPort successfully closed!");
genericObjectsPort.interrupt();
genericObjectsPort.close();
yTrace("genericObjectsPort successfully closed!");
sensManagerPort.interrupt();
sensManagerPort.close();
yTrace("sensManagerPort successfully closed!");
eventsPort.interrupt();
eventsPort.close();
yTrace("eventsPort successfully closed!");
depth2kinPort.interrupt();
depth2kinPort.close();
yTrace("depth2kinPort successfully closed!");
}
bool AllostaticController::openPorts(string driveName)
{
bool allGood = true;
outputm_ports.push_back(new BufferedPort<Bottle>);
outputM_ports.push_back(new BufferedPort<Bottle>);
string portName = "/" + moduleName + "/" + driveName;
// first, min ports
string pn = portName + "/min:i";
if (!outputm_ports.back()->open(pn))
{
yError() << getName() << ": Unable to open port " << pn;
allGood = false;
}
string targetPortName = "/" + homeo_name + "/" + driveName + "/min:o";
yarp::os::Time::delay(0.1);
while(!Network::connect(targetPortName,pn)) {
yInfo() <<"Setting up homeostatic connections... "<< targetPortName << " " << pn ;
yarp::os::Time::delay(0.5);
}
// now, max ports
pn = portName + "/max:i";
yInfo() << "Configuring port " << pn << " ...";
yarp::os::Time::delay(0.1);
if (!outputM_ports.back()->open(pn))
{
yError() << getName() << ": Unable to open port " << pn;
allGood = false;
}
yarp::os::Time::delay(0.1);
targetPortName = "/" + homeo_name + "/" + driveName + "/max:o";
while(!Network::connect(targetPortName, pn))
{
yDebug()<<"Setting up homeostatic connections... "<< targetPortName << " " << pn;
yarp::os::Time::delay(0.5);
}
yarp::os::Time::delay(0.1);
pn = "/" + moduleName + "/toBehaviorManager:o";
targetPortName = "/BehaviorManager/trigger:i";
to_behavior_rpc.open(pn);
while(!Network::connect(pn, targetPortName))
{
yDebug()<<"Setting up BehaviorManager connections... "<< pn << " " << targetPortName;
yarp::os::Time::delay(0.5);
}
return allGood;
}
/* //Feature to be added in a near future
bool FollowingOrder::handlePush(string type, string target)
{
// Point an object (from human order). Independent of proactivetagging
iCub->opc->checkout();
yInfo() << " [handlePush] : opc checkout";
list<Entity*> lEntities = iCub->opc->EntitiesCache();
string e_name = target;
// point RPC useless
//bool pointRPC = false;
for (auto& entity : lEntities)
{
string sName = entity->name();
yDebug() << "Checking entity: " << e_name << " to " << sName;//<<endl;
if (sName == e_name) {
if (entity->entity_type() == "object")//|| (*itEnt)->entity_type() == "agent" || (*itEnt)->entity_type() == "rtobject")
{
yInfo() << "I already knew that the object was in the opc: " << sName;
Object* o = dynamic_cast<Object*>(entity);
if(o && o->m_present) {
//pointRPC=true;
yInfo() << "I'd like to push " << e_name;// <<endl;
Object* obj1 = iCub->opc->addOrRetrieveEntity<Object>(e_name);
string sHand = "right";
if (obj1->m_ego_position[1]<0) sHand = "left";
Bottle bHand(sHand);
//iCub->say("I'm going to push the " + target);
iCub->push(e_name, bHand);
iCub->say("oh! look how I push the " + e_name);
yarp::os::Time::delay(2.0);
iCub->home();
target = "none";//pointList.pop_back();
return true;
}
}
}
}
return false;
}
*/
bool FollowingOrder::handleSearch(string type, string target)
{
// look if the object (from human order) exist and if not, trigger proactivetagging
iCub->opc->checkout();
yInfo() << " [handleSearch] : opc checkout";
list<Entity*> lEntities = iCub->opc->EntitiesCache();
bool tagRPC = false;
string e_name = target;
for (auto& entity: lEntities)
{
string sName = entity->name();
if (sName == e_name) {
yDebug() << "Entity found: "<<e_name;//<<endl;
if (entity->entity_type() == "object")//|| (*itEnt)->entity_type() == "agent" || (*itEnt)->entity_type() == "rtobject")
{
Object* o = dynamic_cast<Object*>(iCub->opc->getEntity(sName));
yInfo() << "I found the entity in the opc: " << sName;
if(o && o->m_present==1.0) {
//searchList.pop_back();
// return, so "if(tagRPC)" ... is never executed
return true;
}
} else {
tagRPC = true;
}
}
}
yInfo() << "I need to explore by name!";// << endl;
// ask for the object
yInfo() << "send rpc to proactiveTagging";//<<endl;
//If there is an unknown object (to see with agents and rtobjects), add it to the rpc_command bottle, and return true
Bottle cmd;
Bottle rply;
cmd.addString("searchingEntity");
cmd.addString(type);
cmd.addString(e_name);
rpc_out_port.write(cmd,rply);
yDebug() << rply.toString(); //<< endl;
//searchList.pop_back();
return true;
//if no unknown object was found, return false
//return false;
}
void stop()
{
switch (action)
{
case ActionCalibrate:
yDebug() << calibratorName << "killing calibration of device" << targetName;
calibrator->quitCalibrate();
break;
case ActionPark:
yDebug() << calibratorName << "killing park of device" << targetName;
calibrator->quitPark();
break;
}
}
bool AllostaticController::updateModule()
{
for(std::map<string, AllostaticDrive>::iterator it=allostaticDrives.begin(); it!=allostaticDrives.end(); ++it) {
if (bool(it->second.inputSensationPort->read()->get(0).asInt())) {
yDebug() << "Sensation ON";
it->second.update(SENSATION_ON);
} else {
yDebug() << "Sensation OFF";
it->second.update(SENSATION_OFF);
}
}
updateAllostatic();
return true;
}
bool AllostaticController::configure(yarp::os::ResourceFinder &rf)
{
moduleName = rf.check("name",Value("AllostaticController")).asString();
setName(moduleName.c_str());
yDebug()<<moduleName<<": finding configuration files...";//<<endl;
period = rf.check("period",Value(0.5)).asDouble();
bool isRFVerbose = true;
iCub = new ICubClient(moduleName, "allostaticController", "client.ini", isRFVerbose);
iCub->opc->isVerbose &= true;
if (!iCub->connect())
{
yInfo() << "iCubClient : Some dependencies are not running...";
Time::delay(1.0);
}
configureAllostatic(rf);
rpc_in_port.open("/" + moduleName + "/rpc");
attach(rpc_in_port);
yInfo()<<"Configuration done.";
return true;
}
eObool_t feat_manage_motioncontrol_data(eOipv4addr_t ipv4, eOprotID32_t id32, void* rxdata)
{
IethResource* mc = NULL;
if(NULL == _interface2ethManager)
{
return eobool_false;
}
mc = _interface2ethManager->getInterface(ipv4, id32);
if(NULL == mc)
{
char ipinfo[20];
char nvinfo[128];
eo_common_ipv4addr_to_string(ipv4, ipinfo, sizeof(ipinfo));
eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
yDebug("feat_manage_motioncontrol_data() fails to get a handle of embObjMotionControl for IP = %s and NV = %s", ipinfo, nvinfo);
return eobool_false;
}
if(false == mc->initialised())
{
return eobool_false;
}
else
{
mc->update(id32, yarp::os::Time::now(), rxdata);
}
return eobool_true;
}
fakestdbool_t feat_manage_motioncontrol_data(FEAT_boardnumber_t boardnum, eOprotID32_t id32, void* rxdata)
{
IethResource* mc = NULL;
ethFeatType_t type;
bool ret = _interface2ethManager->getHandle(boardnum, id32, &mc, &type);
if(!ret)
{
yDebug("EMS callback was unable to get access to the embObjMotionControl");
return fakestdbool_false;
}
if(type != ethFeatType_MotionControl)
{
return fakestdbool_false;
}
else if(false == mc->initialised())
{
return fakestdbool_false;
}
else
{
mc->update(id32, yarp::os::Time::now(), rxdata);
}
return fakestdbool_true;
}
fakestdbool_t feat_manage_skin_data(FEAT_boardnumber_t boardnum, eOprotID32_t id32, void *arrayofcandata)
{
static int error = 0;
IethResource* skin;
ethFeatType_t type;
bool ret = _interface2ethManager->getHandle(boardnum, id32, &skin, &type);
if(!ret)
{
yDebug("EMS callback was unable to get access to the embObjSkin");
return fakestdbool_false;
}
if(type != ethFeatType_Skin)
{ // the ethmanager does not know this object yet or the dynamic cast failed because it is not an embObjSkin
char nvinfo[128];
eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
if(0 == (error%1000) )
yWarning() << "feat_manage_skin_data() received a request from BOARD" << boardnum << "with ID:" << nvinfo << "but no class was jet instatiated for it";
error++;
return fakestdbool_false;
}
else if(false == skin->initialised())
{ // the ethmanager has the object, but the device was not fully opened yet. cannot use it
return fakestdbool_false;
}
else
{ // the object exists and is completed: it can be used
skin->update(id32, yarp::os::Time::now(), (void *)arrayofcandata);
}
return fakestdbool_true;
}
eObool_t feat_manage_skin_data(eOipv4addr_t ipv4, eOprotID32_t id32, void *arrayofcandata)
{
IethResource* skin;
if(NULL == _interface2ethManager)
{
return eobool_false;
}
skin = _interface2ethManager->getInterface(ipv4, id32);
if(NULL == skin)
{
char ipinfo[20];
char nvinfo[128];
eo_common_ipv4addr_to_string(ipv4, ipinfo, sizeof(ipinfo));
eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
yDebug("feat_manage_skin_data() fails to get a handle of embObjSkin for IP = %s and NV = %s", ipinfo, nvinfo);
return eobool_false;
}
if(false == skin->initialised())
{
return eobool_false;
}
else
{
skin->update(id32, yarp::os::Time::now(), (void *)arrayofcandata);
}
return eobool_true;
}
bool ServerSerial::open(Searchable& prop)
{
verb = (prop.check("verbose",Value(0),"Specifies if the device is in verbose mode (0/1).").asInt())>0;
if (verb)
yInfo("running with verbose output\n");
Value *name;
if (prop.check("subdevice",name,"name of specific control device to wrap")) {
yDebug("Subdevice %s\n", name->toString().c_str());
if (name->isString()) {
// maybe user isn't doing nested configuration
Property p;
p.setMonitor(prop.getMonitor(),
"subdevice"); // pass on any monitoring
p.fromString(prop.toString());
p.put("device",name->toString());
poly.open(p);
} else {
Bottle subdevice = prop.findGroup("subdevice").tail();
poly.open(subdevice);
}
if (!poly.isValid()) {
yError("cannot make <%s>\n", name->toString().c_str());
}
} else {
yError("\"--subdevice <name>\" not set for server_serial\n");
return false;
}
if (!poly.isValid()) {
return false;
}
ConstString rootName =
prop.check("name",Value("/serial"),
"prefix for port names").asString().c_str();
command_buffer.attach(toDevice);
reply_buffer.attach(fromDevice);
command_buffer.useCallback(callback_impl);
toDevice.open((rootName+"/in").c_str());
fromDevice.open((rootName+"/out").c_str());
if (poly.isValid())
poly.view(serial);
if(serial != NULL) {
start();
return true;
}
yError("subdevice <%s> doesn't look like a serial port (no appropriate interfaces were acquired)\n",
name->toString().c_str());
return false;
}
void MainWindow::printLog(QString text)
{
QString msg = text + "\n";
//ui->logPanel->appendPlainText(msg);
//qDebug() << msg;
yDebug("%s", msg.toLatin1().data());
}
eObool_t feat_manage_analogsensors_data(eOipv4addr_t ipv4, eOprotID32_t id32, void *data)
{
IethResource* sensor;
if(NULL == _interface2ethManager)
{
return eobool_false;
}
sensor = _interface2ethManager->getInterface(ipv4, id32);
if(NULL == sensor)
{
char ipinfo[20];
char nvinfo[128];
eo_common_ipv4addr_to_string(ipv4, ipinfo, sizeof(ipinfo));
eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
yDebug("feat_manage_analogsensors_data() fails to get a handle of embObjAnalogSensor for IP = %s and NV = %s", ipinfo, nvinfo);
return eobool_false;
}
if(false == sensor->initialised())
{
return eobool_false;
}
else
{ // data is a EOarray* in case of mais or strain but it is a eOas_inertial_status_t* in case of inertial sensor
sensor->update(id32, yarp::os::Time::now(), data);
}
return eobool_true;
}
void run()
{
switch (action)
{
case ActionCalibrate:
yDebug() << calibratorName << "starting calibration of device" << targetName;
calibrator->calibrate(target);
yDebug() << calibratorName << "finished calibration of device" << targetName;
break;
case ActionPark:
yDebug() << calibratorName << "starting park device" << targetName;
calibrator->park(target);
yDebug() << calibratorName << "finished park device" << targetName;
break;
}
}
fakestdbool_t feat_manage_analogsensors_data(FEAT_boardnumber_t boardnum, eOprotID32_t id32, void *as_array)
{
IethResource* sensor;
ethFeatType_t type;
bool ret = _interface2ethManager->getHandle(boardnum, id32, &sensor, &type);
if(!ret)
{
yDebug("EMS callback was unable to get access to the embObjAnalogSensor");
return fakestdbool_false;
}
if(! ((type == ethFeatType_AnalogMais) || (type == ethFeatType_AnalogStrain)) )
{
yError("EMS analog sensor callback - the ethmanager does not know this object YET or a wrong pointer has been returned because it is not an embObjAnalogSensor");
return fakestdbool_false;
}
else if(false == sensor->initialised())
{ // the ethmanager has the object, but the obiect was not full initted yet. cannot use it
return fakestdbool_false;
}
else
{ // the object exists and is completed: it can be used
sensor->update(id32, yarp::os::Time::now(), as_array);
}
return fakestdbool_true;
}
bool cartControlReachAvoidThread::checkTargetFromPortInput(Vector &target_pos, double &target_radius)
{
Bottle *targetBottle=inportTargetCoordinates.read(false);
if(targetBottle != NULL)
{
target_pos(0)=targetBottle->get(0).asDouble();
target_pos(1)=targetBottle->get(1).asDouble();
target_pos(2)=targetBottle->get(2).asDouble();
target_radius = targetBottle->get(3).asDouble();
yDebug("checkTargetFromPortInput: getting target %f %f %f and radius %f from the port:",targetBottle->get(0).asDouble(),targetBottle->get(1).asDouble(),targetBottle->get(2).asDouble(),targetBottle->get(3).asDouble());
yDebug("checkTargetFromPortInput: getting target %s and radius %f from the port:",target_pos.toString().c_str(),target_radius);
return true;
}
else{
return false;
}
}
bool cartControlReachAvoidThread::targetCloseEnough(){
Vector x,o;
double distanceFromTarget = 0.0;
//get current end-eff position
//cartArm is already pointing to the right arm
cartArm->getPose(x,o);
distanceFromTarget = sqrt( pow(targetPos[0]-x[0],2) + pow(targetPos[1]-x[1],2) + pow(targetPos[2]-x[2],2));
yDebug("targetCloseEnough(): distance: %f, requested target: %s, current end-eff pos: %s\n",distanceFromTarget,targetPos.toString().c_str(),x.toString().c_str());
if(distanceFromTarget<reachTol){
yDebug("targetCloseEnough(): returning true: distanceFromTarget: %f < reachTol: %f\n",distanceFromTarget,reachTol);
return true;
}
else{
return false;
}
}
Bottle SpeechRecognizerModule::waitNextRecognition(int timeout)
{
yInfo() <<"Recognition: blocking mode on" ;
Bottle bOutGrammar;
bool gotSomething = false;
double endTime = Time::now() + timeout/1000.0;
interruptRecognition = false;
cout << endl ;
yInfo() << "=========== GO Waiting for recog! ===========" ;
while(Time::now()<endTime && !gotSomething && !interruptRecognition)
{
//std::cout<<".";
const float ConfidenceThreshold = 0.3f;
SPEVENT curEvent;
ULONG fetched = 0;
HRESULT hr = S_OK;
m_cpRecoCtxt->GetEvents(1, &curEvent, &fetched);
while (fetched > 0)
{
yInfo() << " received something in waitNextRecognition" ;
gotSomething = true;
ISpRecoResult* result = reinterpret_cast<ISpRecoResult*>(curEvent.lParam);
CSpDynamicString dstrText;
result->GetText(SP_GETWHOLEPHRASE, SP_GETWHOLEPHRASE, TRUE, &dstrText, NULL);
string fullSentence = ws2s(dstrText);
yInfo() <<fullSentence ;
if (m_useTalkBack)
say(fullSentence);
bOutGrammar.addString(fullSentence);
SPPHRASE* pPhrase = NULL;
result->GetPhrase(&pPhrase);
bOutGrammar.addList() = toBottle(pPhrase,&pPhrase->Rule);
yInfo() <<"Sending semantic bottle : "<<bOutGrammar.toString() ;
m_cpRecoCtxt->GetEvents(1, &curEvent, &fetched);
if (m_forwardSound)
{
yarp::sig::Sound& rawSnd = m_portSound.prepare();
rawSnd = toSound(result);
m_portSound.write();
}
}
}
if(interruptRecognition) {
yDebug() << "interrupted speech recognizer!";
}
yInfo() <<"Recognition: blocking mode off";
return bOutGrammar;
}
void checkLog() {
int i = 13;
yTrace("This is a trace");
yTrace("This is %s (%d)", "a trace", i);
yDebug("This is a debug");
yDebug("This is %s (%d)", "a debug", i);
yInfo("This is info");
yInfo("This is %s (%d)", "info", i);
yWarning("This is a warning");
yWarning("This is %s (%d)", "a warning", i);
yError("This is an error");
yError("This is %s (%d)", "an error", i);
}
RobotInterface::Robot& RobotInterface::XMLReader::Private::readRobotFile(const std::string &fileName)
{
filename = fileName;
#ifdef WIN32
std::replace(filename.begin(), filename.end(), '/', '\\');
#endif
curr_filename = fileName;
#ifdef WIN32
path = filename.substr(0, filename.rfind("\\"));
#else // WIN32
path = filename.substr(0, filename.rfind("/"));
#endif //WIN32
yDebug() << "Reading file" << filename.c_str();
TiXmlDocument *doc = new TiXmlDocument(filename.c_str());
if (!doc->LoadFile()) {
SYNTAX_ERROR(doc->ErrorRow()) << doc->ErrorDesc();
}
if (!doc->RootElement()) {
SYNTAX_ERROR(doc->Row()) << "No root element.";
}
for (TiXmlNode* childNode = doc->FirstChild(); childNode != 0; childNode = childNode->NextSibling()) {
if (childNode->Type() == TiXmlNode::TINYXML_UNKNOWN) {
if(dtd.parse(childNode->ToUnknown(), curr_filename)) {
break;
}
}
}
if (!dtd.valid()) {
SYNTAX_WARNING(doc->Row()) << "No DTD found. Assuming version robotInterfaceV1.0";
dtd.setDefault();
dtd.type = RobotInterfaceDTD::DocTypeRobot;
}
if(dtd.type != RobotInterfaceDTD::DocTypeRobot) {
SYNTAX_WARNING(doc->Row()) << "Expected document of type" << DocTypeToString(RobotInterfaceDTD::DocTypeRobot)
<< ". Found" << DocTypeToString(dtd.type);
}
if(dtd.majorVersion != 1 || dtd.minorVersion != 0) {
SYNTAX_WARNING(doc->Row()) << "Only robotInterface DTD version 1.0 is supported";
}
readRobotTag(doc->RootElement());
delete doc;
// yDebug() << robot;
return robot;
}
bool ServerSerial::send(char *msg, size_t size)
{
if(verb)
yDebug("ConstString to send : %s\n", msg);
if(serial != NULL) {
serial->send(msg, size);
return true;
}
else
return false;
}
bool ServerSerial::send(const Bottle& msg)
{
if(verb)
yDebug("ConstString to send : %s\n", msg.toString().c_str());
if(serial != NULL) {
serial->send(msg);
return true;
}
else
return false;
}
bool cartControlReachAvoidThread::getAvoidanceGainFromPort()
{
Bottle* avoidanceGainBottle = inportAvoidanceGain.read(false);
if(avoidanceGainBottle != NULL){
avoidanceGain = avoidanceGainBottle->get(0).asDouble();
yDebug("getAvoidanceGainFromPort(): Reading %f from port and setting to global avoidanceGain.\n",avoidanceGain);
return true;
}
else{
return false;
}
}
void checkLogStream() {
int i = 13;
std::vector<int> v(4);
v[0] = 1;
v[1] = 2;
v[2] = 3;
v[3] = 4;
yTrace("This is a trace");
yTrace("This is %s (%d)", "a trace", i);
yTrace();
yTrace() << "This is" << "another" << "trace" << i;
yTrace() << v;
yDebug("This is a debug");
yDebug("This is %s (%d)", "a debug", i);
yDebug();
yDebug() << "This is" << "another" << "debug" << i;
yDebug() << v;
yInfo("This is info");
yInfo("This is %s (%d)", "info", i);
yInfo();
yInfo() << "This is" << "more" << "info" << i;
yInfo() << v;
yWarning("This is a warning");
yWarning("This is %s (%d)", "a warning", i);
yWarning();
yWarning() << "This is" << "another" << "warning" << i;
yWarning() << v;
yError("This is an error");
yError("This is %s (%d)", "an error", i);
yError();
yError() << "This is" << "another" << "error" << i;
yError() << v;
}
TextureLogo::TextureLogo(ovrSession session) :
session(session),
textureSwapChain(nullptr),
width(yarp_logo.width),
height(yarp_logo.height),
bytes_per_pixel(yarp_logo.bytes_per_pixel),
// see http://stackoverflow.com/questions/27294882/glteximage2d-fails-with-error-1282-using-pbo-bad-texture-resolution
padding((4 - (width * bytes_per_pixel) % 4) % 4),
rowSize(width * bytes_per_pixel + padding),
bufferSize(rowSize * height),
ptr((GLubyte*)yarp_logo.pixel_data)
{
yDebug() << width;
createTexture();
}
void FollowingOrder::run(Bottle args/*=Bottle()*/) {
yInfo() << "FollowingOrder::run";
Bottle* sens = sensation_port_in.read();
string action = sens->get(0).asString();
string type;
string target;
if (sens->size()>0)
type = sens->get(1).asString();
if (sens->size()>1)
target = sens->get(2).asString();
yDebug() << action;
yDebug() << type;
yInfo() << target;
if (target != "none"){
yInfo() << "there are elements to search!!!";//<<endl;
finding = handleSearch(type, target);
}
if (action == "point"){
// Be carfull: both handlePoint (point in response of a human order) and handlePointing (point what you know)
if (sens->size()<2){
yInfo()<<"I can't point if you don't tell me the objects";
iCub->say("I can't point if you don't tell me the objects");
}else{
pointing = handlePoint(type, target);
yInfo() << "pointing elements to point!!!";//<<endl;
yDebug() << finding;// << endl;
}
}else if (action == "look at"){
if (sens->size()<2){
yInfo()<<"I can't look if you don't tell me the objects";
iCub->say("I can't look if you don't tell me the objects");
}else{
handleLook(type, target);
yInfo() << "looking elements to look at!!!";//<<endl;
yDebug() << finding;// << endl;
}
}else if (action == "push"){
if (sens->size()<2){
yInfo()<<"I can't push if you don't tell me the objects";
iCub->say("I can't push if you don't tell me the objects");
}else{
//handlePush(type, target); //Feature under development
yInfo() << "pushing elements to look at!!!";//<<endl;
yDebug() << finding;// << endl;
}
}else if (action == "narrate"){
handleNarrate();
yInfo() << "narrating!!!";//<<endl;
yDebug() << finding;// << endl;
}
}
RobotInterface::ActionList RobotInterface::XMLReader::Private::readActionsFile(const std::string &fileName)
{
std::string old_filename = curr_filename;
curr_filename = fileName;
yDebug() << "Reading file" << fileName.c_str();
TiXmlDocument *doc = new TiXmlDocument(fileName.c_str());
if (!doc->LoadFile()) {
SYNTAX_ERROR(doc->ErrorRow()) << doc->ErrorDesc();
}
if (!doc->RootElement()) {
SYNTAX_ERROR(doc->Row()) << "No root element.";
}
RobotInterfaceDTD actionsFileDTD;
for (TiXmlNode* childNode = doc->FirstChild(); childNode != 0; childNode = childNode->NextSibling()) {
if (childNode->Type() == TiXmlNode::TINYXML_UNKNOWN) {
if(actionsFileDTD.parse(childNode->ToUnknown(), curr_filename)) {
break;
}
}
}
if (!actionsFileDTD.valid()) {
SYNTAX_WARNING(doc->Row()) << "No DTD found. Assuming version robotInterfaceV1.0";
actionsFileDTD.setDefault();
actionsFileDTD.type = RobotInterfaceDTD::DocTypeActions;
}
if (actionsFileDTD.type != RobotInterfaceDTD::DocTypeActions) {
SYNTAX_ERROR(doc->Row()) << "Expected document of type" << DocTypeToString(RobotInterfaceDTD::DocTypeActions)
<< ". Found" << DocTypeToString(actionsFileDTD.type);
}
if (actionsFileDTD.majorVersion != dtd.majorVersion) {
SYNTAX_ERROR(doc->Row()) << "Trying to import a file with a different robotInterface DTD version";
}
RobotInterface::ActionList actions = readActionsTag(doc->RootElement());
delete doc;
curr_filename = old_filename;
return actions;
}
