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;
}
예제 #3
0
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;
}
예제 #4
0
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!");
}
예제 #5
0
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;
}
예제 #6
0
/* //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;
     }
 }
예제 #8
0
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;
}
예제 #9
0
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;
}
예제 #10
0
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;
}
예제 #13
0
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;
}
예제 #14
0
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;
}
예제 #15
0
void MainWindow::printLog(QString text)
{
    QString msg = text + "\n";
    //ui->logPanel->appendPlainText(msg);
    //qDebug() << msg;
    yDebug("%s", msg.toLatin1().data());
}
예제 #16
0
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;
}
예제 #19
0
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;   
        }
}
예제 #20
0
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;
    }
    
}
예제 #21
0
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;
}
예제 #22
0
    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);
    }
예제 #23
0
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;
}
예제 #24
0
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;
}
예제 #25
0
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;
}
예제 #26
0
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;
    }
}
예제 #27
0
    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;
    }
예제 #28
0
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();
}
예제 #29
0
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;
    }

}
예제 #30
0
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;
}