示例#1
0
/*
* create the vAugmentedTime for the current used Instance : sql to obtain the foreign key for the feedback table
*
*/
bool abmInteraction::insertFeedback(int feedback, int feedbackInstance)
{
    Bottle bResult;
    ostringstream osRequest;
    //take info from visualdata to put in feedback. In particular  the time of original memories is the smallest one from augmented
    osRequest << "SELECT DISTINCT instance, augmented_time, img_provider_port, min(\"time\") FROM visualdata WHERE instance = " << rememberedInstance << " AND augmented IS NOT NULL AND augmented_time = '" << *it_augmentedTime << "' AND img_provider_port = '" << img_provider_port << "' GROUP BY instance, augmented_time, img_provider_port;";
    bResult = iCub->getABMClient()->requestFromString(osRequest.str());
    yInfo() << bResult.toString();

    if (bResult.toString() != "NULL") {
        //we should only have one line, because instance,augmented_time,img_provider_port is primary key
        Bottle bResInsert;
        ostringstream osInsertFeedback;



        osInsertFeedback << "INSERT INTO feedback VALUES (" << feedbackInstance << ", '" << bResult.get(0).asList()->get(1).toString() << "', '" << bResult.get(0).asList()->get(2).toString() << "', '" << bResult.get(0).asList()->get(3).toString() << "', '" << agentName << "', " << feedback << ", 'none');";
        bResInsert = iCub->getABMClient()->requestFromString(osInsertFeedback.str().c_str());

        yInfo() << " Request sent : " << osInsertFeedback.str();
    }
    else {
        yError() << "Request to obtain augmented from instance " << rememberedInstance << " with augmented_time = " << *it_augmentedTime << " is NULL";
        return false;
    }

    return true;
}
示例#2
0
void RosNameSpace::run() {
    int pct = 0;
    do {
        mutex.wait();
        pct = pending.size();
        mutex.post();
        if (pct>0) {
            mutex.wait();
            Bottle *bot = pending.get(0).asList();
            Bottle curr = *bot;
            mutex.post();

            dbg_printf("ROS connection begins: %s\n", curr.toString().c_str());
            ContactStyle style;
            style.admin = true;
            style.carrier = "tcp";
            Bottle cmd = curr.tail();
            Contact contact = Contact::fromString(curr.get(0).asString());
            contact.setName("");
            Bottle reply;
            NetworkBase::write(contact, cmd, reply, style);
            dbg_printf("ROS connection ends: %s\n", curr.toString().c_str());

            mutex.wait();
            pending = pending.tail();
            pct = pending.size();
            mutex.post();
        }
    } while (pct>0);
}
示例#3
0
bool TouchingOrder::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";

    Object *o = dynamic_cast<Object*>(iCub->opc->getEntity(target));
    if(o!=NULL) {
        yInfo() << "I found the entity in the opc: " << target << " and thus I'll leave handleSearch";
        return true;
    }

    yInfo() << "I need to call 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(target);
    yDebug() << "Send to proactiveTagging: " << cmd.toString();
    rpc_out_port.write(cmd,rply);
    yDebug() << rply.toString(); //<< endl;

    return true;
}
示例#4
0
string SpeechRecognizerModule::getFromDictaction(int timeout, LPCWSTR options )
{
    bool everythingIsFine = TRUE;
    everythingIsFine &= SUCCEEDED(m_cpGrammarDictation->UnloadDictation());
    everythingIsFine &= SUCCEEDED(m_cpGrammarDictation->LoadDictation(options, SPLO_STATIC));
    everythingIsFine &= SUCCEEDED(m_cpGrammarDictation->SetDictationState( SPRS_ACTIVE ));
    yInfo() <<"Dictation is on..." ;
    Bottle botTmp;
    if (!USE_LEGACY)
    {
        botTmp = waitNextRecognition(m_timeout);
    }
    else
    {   
        list< pair<string, double> > results = waitNextRecognitionLEGACY(m_timeout);
        for(list< pair<string, double> >::iterator it = results.begin(); it != results.end(); it++)
        {
            botTmp.addString(it->first.c_str());
            //botTmp.addDouble(it->second);
        }
    }
    yInfo() <<"Dictation is off...";
    yInfo() <<"Got : "<<botTmp.toString();
    //Turn off dictation and go back to the file grammar
    everythingIsFine &= SUCCEEDED(m_cpGrammarDictation->SetDictationState( SPRS_INACTIVE ));
    everythingIsFine &=SUCCEEDED(m_cpGrammarFromFile->SetGrammarState(SPGS_ENABLED));
    return botTmp.toString();
}
示例#5
0
    void checkAccept() {
        report(0, "checking direct object accept...");

        PortReaderBuffer<Bottle> buffer;
        Bottle dummy;
        Bottle data("hello");
        Bottle data2("there");

        buffer.acceptObject(&data, &dummy);
        
        Bottle *bot = buffer.read();
        checkTrue(bot!=NULL,"Inserted message received");
        if (bot!=NULL) {
            checkEqual(bot->toString().c_str(),"hello","value ok");
        }

        buffer.acceptObject(&data2, NULL);

        bot = buffer.read();
        checkTrue(bot!=NULL,"Inserted message received");
        if (bot!=NULL) {
            checkEqual(bot->toString().c_str(),"there","value ok");
        }
        
        buffer.read(false);
    }
