C++ Bottle::addInt方法代码示例

/*
*   Dump in the port DumperPort the human skeleton, and the object of the OPC: sObjectToDump
*
*/
void humanRobotDump::DumpHumanObject()
{
    Agent* ag = iCub->opc->addOrRetrieveEntity<Agent>(sAgentName);

    Bottle bDump;
    bDump.addString(sActionName);
    bDump.addString(sObjectToDump);
    bDump.addList() = ag->m_body.asBottle();

    iCub->opc->checkout();
    list<Entity*> lEntity = iCub->opc->EntitiesCacheCopy();
    for (list<Entity*>::iterator itEnt = lEntity.begin(); itEnt != lEntity.end(); itEnt++)
    {
        if ((*itEnt)->entity_type() == EFAA_OPC_ENTITY_AGENT ||
            (*itEnt)->entity_type() == EFAA_OPC_ENTITY_OBJECT ||
            (*itEnt)->entity_type() == EFAA_OPC_ENTITY_RTOBJECT)
        {
            if ((*itEnt)->name() != "icub")
            {
                Object* ob = iCub->opc->addOrRetrieveEntity<Object>((*itEnt)->name());
                Bottle bObject;
                bObject.addString(ob->name());
                bObject.addDouble(ob->m_ego_position[0]);
                bObject.addDouble(ob->m_ego_position[1]);
                bObject.addDouble(ob->m_ego_position[2]);
                bObject.addInt(ob->m_present);
                bDump.addList() = bObject;
            }
        }
    }

    bDump.addInt(m_iterator);
    
    DumperPort.write(bDump);
}

    /*
    * 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;
    }

void vtWThread::sendGuiTarget()
{
    if (outPortGui.getOutputCount()>0)
    {
        Bottle obj;
        obj.addString("object");
        obj.addString("Target");
     
        // size 
        obj.addDouble(50.0);
        obj.addDouble(50.0);
        obj.addDouble(50.0);
    
        // positions
        obj.addDouble(1000.0*events[0].Pos[0]);
        obj.addDouble(1000.0*events[0].Pos[1]);
        obj.addDouble(1000.0*events[0].Pos[2]);
    
        // orientation
        obj.addDouble(0.0);
        obj.addDouble(0.0);
        obj.addDouble(0.0);
    
        // color
        obj.addInt(255);
        obj.addInt(125);
        obj.addInt(125);
    
        // transparency
        obj.addDouble(0.9);
    
        outPortGui.write(obj);
    }
}

void GuiUpdaterModule::addObject(Object* o, const string &opcTag)
{
    //Get the position of the object in the current reference frame of the robot (not the initial one)
    Vector inCurrentRootReference = iCub->getSelfRelativePosition(o->m_ego_position);
    //cout<<o->name()<<" init Root: \t \t"<<o->m_ego_position.toString(3,3)<<endl
    //    <<o->name()<<" current Root: \t \t"<<inCurrentRootReference.toString(3,3)<<endl;

    Bottle cmd;
    cmd.addString("object");
    cmd.addString(opcTag.c_str());
                                
    cmd.addDouble(o->m_dimensions[0] *1000.0);    // dimX in [mm]
    cmd.addDouble(o->m_dimensions[1] *1000.0);    // dimY in [mm]
    cmd.addDouble(o->m_dimensions[2] *1000.0);    // dimZ in [mm]
    cmd.addDouble(inCurrentRootReference[0] *1000.0);        // posX in [mm]
    cmd.addDouble(inCurrentRootReference[1] *1000.0);        // posY in [mm]
    cmd.addDouble(inCurrentRootReference[2] *1000.0);        // posZ in [mm]
    cmd.addDouble(o->m_ego_orientation[0] - iCub->m_ego_orientation[0]);             // Deal with the object orientation that is moving with the base
    cmd.addDouble(o->m_ego_orientation[1] - iCub->m_ego_orientation[1]);             // "
    cmd.addDouble(o->m_ego_orientation[2] - iCub->m_ego_orientation[2]);              // "
    cmd.addInt((int)o->m_color[0]);            // color R
    cmd.addInt((int)o->m_color[1]);            // color G
    cmd.addInt((int)o->m_color[2]);            // color B
    cmd.addDouble(1);                     // alpha coefficient [0,1]
    toGui.write(cmd);
}

Bottle NameServer::botify(const Contact& address) {
    Bottle result;
    if (address.isValid()) {
        Bottle bname;
        bname.addString("name");
        bname.addString(address.getRegName().c_str());
        Bottle bip;
        bip.addString("ip");
        bip.addString(address.getHost().c_str());
        Bottle bnum;
        bnum.addString("port_number");
        bnum.addInt(address.getPort());
        Bottle bcarrier;
        bcarrier.addString("carrier");
        bcarrier.addString(address.getCarrier().c_str());

        result.addString("port");
        result.addList() = bname;
        result.addList() = bip;
        result.addList() = bnum;
        result.addList() = bcarrier;
    } else {
        Bottle bstate;
        bstate.addString("error");
        bstate.addInt(-2);
        bstate.addString("port not known");
        result.addString("port");
        result.addList() = bstate;
    }
    return result;
}

bool SubscriberOnSql::listSubscriptions(const ConstString& port,
                                        yarp::os::Bottle& reply) {
    mutex.wait();
    sqlite3_stmt *statement = NULL;
    char *query = NULL;
    if (ConstString(port)!="") {
        query = sqlite3_mprintf("SELECT s.srcFull, s.DestFull, EXISTS(SELECT topic FROM topics WHERE topic = s.src), EXISTS(SELECT topic FROM topics WHERE topic = s.dest), s.mode FROM subscriptions s WHERE s.src = %Q OR s.dest= %Q ORDER BY s.src, s.dest",port.c_str(),port.c_str());
    } else {
        query = sqlite3_mprintf("SELECT s.srcFull, s.destFull, EXISTS(SELECT topic FROM topics WHERE topic = s.src), EXISTS(SELECT topic FROM topics WHERE topic = s.dest), s.mode FROM subscriptions s ORDER BY s.src, s.dest");
    }
    if (verbose) {
        printf("Query: %s\n", query);
    }
    int result = sqlite3_prepare_v2(SQLDB(implementation),query,-1,&statement,
                                    NULL);
   if (result!=SQLITE_OK) {
        const char *msg = sqlite3_errmsg(SQLDB(implementation));
        if (msg!=NULL) {
            fprintf(stderr,"Error: %s\n", msg);
        }
    }
    reply.addString("subscriptions");
    while (result == SQLITE_OK && sqlite3_step(statement) == SQLITE_ROW) {
        char *src = (char *)sqlite3_column_text(statement,0);
        char *dest = (char *)sqlite3_column_text(statement,1);
        int srcTopic = sqlite3_column_int(statement,2);
        int destTopic = sqlite3_column_int(statement,3);
        char *mode = (char *)sqlite3_column_text(statement,4);
        Bottle& b = reply.addList();
        b.addString("subscription");
        Bottle bsrc;
        bsrc.addString("src");
        bsrc.addString(src);
        Bottle bdest;
        bdest.addString("dest");
        bdest.addString(dest);
        b.addList() = bsrc;
        b.addList() = bdest;
        if (mode!=NULL) {
            if (mode[0]!='\0') {
                Bottle bmode;
                bmode.addString("mode");
                bmode.addString(mode);
                b.addList() = bmode;
            }
        }
        if (srcTopic||destTopic) {
            Bottle btopic;
            btopic.addString("topic");
            btopic.addInt(srcTopic);
            btopic.addInt(destTopic);
            b.addList() = btopic;
        }
    }
    sqlite3_finalize(statement);
    sqlite3_free(query);
    mutex.post();

    return true;
}

void CB::YARPConfigurationVariables::setVelocityControlMode(bool mode, string portName) {

    bool ok = true;

    // specify the local port names that connect to the velocityControl module
    string velocityOutputPortName = "/cb/configuration" + deviceName + "/vel:o";
    string velocityRPCOutputPortName = "/cb/configuration" + deviceName + "/vel/rpc:o";
    //    velocityPortName = portName + "/command";      
    velocityPortName = portName + "/fastCommand";      
    velocityRPCPortName = portName + "/input";      

    Bottle b;

    if(mode && !velocityControlMode) {

        // if we're turning on the velocityControlMode (and it wasnt previously on)

        // open and connect the data and config portsport
        ok &= velocityPort.open(velocityOutputPortName.c_str());
        ok &= Network::connect(velocityOutputPortName.c_str(),velocityPortName.c_str(),"tcp");
        Time::delay(0.5);
        ok &= velocityRPCPort.open(velocityRPCOutputPortName.c_str());
        ok &= Network::connect(velocityRPCOutputPortName.c_str(),velocityRPCPortName.c_str(),"tcp");

        // send gain and maxVel paramaters to the vc module 
        for(int i=0; i<mask.size(); i++) {
            if(mask[i]) {
                cout << "setting vc params joint[" << i << "]: gain=" << velocityGain << ", svel=" << maxSetVal << endl;
                b.clear();
                b.addString("gain");
                b.addInt(i);
                b.addDouble(velocityGain);
                velocityRPCPort.write(b);
                Time::delay(0.1);
                b.clear();
                b.addString("svel");
                b.addInt(i);
                b.addDouble(maxSetVal);
                velocityRPCPort.write(b);
                Time::delay(0.1);
            }
        }


    } else if(!mode && velocityControlMode) {

        // turning off velocity control mode by disconnecting and closing ports
        ok &= Network::disconnect(velocityOutputPortName.c_str(),velocityPortName.c_str());
        ok &= Network::disconnect(velocityRPCOutputPortName.c_str(),velocityRPCPortName.c_str());
        velocityRPCPort.close();
        velocityPort.close();

    }

    // set the current mode
    velocityControlMode = mode;


}

bool utManagerThread::getPointFromStereo()
{
    Bottle cmdSFM;
    Bottle respSFM;
    cmdSFM.clear();
    respSFM.clear();
    cmdSFM.addString("Root");
    cmdSFM.addInt(int(templatePFTrackerPos(0)));
    cmdSFM.addInt(int(templatePFTrackerPos(1)));
    SFMrpcPort.write(cmdSFM, respSFM);

    // Read the 3D coords and compute the distance to the set reference frame origin
    if (respSFM.size() == 3)
    {
        Vector SFMtmp(3,0.0);
        SFMtmp(0) = respSFM.get(0).asDouble(); // Get the X coordinate
        SFMtmp(1) = respSFM.get(1).asDouble(); // Get the Y coordinate
        SFMtmp(2) = respSFM.get(2).asDouble(); // Get the Z coordinate

        if (SFMtmp(0) == 0.0 && SFMtmp(1) == 0.0 && SFMtmp(2) == 0.0)
        {
            return false;
        }

        SFMPos = SFMtmp;
        return true;
    } 

    return false;
}

    void testUnbufferedSubscriber() {
        report(0,"Unbuffereded Subscriber test");

        Node n("/node");
        BufferedPort<Bottle> pout;
        pout.setWriteOnly();
        pout.open("very_interesting_topic");

        {
            Node n2("/node2");
            Subscriber<Bottle> pin("/very_interesting_topic");

            waitForOutput(pout,10);

            Bottle& b = pout.prepare();
            b.clear();
            b.addInt(42);
            pout.write();

            Bottle bin;
            bin.addInt(99);
            pin.read(bin);
            pout.waitForWrite();
            checkEqual(bin.get(0).asInt(),42,"message is correct");
        }
    }

bool Sound::write(ConnectionWriter& connection) {
    // lousy format - fix soon!
    FlexImage& img = HELPER(implementation);
    Bottle bot;
    bot.addInt(frequency);
    return PortablePair<FlexImage,Bottle>::writePair(connection,img,bot);
}

    void testUnbufferedPublisher() {
        report(0,"Unbuffered Publisher test");

        Node n("/node");
        Publisher<Bottle> p("/very_interesting_topic");

        {
            Node n2("/node2");
            BufferedPort<Bottle> pin;
            pin.setReadOnly();
            pin.setStrict();
            pin.open("very_interesting_topic");

            waitForOutput(p,10);

            Bottle b;
            b.addInt(42);
            p.write(b);

            Bottle *bin = pin.read();
            checkTrue(bin!=NULL,"message arrived");
            if (!bin) return;
            checkEqual(bin->get(0).asInt(),42,"message is correct");
        }
    }

    void getObjectList(Bottle &reply)
    {
        double t0=Time::now();

        int max_size=0;
        vector<Blob> max_blobs;

        while(Time::now()-t0<0.5)
        {
            mutex.wait();
            reply.clear();

            int size=0;
            for(unsigned int i=0; i<blobs.size(); i++)
                if(blobs[i].getBest()!="")
                    size++;

            if(max_size<size)
            {
                max_size=size;
                max_blobs=blobs;
            }
            mutex.post();   

            Time::delay(0.05);        
        }


        reply.addInt(max_size);

        for(unsigned int i=0; i<max_blobs.size(); i++)
            if(max_blobs[i].getBest()!="")
                reply.addString(max_blobs[i].getBest().c_str());
    }

bool skinCalibrationClient::quitModule(){
	Bottle b;
    b.addInt(quit);
	if(!skinCalibrationRpcPort.write(b))
        return false;
    return true;
}

bool skinCalibrationClient::startCalibration(){
	Bottle b;
    b.addInt(start_calibration);
	if(!skinCalibrationRpcPort.write(b))
        return false;
    return true;
}

    bool updateModule()
    {
        double verg = 0;

        Bottle bin;
        Bottle bout;
        inport.read(bin);
        if (reset_offset)
        {
            initial_offset[0] = bin.get(0).asDouble();
            initial_offset[1] = bin.get(1).asDouble();
            initial_offset[2] = bin.get(2).asDouble();
            initial_offset[3] = bin.get(3).asDouble();
            initial_offset[4] = bin.get(4).asDouble();
            initial_offset[5] = bin.get(5).asDouble();
            reset_offset = false;
        }

        double x_torque = fabs(bin.get(3).asDouble()) - initial_offset [3];
        double y_force = fabs(bin.get(1).asDouble()) - initial_offset[1];
        bout.addInt(2);
        double j_torque = x_torque + y_force * 0.70; //Nm + N * m
        bout.addDouble(j_torque); //j0
        bout.addDouble(j_torque); //j1
        bout.addDouble(j_torque); //j2
        bout.addDouble(j_torque); //j3
        bout.addDouble(j_torque); //j4
        bout.addDouble(j_torque); //j5
        outport.write(bout);

        return true;
    }