コード例 #1
0
ファイル: humanRobotDump.cpp プロジェクト: GunnyPong/wysiwyd
/*
*   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);
}
コード例 #2
0
ファイル: module.cpp プロジェクト: iron76/Teaching
    /*
    * 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;
    }
コード例 #3
0
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);
    }
}
コード例 #4
0
ファイル: world.cpp プロジェクト: MagnusJohnsson/wysiwyd
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);
}
コード例 #5
0
ファイル: NameServer.cpp プロジェクト: apaikan/yarp
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;
}
コード例 #6
0
ファイル: SubscriberOnSql.cpp プロジェクト: JoErNanO/yarp
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;
}
コード例 #7
0
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;


}
コード例 #8
0
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;
}
コード例 #9
0
    bool updateModule()
    {
        if (imgOutPort.getOutputCount()>0)
        {
            if (ImageOf<PixelBgr> *pImgBgrIn=imgInPort.read(false))
            {
                Vector xa,oa;
                iarm->getPose(xa,oa);

                Matrix Ha=axis2dcm(oa);
                Ha.setSubcol(xa,0,3);

                Vector v(4,0.0); v[3]=1.0;
                Vector c=Ha*v;

                v=0.0; v[0]=0.05; v[3]=1.0;
                Vector x=Ha*v;

                v=0.0; v[1]=0.05; v[3]=1.0;
                Vector y=Ha*v;

                v=0.0; v[2]=0.05; v[3]=1.0;
                Vector z=Ha*v;

                v=solution; v.push_back(1.0);
                Vector t=Ha*v;

                Vector pc,px,py,pz,pt;
                int camSel=(eye=="left")?0:1;
                igaze->get2DPixel(camSel,c,pc);
                igaze->get2DPixel(camSel,x,px);
                igaze->get2DPixel(camSel,y,py);
                igaze->get2DPixel(camSel,z,pz);
                igaze->get2DPixel(camSel,t,pt);

                CvPoint point_c=cvPoint((int)pc[0],(int)pc[1]);
                CvPoint point_x=cvPoint((int)px[0],(int)px[1]);
                CvPoint point_y=cvPoint((int)py[0],(int)py[1]);
                CvPoint point_z=cvPoint((int)pz[0],(int)pz[1]);
                CvPoint point_t=cvPoint((int)pt[0],(int)pt[1]);

                cvCircle(pImgBgrIn->getIplImage(),point_c,4,cvScalar(0,255,0),4);
                cvCircle(pImgBgrIn->getIplImage(),point_t,4,cvScalar(255,0,0),4);

                cvLine(pImgBgrIn->getIplImage(),point_c,point_x,cvScalar(0,0,255),2);
                cvLine(pImgBgrIn->getIplImage(),point_c,point_y,cvScalar(0,255,0),2);
                cvLine(pImgBgrIn->getIplImage(),point_c,point_z,cvScalar(255,0,0),2);
                cvLine(pImgBgrIn->getIplImage(),point_c,point_t,cvScalar(255,255,255),2);

                tip.clear();
                tip.addInt(point_t.x);
                tip.addInt(point_t.y);

                imgOutPort.prepare()=*pImgBgrIn;
                imgOutPort.write();
            }
        }

        return true;
    }
コード例 #10
0
    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");
        }
    }
コード例 #11
0
ファイル: Sound.cpp プロジェクト: JoErNanO/yarp
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);
}
コード例 #12
0
    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");
        }
    }
コード例 #13
0
ファイル: main.cpp プロジェクト: xufango/contrib_bk
    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());
    }
コード例 #14
0
bool skinCalibrationClient::quitModule(){
	Bottle b;
    b.addInt(quit);
	if(!skinCalibrationRpcPort.write(b))
        return false;
    return true;
}
コード例 #15
0
bool skinCalibrationClient::startCalibration(){
	Bottle b;
    b.addInt(start_calibration);
	if(!skinCalibrationRpcPort.write(b))
        return false;
    return true;
}
コード例 #16
0
ファイル: main.cpp プロジェクト: randaz81/icub-stuff
    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;
    }
コード例 #17
0
ファイル: BottleTest.cpp プロジェクト: andreadelprete/yarp
 void testModify() {
     report(0,"test bottle modification...");
     Bottle b;
     b.addInt(3);
     b.get(0) = 5;
     b.hasChanged();
     checkEqual(b.get(0).asInt(),5,"assignment works");
 }
コード例 #18
0
ファイル: main.cpp プロジェクト: xufango/contrib_bk
    void display_cog(string text,yarp::sig::Vector v, int r, int g, int b)
    {
        Bottle obj;
        obj.clear();
        obj.addString("object"); // command to add/update an object
        obj.addString(text.c_str());

        // object dimensions in millimiters 
        // (it will be displayed as an ellipsoid with the tag "my_object_name")
        bool fixed_size = false;
        if (fixed_size)
        {
            obj.addDouble(20);
            obj.addDouble(20);
            obj.addDouble(20);
        }
        else
        {
            obj.addDouble(pow(v[3],1.0/3.0)*20.0);
            obj.addDouble(pow(v[3],1.0/3.0)*20.0);
            obj.addDouble(pow(v[3],1.0/3.0)*20.0);
        }

        // object position in millimiters
        // reference frame: X=fwd, Y=left, Z=up
        obj.addDouble(v[0]*1000);
        obj.addDouble(v[1]*1000);
        obj.addDouble(v[2]*1000);

        // object orientation (roll, pitch, yaw) in degrees
        obj.addDouble(0);
        obj.addDouble(0);
        obj.addDouble(0);

        // object color (0-255)
        obj.addInt(r);
        obj.addInt(g);
        obj.addInt(b);
        // transparency (0.0=invisible 1.0=solid)
        obj.addDouble(0.9);

        port_out.prepare() = obj;
        port_out.write();
        Time::delay(0.1);
    }
コード例 #19
0
Bottle TRACKERManager::getPausedIDs()
{
    Bottle listID;

    for (int i = 0; i < pausedThreads.size(); i++  )
        listID.addInt(pausedThreads[i]);
    
    return listID;
}
コード例 #20
0
void matrixOfIntIntoBottle(const yarp::sig::Matrix m, Bottle &b)
{
    Vector v = toVector(m);

    for (unsigned int i = 0; i < v.size(); i++)
    {
        b.addInt(int(v[i]));
    }
}
コード例 #21
0
//*************************************************************************************************************************
bool ParamHelperBase::checkParamConstraints(int id, const Bottle &v, Bottle &reply)
{
    if(!hasParam(id))
    {
        reply.addString("There is no parameter with the specified id: ");
        reply.addInt(id);
        return false;
    }
    return paramList[id]->checkConstraints(v, &reply);
}
コード例 #22
0
Bottle TRACKERManager::getIDs()
{
    Bottle listID;
    std::map< unsigned int, ParticleThread* >::iterator itr;
    for (itr = workerThreads.begin(); itr!=workerThreads.end(); itr++)
        listID.addInt(itr->first);
    

    return listID;
}
コード例 #23
0
ファイル: PortTest.cpp プロジェクト: JoErNanO/yarp
 virtual bool read(ConnectionReader& connection) {
     Bottle receive;
     receive.read(connection);
     receive.addInt(5);
     ConnectionWriter *writer = connection.getWriter();
     if (writer!=NULL) {
         receive.write(*writer);
     }
     return true;
 }
コード例 #24
0
void vtWThread::sendGuiEvents()
{
    if (outPortGui.getOutputCount()>0)
    {
        int counter = 1;
        Bottle obj;

        for (std::vector<IncomingEvent>::const_iterator it = events.begin() ; it != events.end(); ++it){
         
            stringstream ss;
            obj.clear();
            obj.addString("object");
            ss << "obstacle" << counter;
            obj.addString(ss.str());
        
            // size 
            obj.addDouble(1000.0* (*it).Radius);
            obj.addDouble(1000.0* (*it).Radius);
            obj.addDouble(1000.0* (*it).Radius);
        
            // positions
            obj.addDouble(1000.0 * (*it).Pos[0]);
            obj.addDouble(1000.0 * (*it).Pos[1]);
            obj.addDouble(1000.0 * (*it).Pos[2]);
        
            // orientation
            obj.addDouble(0.0);
            obj.addDouble(0.0);
            obj.addDouble(0.0);
        
            // color
            obj.addInt(50 + (*it).Threat*200.0); //threatening objects will be more red
            obj.addInt(50);
            obj.addInt(50);
        
            // transparency
            obj.addDouble(0.9);
        
            outPortGui.write(obj);
            counter++;
       }
    }
}
コード例 #25
0
Bottle SimBox::releaseObjectBottle(iCubArm arm) {

    Bottle cmd;
    cmd.addString("world");
    cmd.addString("grab");
    cmd.addString("box");
    cmd.addInt   (objSubIndex);
    switch(arm) {
        case RIGHT:
            cmd.addString("right");
            break;
        case LEFT:
            cmd.addString("left");
            break;
        default:
            cmd.addString("right");
    }
    cmd.addInt(0);
    return cmd;
}
コード例 #26
0
Bottle SimModel::grabObjectBottle(iCubArm arm) {

    Bottle cmd;
    cmd.addString("world");
    cmd.addString("grab");
    cmd.addString("model");
    cmd.addInt   (objSubIndex);
    switch(arm) {
        case RIGHT:
            cmd.addString("right");
            break;
        case LEFT:
            cmd.addString("left");
            break;
        default:
            cmd.addString("right");
    }
    cmd.addInt(1);
    return cmd;
}
コード例 #27
0
Bottle SimBox::moveObjectBottle() {

    Bottle cmd;
    cmd.addString("world");
    cmd.addString("set");
    cmd.addString("box");
    cmd.addInt   (objSubIndex);
    cmd.addDouble(positionX);
    cmd.addDouble(positionY);
    cmd.addDouble(positionZ);
    return cmd;
}
コード例 #28
0
Bottle SimBox::rotateObjectBottle() {

    Bottle cmd;
    cmd.addString("world");
    cmd.addString("rot");
    cmd.addString("box");
    cmd.addInt   (objSubIndex);
    cmd.addDouble(rotationX);
    cmd.addDouble(rotationY);
    cmd.addDouble(rotationZ);
    return cmd;
}
コード例 #29
0
ファイル: simple_sender.cpp プロジェクト: johnty/timer
int main(int argc, char *argv[]) {
    Network yarp;
    Port output;
	string from_port;
	string to_port;

	if (argc >=3) {
		from_port = string(argv[1]);
		to_port = string(argv[2]);
	}
	else {
		from_port = "/sender";
		to_port = "/timer";
	}
	output.open(from_port.c_str());

	yarp::os::Network::connect(from_port.c_str(),to_port.c_str());
    int top = 100000;
	Random::seed(std::clock());
    for (int i=1; i<=top; i++) {
        // prepare a message
        Bottle bot;
		bot.addString("testing");
		bot.addInt(i); //id
		bot.addInt(i); //dev type
		bot.addInt(i); //server type
		bot.addInt(i); //time stamp
		for (int j=0; j<15; j++) { //15 sensor values
			bot.addDouble(Random::uniform());
		}
        // send the message
        output.write(bot);
        //printf("Sent message: %s\n", bot.toString().c_str());
        // wait a while
        //Time::delay(1);
    }
    output.close();
    return 0;
}
コード例 #30
0
ファイル: world.cpp プロジェクト: MagnusJohnsson/wysiwyd
void GuiUpdaterModule::addDrives(Agent* a)
{
    Vector driveDimension(3);
    driveDimension[0] = 10;
    driveDimension[1] = 500;
    driveDimension[2] = 10;

    //Create drives
    Vector overHeadPos = iCub->getSelfRelativePosition(a->m_ego_position);
    int driveCount = 0;
    for(map<string,Drive>::iterator drive = a->m_drives.begin(); drive != a->m_drives.end(); drive++)
    {    

        double ySize = driveDimension[1] * drive->second.value + 10;
        ostringstream opcTag;
        opcTag<<a->name()<<"_"<<drive->second.name;

        Bottle cmd;
        cmd.addString("object");
        cmd.addString(opcTag.str().c_str());              
        cmd.addDouble(driveDimension[0]);
        cmd.addDouble(ySize);
        cmd.addDouble(driveDimension[2]);
        cmd.addDouble(overHeadPos[0] *1000.0);
        cmd.addDouble(overHeadPos[1] *1000.0 - ySize / 2.0 + driveDimension[1] / 2.0);
        cmd.addDouble(overHeadPos[2] *1000.0 + 500 + driveCount * (driveDimension[2]+30) );        
        cmd.addDouble(a->m_ego_orientation[0]);             
        cmd.addDouble(a->m_ego_orientation[1]);             
        cmd.addDouble(a->m_ego_orientation[2]);
        Vector color = getDriveColor(drive->second);
        cmd.addInt((int)color[0]);            // color R
        cmd.addInt((int)color[1]);            // color G
        cmd.addInt((int)color[2]);            // color B
        cmd.addDouble(1);                     // alpha coefficient [0,1]
        toGui.write(cmd);
        driveCount++;
    }
}