示例#6
0
int main(int argc, char *argv[]) {
    if (argc<3) {
        fprintf(stderr, "Please supply (1) a port name for the client\n");
        fprintf(stderr, "              (2) a port name for the server\n");
        return 1;
    }

    Network yarp;
    const char *client_name = argv[1];
    const char *server_name = argv[2];

    RpcClient port;
    port.open(client_name);

    int ct = 0;
    while (true) {
	if (port.getOutputCount()==0) {
	  printf("Trying to connect to %s\n", server_name);
	  yarp.connect(client_name,server_name);
	} else {
	  Bottle cmd;
	  cmd.addString("COUNT");
	  cmd.addInt32(ct);
	  ct++;
	  printf("Sending message... %s\n", cmd.toString().c_str());
	  Bottle response;
	  port.write(cmd,response);
	  printf("Got response: %s\n", response.toString().c_str());
	}
	Time::delay(1);
    }
}
//*************************************************************************************************************************
MotorFrictionExcitationThread::MotorFrictionExcitationThread(string _name, string _robotName, int _period, ParamHelperServer *_ph,
    wholeBodyInterface *_wbi, ResourceFinder &rf, ParamHelperClient *_identificationModule)
    :  RateThread(_period), name(_name), robotName(_robotName), paramHelper(_ph), robot(_wbi), identificationModule(_identificationModule)
{
    status = EXCITATION_OFF;
    sendCmdToMotors = SEND_COMMANDS_TO_MOTORS;
    printCountdown = 0;
    freeExcCounter = 0;
    contactExcCounter = 0;
    fricStdDevThrMonitor = 0.0;
    ktStdDevThrMonitor = 0.0;
    _n = robot->getDoFs();
    isFrictionStdDevBelowThreshold = false;

    Bottle reply;
    if(!contactExc.readFromConfigFile(rf, reply))
        printf("Error while reading contact excitation from config file: \n%s\n", reply.toString().c_str());
    printf("Results of contact excitation reading: %s\n", reply.toString().c_str());
    printf("Contact excitation value read:\n%s\n", contactExc.toString().c_str());
    reply.clear();
    if(!freeMotionExc.readFromConfigFile(rf, reply))
        printf("Error while reading free motion excitation from config file: \n%s\n", reply.toString().c_str());
    printf("Results of free motion excitation reading: %s\n", reply.toString().c_str());
    printf("Free motion excitation value read:\n%s\n", freeMotionExc.toString().c_str());
}
示例#8
0
    void testSequence(char *seq,
                      size_t len,
                      const char *fmt,
                      Bottle ref,
                      bool testWrite = true)
    {
        char err[1024];
        printf("\n");
        printf("================================================\n");
        printf(" READ %s\n", fmt);
        Bytes b1(seq, len);
        WireTwiddler tt;
        tt.configure(fmt, fmt);
        printf(">>> %s\n", tt.toString().c_str());
        Bottle bot;

        checkTrue(tt.read(bot, b1), "Read failed");
        snprintf(err, 1024, "%s: read %s, expected %s", fmt, bot.toString().c_str(), ref.toString().c_str());
        checkTrue(bot == ref, err);
        printf("[1] %s: read %s as expected\n", fmt, bot.toString().c_str());

        StringInputStream sis;
        sis.add(b1);
        sis.add(b1);
        WireTwiddlerReader twiddled_input(sis, tt);
        Route route;
        bot.clear();
        twiddled_input.reset();
        ConnectionReader::readFromStream(bot, twiddled_input);

        snprintf(err, 1024, "%s: read %s, expected %s", fmt, bot.toString().c_str(), ref.toString().c_str());
        checkTrue(bot == ref, err);
        printf("[2] %s: read %s as expected\n", fmt, bot.toString().c_str());

        bot.clear();
        twiddled_input.reset();
        ConnectionReader::readFromStream(bot, twiddled_input);

        snprintf(err, 1024, "%s: read %s, expected %s", fmt, bot.toString().c_str(), ref.toString().c_str());
        checkTrue(bot == ref, err);
        printf("[3] %s: read %s as expected\n", fmt, bot.toString().c_str());

        if (testWrite) {

            printf("\n");
            printf("================================================\n");
            printf(" WRITE %s\n", fmt);
            ManagedBytes output;
            checkTrue(tt.write(ref, output), "WRITE FAILED");
            snprintf(err, 1024, "WRITE MISMATCH, length %zd, expected %zd", output.length(), len);
            checkTrue(output.length() == len, err);

            for (size_t i = 0; i < output.length(); i++) {
                snprintf(err, 1024, "WRITE MISMATCH, at %zd, have [%d:%c] expected [%d:%c]\n", i, output.get()[i], output.get()[i], seq[i], seq[i]);
                checkTrue(output.get()[i] == seq[i], err);
            }
            printf("[4] %s: wrote %s as expected\n", fmt, bot.toString().c_str());
        }
    }
bool MotorFrictionIdentificationModule::respond(const Bottle& cmd, Bottle& reply)
{
    paramHelper->lock();
    if(!paramHelper->processRpcCommand(cmd, reply))
        reply.addString( (string("Command ")+cmd.toString().c_str()+" not recognized.").c_str());
    paramHelper->unlock();

    // if reply is empty put something into it, otherwise the rpc communication gets stuck
    if(reply.size()==0)
        reply.addString( (string("Command ")+cmd.toString().c_str()+" received.").c_str());
	return true;
}
示例#10
0
bool MainWindow::step()
{
    Bottle reply;
    emit internalStep(&reply);
    if (reply.toString() == "error"){
        return false;
    }
    if (reply.toString() == "ok"){
        return true;
    }
    return false;
}
示例#11
0
/*
* create the vAugmentedTime for the current used Instance : sql to obtain the foreign key for the feedback table
*
*/
bool abmInteraction::createAugmentedTimeVector(pair<string,int> & bestTimeAndRank)
{
    Bottle bResult;
    ostringstream osRequest;

    //only augmented_time is needed but better clarity for the print
    osRequest << "SELECT DISTINCT instance, augmented_time, img_provider_port FROM visualdata WHERE instance = " << rememberedInstance << " AND augmented IS NOT NULL AND img_provider_port = '" << img_provider_port << "' " ;
    if(resume == "agent"){
        // not proposing instance with feedback from the agent already. 
        osRequest << "AND augmented_time NOT IN (SELECT DISTINCT augmented_time FROM feedback WHERE instance = '" << rememberedInstance << "' AND agent = '" << agentName << "') " ;
        createBestAugmentedTime(bestTimeAndRank) ;
    } else if (resume == "yes") {
        osRequest << "AND augmented_time NOT IN (SELECT DISTINCT augmented_time FROM feedback WHERE instance = '" << rememberedInstance << "') " ;
        createBestAugmentedTime(bestTimeAndRank);
    } else if (resume == "no") {
        //come back to default value for best rank/time
        bestTimeAndRank.first = "";
        bestTimeAndRank.second = 0;
    }

    osRequest << " ORDER BY augmented_time ASC ;";

    osRequest << " ;";
    bResult = iCub->getABMClient()->requestFromString(osRequest.str());
    yInfo() << "[createAugmentedTimeVector] SQL request bReply : " << bResult.toString();
    vAugmentedTime.clear();

    if (bResult.toString() != "NULL") {
        for (int i = 0; i < bResult.size(); i++){
            //get(1) because augmented is in second column of the result
            vAugmentedTime.push_back(bResult.get(i).asList()->get(1).toString());
        }
    }
    else {
        yError() << "Request to obtain augmented from instance " << rememberedInstance << " is NULL : are you sure there are some augmented images?";
        bestTimeAndRank.first = "";
        bestTimeAndRank.second = -1;
        return false;
    }

    ostringstream osAugmentedTime;
    const char* const delim = ", ";

    copy(vAugmentedTime.begin(), vAugmentedTime.end(), std::ostream_iterator<std::string>(osAugmentedTime, delim));
    yInfo() << " vAugmentedTime = " << osAugmentedTime.str();

    it_augmentedTime = vAugmentedTime.begin();

    return true;
}
示例#12
0
 virtual void run() {
     printf("Listening to terminal (type \"quit\" to stop module)\n");
     bool isEof = false;
     while (!(isEof||isStopping()||owner.isStopping())) {
         ConstString str = NetworkBase::readString(&isEof);
         if (!isEof) {
             Bottle cmd(str.c_str());
             Bottle reply;
             bool ok = owner.safeRespond(cmd,reply);
             if (ok) {
                 //printf("ALL: %s\n", reply.toString().c_str());
                 //printf("ITEM 1: %s\n", reply.get(0).toString().c_str());
                 if (reply.get(0).toString()=="help") {
                     for (int i=0; i<reply.size(); i++) {
                         ACE_OS::printf("%s\n",
                                        reply.get(i).toString().c_str());
                     }
                 } else {
                     ACE_OS::printf("%s\n", reply.toString().c_str());
                 }
             } else {
                 ACE_OS::printf("Command not understood -- %s\n", str.c_str());
             }
         }
     }
     //printf("terminal shutting down\n");
     //owner.interruptModule();
 }
示例#13
0
    /*
    * Message handler. Just echo all received messages.
    */
    bool respond(const Bottle& command, Bottle& reply) 
    {
        cout<<"Got "<<command.toString().c_str()<<endl;

        if(command.size() < 3) {
            reply.addString("Command error! example: 'sum 3 4'");
            return true;
        }
        int a = command.get(1).asInt();
        int b = command.get(2).asInt();

        if( command.get(0).asString() == "sum") {
           int c = sum(a, b);
            reply.addInt(c);
            return true;
        }

        if( command.get(0).asString() == "sub") {
            int c = sub(a, b);
            reply.addInt(c);
            return true;
        }

        reply.addString("Unknown command");
        reply.addString(command.get(0).asString().c_str());
        return true;
    }
示例#14
0
    void testStringWithNull() {
        report(0,"test string with null");
        char buf1[] = "hello world";
        char buf2[] = "hello world";
        buf2[5] = '\0';
        ConstString str1(buf1,12);
        ConstString str2(buf2,12);
        checkEqual(str1.length(),12,"unmodified string length ok");
        checkEqual(str2.length(),12,"modified string length ok");
        ConstString str3(str2);
        checkEqual(str3.length(),12,"copied string length ok");
        Bottle bot;
        bot.addString(str2);
        checkEqual(bot.get(0).asString().length(),12,"bottled string asString() length ok");
        checkEqual(bot.get(0).toString().length(),12,"bottled string toString() length ok");
        Bottle bot2 = bot;
        checkEqual(bot2.get(0).asString().length(),12,"bottled, copied string length ok");

        Bottle bot3;
        bot.write(bot3);
        checkEqual(bot3.get(0).asString().length(),12,"bottled, serialized string length ok");

        Bottle bot4;
        bot4.fromString(bot.toString());
        checkEqual(bot4.get(0).asString().length(),12,"bottled, text-serialized string length ok");
    }
示例#15
0
bool SpringyFingersModel::getOutput(Value &out) const
{
    if (configured)
    {
        Value val[5];
        fingers[0].getOutput(val[0]);
        fingers[1].getOutput(val[1]);
        fingers[2].getOutput(val[2]);
        fingers[3].getOutput(val[3]);
        fingers[4].getOutput(val[4]);
        
        Bottle bOut; Bottle &ins=bOut.addList();
        ins.addDouble(val[0].asDouble());
        ins.addDouble(val[1].asDouble());
        ins.addDouble(val[2].asDouble());
        ins.addDouble(val[3].asDouble());
        ins.addDouble(val[4].asDouble());

        out.fromString(bOut.toString().c_str());

        return true;
    }
    else
        return false;
}
示例#16
0
bool test_surface_mesh() {
    printf("\n*** test_surface_mesh()\n");
    SurfaceMesh mesh;
    Box3D bb;
    mesh.meshName = "testing";
    bb.corners.push_back(PointXYZ(1,2,3));
    SurfaceMeshWithBoundingBox obj(mesh,bb);

    Bottle bot;
    bot.read(obj);

    SurfaceMeshWithBoundingBox obj2;
    bot.write(obj2);
    Bottle bot2;
    bot2.read(obj2);

    printf("mesh copy: %s -> %s\n", bot.toString().c_str(),
           bot2.toString().c_str());

    if (obj2.mesh.meshName!=obj.mesh.meshName) {
        printf("mesh name not copied correctly\n");
        return false;
    }
    if (obj2.boundingBox.corners.size()!=obj.boundingBox.corners.size()) {
        printf("corners not copied correctly\n");
        return false;
    }
    if (bot!=bot2) {
        printf("not copied correctly\n");
        return false;
    }

    return true;
}
示例#17
0
ConstString NameServer::apply(const ConstString& txt, const Contact& remote) {
    ConstString result = "no command given";
    mutex.wait();

    SplitString ss(txt.c_str());
    if (ss.size()>=2) {
        ConstString key = ss.get(1);
        //YARP_DEBUG(Logger::get(),ConstString("dispatching to ") + key);
        ss.set(1,remote.getHost().c_str());
        result = dispatcher.dispatch(this,key.c_str(),ss.size()-1,
                                     (char **)(ss.get()+1));
        if (result == "") {
            Bottle b = ndispatcher.dispatch(this,key.c_str(),ss.size()-1,
                                            (char **)(ss.get()+1));
            result = b.toString().c_str();
            if (result!="") {
                result = result + "\n";
                result = terminate(result);
            }
        }
        //YARP_DEBUG(Logger::get(), ConstString("name server request -- ") + txt);
        //YARP_DEBUG(Logger::get(), ConstString("name server result  -- ") + result);
    }
    mutex.post();
    return result;
}
void oInteractorThread::run() {
    count++;
    if(locationPort.getInputCount()) {
        Bottle* locBottle = &locationPort.prepare();
        locBottle=locationPort.read();
        if(locBottle!=0) {
            printf("Bottle: %s \n", locBottle->toString().c_str());
            Bottle* list = locBottle->get(1).asList();
            if (list!=0) {
                double x = list->get(0).asDouble();
                double y = list->get(1).asDouble();
                double z = list->get(2).asDouble();
                printf("x:%f,y:%f,z:%f \n",x,y,z);
                if((x>=-0.4) &&(x<=-0.2)&&(y>=-0.4)&&(y<=0.2)&&(z>=-0.1)&&(z<=0.5)) {
                    Vector xd(3);
                    xd(0) = x;
                    xd(1) = y;
                    xd(2) = z;
                    armCart->goToPositionSync(xd);
                }
            }
            int command = locBottle->get(0).asVocab();
        }
    }
}
示例#19
0
bool BufferedConnectionWriter::applyConvertTextMode()
{
    if (convertTextModePending) {
        convertTextModePending = false;

        Bottle b;
        StringOutputStream sos;
        for (size_t i = 0; i < lst_used; i++) {
            yarp::os::ManagedBytes& m = *(lst[i]);
            sos.write(m.usedBytes());
        }
        const std::string& str = sos.str();
        b.fromBinary(str.c_str(), (int)str.length());
        std::string replacement = b.toString() + "\n";
        for (auto& i : lst) {
            delete i;
        }
        lst_used = 0;
        target = &lst;
        lst.clear();
        stopPool();
        Bytes data((char*)replacement.c_str(), replacement.length());
        appendBlockCopy(data);
    }
    return true;
}
示例#20
0
// \todo TODO bug ? 
bool checkRequiredParamIsVectorOfString(yarp::os::Searchable& config,
                                     const std::string& paramName,
                                     std::vector<std::string> & output_vector)
{
    bool correct = !(config.findGroup(paramName).isNull());
    if( correct )
    correct = true;
    {
        Bottle ids = config.findGroup(paramName).tail();
        std::cout << "ids : " << ids.toString() << std::endl;
        std::cout << "ids : " << config.find(paramName).toString() << std::endl;
        output_vector.resize(ids.size());
        for(int i = 0; i < ids.size(); i++ )
        {
            output_vector[i] = ids.get(i).asString().c_str();
        }
    }

    if( !correct )
    {
        yError("CanBusInertialMTB: problem loading parameter %s as vector of string",paramName.c_str());
    }

    return correct;
}
示例#21
0
bool DeviceGroup::open(yarp::os::Searchable& config) {
    if (implementation==NULL) {
        implementation = new DeviceGroupHelper;
    }
    if (implementation==NULL) {
        printf("Out of memory\n");
        return false;
    }

    if (config.check("part","a list of section names, with each section containing a device")) {
        Bottle bot = config.findGroup("part").tail();
        printf("Assembly of: %s\n", bot.toString().c_str());
        for (int i=0; i<bot.size(); i++) {
            ConstString name = bot.get(i).asString();
            printf("  %s -> %s\n", name.c_str(),
                   config.findGroup(name).toString().c_str());
            bool result = HELPER(implementation).add(name,
                          config.findGroup(name));
            if (!result) {
                HELPER(implementation).close();
                return false;
            }
        }
        return true;
    }
    return false;
}
示例#22
0
    virtual bool read(ConnectionReader& connection) {
        Bottle b;
        b.read(connection);
        // process data in b
        printf("Got %s\n", b.toString().c_str());
        if(waitForFirst) {
            xKeep = b.get(0).asInt();
            yKeep = b.get(1).asInt();
            waitForFirst = false;
        } else {
            if((b.get(0).asInt()<xKeep)||(b.get(1).asInt()<yKeep)){
                x = 0;
                y = 0;
                w = 0;
                h = 0;
            } else {
                x = xKeep;
                y = yKeep;
                w = b.get(0).asInt() - x;
                h = b.get(1).asInt() - y;
            }
            waitForFirst = true;
        }
        return true;

    }
bool cartControlReachAvoidThread::getAvoidanceVectorsFromPort()
{
    avoidanceStruct_t avoidanceStruct;
    avoidanceStruct.skin_part = SKIN_PART_UNKNOWN;
    avoidanceStruct.x.resize(3,0.0);
    avoidanceStruct.n.resize(3,0.0);
    
    Bottle* avoidanceMultiBottle = inportAvoidanceVectors.read(false);
    if(avoidanceMultiBottle != NULL){
         yDebug("getAvoidanceVectorsFromPort(): There were %d bottles on the port.\n",avoidanceMultiBottle->size());
         for(int i=0; i< avoidanceMultiBottle->size();i++){
             Bottle* avoidanceStructBottle = avoidanceMultiBottle->get(i).asList();
             yDebug("Bottle %d contains %s", i,avoidanceStructBottle->toString().c_str());
             avoidanceStruct.skin_part =  (SkinPart)(avoidanceStructBottle->get(0).asInt());
             avoidanceStruct.x(0) = avoidanceStructBottle->get(1).asDouble();
             avoidanceStruct.x(1) = avoidanceStructBottle->get(2).asDouble();
             avoidanceStruct.x(2) = avoidanceStructBottle->get(3).asDouble();
             avoidanceStruct.n(0) = avoidanceStructBottle->get(4).asDouble();
             avoidanceStruct.n(1) = avoidanceStructBottle->get(5).asDouble();
             avoidanceStruct.n(2) = avoidanceStructBottle->get(6).asDouble();
             avoidanceVectors.push_back(avoidanceStruct);
         }
        return true;
    }
    else{
       yDebug("getAvoidanceVectorsFromPort(): no avoidance vectors on the port.") ;  
       return false;
    };
}
示例#24
0
void boardDumperThread::run()
{
    //printf("Entering the main thread\n");
    //enc->getEncoders(data);
    //Bottle bData;
    //for (int i = 0; i < numberOfJointsRead; i++)
    //  {
    //    dataRead[i] = data[dataMap[i]];
    //    bData.addDouble(dataRead[i]);
    //  }
    //port->write(bData);

    if (getter)
    {
        //printf("Getter is getting something\n");
        getter -> getData(data);

        //fprintf(stderr, "Time is %lf \n", stmp.getTime());

        Bottle bData;
        for (int i = 0; i < numberOfJointsRead; i++)
        {
            //printf("%.2f \n", data[dataMap[i]]);
            dataRead[i] = data[dataMap[i]];
            bData.addDouble(dataRead[i]);
        }

        if (getter->getStamp(stmp)) 
        {
            if (stmp.isValid())
            {
                port->setEnvelope(stmp);
            }
            else
            {
                //stmp.update();
                stmp=Stamp(-1,0.0);
                port->setEnvelope(stmp);
            }
        }
        else
        {
            fprintf(stderr, "boardDumperThread::warning. Trying to get a stamp without a proper IPreciselyTimed defined. \n");
        }

        if (logFile)
        {
            char buff [20];
            sprintf(buff,"%d ",stmp.getCount());
            fputs (buff,logFile);
            sprintf(buff,"%f ",stmp.getTime());
            fputs (buff,logFile);
            fputs (bData.toString().c_str(),logFile);
            fputs ("\n",logFile);
        }

        port->write(bData);
    }
}  
示例#25
0
	void SamIter(void)
	{
		Bottle *b = bRead.read(false);

		//while (bRead.getPendingReads() > 0)
		//	b = bRead.read(false); // get in the input from the port, if you want it to wait use true, else use false

		if(b!=NULL)						 // check theres data
		{
			
			if(b->size()>0)
			{
				task = b->get(0).asString();
				int iStatus=atoi(b->get(1).asString().c_str());
				//check if message is related to charging
				if(task.compare("ChargingStatus")==0)
				{
					
					//relay switch: off mobile base and close modules
					if(iStatus==0)
					{
						yarp::os::Time::delay(10);
						CPhidgetInterfaceKit_setOutputState (ifKit, 6, 0);
						system("stop.bat");
						bCharging=true;
					}
					else if(iStatus==1 && bCharging==true)//relay switch: on mobile base and start modules
					{
						CPhidgetInterfaceKit_setOutputState (ifKit, 6, 1);
						yarp::os::Time::delay(2);
						system("start.bat");
						bCharging=false;
					}
				}
				else
				{
					iflag = iStatus;
					//start timer 
					if(iflag==1)
						time (&start);
				}
			
			}

			std::cout << "got a task " << b->toString().c_str() << std::endl;
		}

		//send phone ring sensor data only if sensor value is changed
		if(bPhoneRingChanged)
		{
			// send back a bottle with current voltage value
			Bottle& b3 = bPhone.prepare();	  // prepare the bottle/port
			b3.clear();
			b3.addInt( iPhoneRing ); // indicates robot voltage		
			bPhone.writeStrict();
			bPhoneRingChanged=false;
		}

	}
示例#26
0
  	void onRead(Bottle &v)
	{
        mutex.wait(); 
        Datum=v;
        //Time::delay(5);
        mutex.post();
        fprintf(stderr, "Callback got: %s\n",Datum.toString().c_str());
    }
示例#27
0
bool MapGrid2D::loadROSParams(string ros_yaml_filename, string& pgm_occ_filename, double& resolution, double& orig_x, double& orig_y, double& orig_t )
{
    std::string file_string;
    std::ifstream file;
    file.open(ros_yaml_filename.c_str());
    if (!file.is_open())
    {
        yError() << "failed to open file" << ros_yaml_filename;
        return false;
    }

    string line;
    while (getline(file, line))
    {
        if (line.find("origin") != std::string::npos)
        {
            std::replace(line.begin(), line.end(), ',', ' ');
            std::replace(line.begin(), line.end(), '[', '(');
            std::replace(line.begin(), line.end(), ']', ')');
            /*
            auto it = line.find('[');
            if (it != string::npos) line.replace(it, 1, "(");
            it = line.find(']');
            if(it != string::npos) line.replace(it, 1, ")");*/
        }
        file_string += (line + '\n');
    }
    file.close();

    bool ret = true;
    Bottle bbb;
    bbb.fromString(file_string);
    string debug_s = bbb.toString();

    if (bbb.check("image:") == false) { yError() << "missing image"; ret = false; }
    pgm_occ_filename = bbb.find("image:").asString();
    //ppm_flg_filename = (pgm_occ_filename.substr(0, pgm_occ_filename.size()-4))+"_yarpflags"+".ppm";

    if (bbb.check("resolution:") == false) { yError() << "missing resolution"; ret = false; }
    resolution = bbb.find("resolution:").asDouble();

    if (bbb.check("origin:") == false) { yError() << "missing origin"; ret = false; }
    Bottle* b = bbb.find("origin:").asList();
    if (b)
    {
        orig_x = b->get(0).asDouble();
        orig_y = b->get(1).asDouble();
        orig_t = b->get(2).asDouble();
    }

    if (bbb.check("occupied_thresh:"))
    {m_occupied_thresh = bbb.find("occupied_thresh:").asDouble();}

    if (bbb.check("free_thresh:"))
    {m_free_thresh = bbb.find("free_thresh:").asDouble();}

    return ret;
}
示例#28
0
bool Agent::fromBottle(Bottle b)
{
    if (!this->Object::fromBottle(b))
        return false;

    if (!b.check("belief")||!b.check("emotions"))
        return false;

    m_belief.clear();
    Bottle* beliefs = b.find("belief").asList();
    for(int i=0; i<beliefs->size() ; i++)
    {
        Bottle* bRelation = beliefs->get(i).asList();
        Relation r(*bRelation);
        m_belief.push_back(r);
    }

    m_emotions_intrinsic.clear();
    Bottle* emotions = b.find("emotions").asList();
    for(int i=0; i<emotions->size() ; i++)
    {
        Bottle* bEmo = emotions->get(i).asList();
        string emotionName = bEmo->get(0).asString().c_str();
        double emotionValue = bEmo->get(1).asDouble();
        m_emotions_intrinsic[emotionName.c_str()] = emotionValue;
    }

    m_drives.clear();
    Bottle* drivesProperty = b.find("drives").asList();
    string drivesDebug = drivesProperty->toString().c_str();
    for(int i=0; i<drivesProperty->size() ; i++)
    {
            Bottle* bD = drivesProperty->get(i).asList();
            string drivesDebug1 = bD->toString().c_str();
            Drive currentDrive;
            currentDrive.fromBottle(*bD);
            m_drives[currentDrive.name] = currentDrive;

    }
               
    Bottle* bodyProperty = b.find("body").asList();
    m_body.fromBottle(*bodyProperty);

    return true;
}
示例#29
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;
}
示例#30
0
Bottle learnPrimitive::actionCommand(string sActionName, string sArg){

    Bottle bOutput;

    //1. check if primitive is known
    //   vPrimitiveActionBottle =
    //   open    (hand)     ( (unfold thumb) (unfold index) (unfold middle) (unfold ring) )
    //   close   (hand)     ( (fold thumb) (fold index) (fold middle) (fold ring) )
    //   b.get(1) b.get(2)  b.get(3)
    //   name     arg        list of proto-action
    Bottle bSubActionList;
    for(std::vector<yarp::os::Bottle>::iterator it = vActionBottle.begin(); it < vActionBottle.end(); it++){
        string currentName = it->get(0).toString();
        //yInfo() << "Current name of the knwon actions : " << currentName ;
        if(currentName == sActionName){
            yInfo() << "found " << currentName << "as a known complex action";
            string currentArg = it->get(1).toString();
            if(currentArg == sArg){
                yInfo() << "and we have a corresponding argument " << currentArg ;
                for(int i = 0; i < it->get(2).asList()->size(); i++){
                    bSubActionList.addList() = *it->get(2).asList()->get(i).asList() ;
                }
                break;
            } else {
                yInfo() << " BUT argument " << currentArg << " does NOT match" ;
            }

        }
    }

    if (bSubActionList.size() == 0){
        yError() << " error in learnPrimitive::actionCommand | action '" << sActionName << " " << sArg << "' is NOT known";
        bOutput.addString("error");
        bOutput.addString("action is NOT known");
        return bOutput ;
    }

    yInfo() << "Actions to do : " << bSubActionList.toString() ;

    for(int i = 0; i < bSubActionList.size(); i++){
        yInfo() << "action #" << i << " : "<< bSubActionList.get(i).asList()->get(0).toString() << " the " << bSubActionList.get(i).asList()->get(1).asList()->get(0).toString() ;

        //1. check if subaction is a proto 
        if ( mProtoActionEnd.find(bSubActionList.get(i).asList()->get(0).toString()) != mProtoActionEnd.end() ) {  //proto-action
            bOutput.addList() = protoCommand(bSubActionList.get(i).asList()->get(0).toString(), bSubActionList.get(i).asList()->get(1).asList()->get(0).toString());
        } else { //primitive
            bOutput.addList() = primitiveCommand(bSubActionList.get(i).asList()->get(0).toString(), bSubActionList.get(i).asList()->get(1).asList()->get(0).toString());
            yarp::os::Time::delay(2);
        } //else { //another action

        //}
    }

    bOutput.addString("ack");

    return bOutput;
}