Exemple #1
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;
}
 //********************************************
 bool read(ConnectionReader &connection)
 {
     Bottle input;
     input.read(connection);
     if (input.size()>=7)
     {
         string arm=input.get(0).asString().c_str();
         transform(arm.begin(),arm.end(),arm.begin(),::tolower);
         mutex.lock();
         map<string,Data>::iterator it=data.find(arm);
         if (it!=data.end())
         {
             Data &d=it->second;
             d.point[0]=input.get(1).asDouble();
             d.point[1]=input.get(2).asDouble();
             d.point[2]=input.get(3).asDouble();
             d.dir[0]=input.get(4).asDouble();
             d.dir[1]=input.get(5).asDouble();
             d.dir[2]=input.get(6).asDouble();
             d.persistence=PPS_AVOIDANCE_PERSISTENCE;
             d.timeout=PPS_AVOIDANCE_TIMEOUT;
         }
         mutex.unlock();
     }
     return true;
 }
Exemple #3
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;

    }
 virtual bool read(ConnectionReader& reader) override {
     go.wait();
     mutex.lock();
     bool ok = last.read(reader);
     mutex.unlock();
     gone.post();
     return ok;
 }
 virtual bool read(ConnectionReader& reader) {
     go.wait();
     mutex.wait();
     bool ok = last.read(reader);
     mutex.post();
     gone.post();
     return ok;
 }
Exemple #6
0
 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;
 }
Exemple #7
0
 void testSerialCopy() {
     report(0,"test serialization by copy");
     Value v(3.14);
     Bottle b;
     b.read(v);
     checkEqualish(b.get(0).asDouble(),3.14,"copy to bottle succeeded");
     Bottle b2;
     b.write(b2);
     checkEqualish(b2.get(0).asDouble(),3.14,"copy from bottle succeeded");
 }
Exemple #8
0
 void checkReadWrite() {
     report(0,"check read/write");
     Value v(4.2);
     Bottle b;
     b.read(v);
     checkTrue(b.get(0).isFloat64(),"structure ok");
     checkEqualish(b.get(0).asFloat64(),4.2,"value ok");
     Value v2;
     b.write(v2);
     checkEqualish(v2.asFloat64(),4.2,"value reread ok");
 }
    bool read(ConnectionReader &connection)
    {
        Bottle data; data.read(connection);
        if (data.size()>=2)
        {
            LockGuard lg(mutex);
            cv::Point point(data.get(0).asInt(),data.get(1).asInt());
            contour.push_back(point);
        }

        return true;
    }
Exemple #10
0
    bool read(ConnectionReader &connection)
    {
        Bottle data;
        data.read(connection);
        if ((data.size()>=2) && enabled)
        {
            Vector xa,oa;
            iarm->getPose(xa,oa);

            Vector xe,oe;
            if (eye=="left")
                igaze->getLeftEyePose(xe,oe);
            else
                igaze->getRightEyePose(xe,oe);

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

            Matrix He=axis2dcm(oe);
            He.setSubcol(xe,0,3);

            Matrix H=Prj*SE3inv(He)*Ha;
            Vector p(2);
            p[0]=data.get(0).asDouble();
            p[1]=data.get(1).asDouble();

            if (logPort.getOutputCount()>0)
            {
                Vector &log=logPort.prepare();

                log=p;
                for (int i=0; i<H.rows(); i++)
                    log=cat(log,H.getRow(i));
                for (int i=0; i<Prj.rows(); i++)
                    log=cat(log,Prj.getRow(i));
                for (int i=0; i<Ha.rows(); i++)
                    log=cat(log,Ha.getRow(i));
                for (int i=0; i<He.rows(); i++)
                    log=cat(log,He.getRow(i));

                logPort.write();
            }

            mutex.wait();
            solver.addItem(p,H);
            mutex.post();
        }

        return true;
    }
Exemple #11
0
 virtual bool read(ConnectionReader& con) {
   Bottle b;
   if (!b.read(con)) {
     return false;
   }
   ConstString src = con.getRemoteContact().getName();
   ConstString txt = b.get(0).asString();
   printf("Reading [%s] from [%s]\n", txt.c_str(), src.c_str());
   if (src!=txt) {
     printf("YIKES!!!\n");
     exit(1);
   }
   return true;
 }
Exemple #12
0
 virtual bool read(ConnectionReader& connection) {
     Bottle receive;
     //printf("service provider reading data\n");
     receive.read(connection);
     //printf("service provider read data\n");
     receive.addInt(5);
     ConnectionWriter *writer = connection.getWriter();
     if (writer!=NULL) {
         //printf("service provider replying\n");
         receive.write(*writer);
         //printf("service provider replied\n");
     }
     return true;
 }
bool StreamConnectionReader::convertTextMode() {
    if (isTextMode()) {
        if (!convertedTextMode) {
            Bottle bot;
            bot.read(*this);
            BufferedConnectionWriter writer;
            bot.write(writer);
            String s = writer.toString();
            altStream.reset(s);
            in = &altStream;
            convertedTextMode = true;
        }
    }

    return true;
}
    //********************************************
    bool read(ConnectionReader &connection)
    {
        Bottle in;
        in.read(connection);

        if (in.size()>=1)
        {
            Bottle input;
            input=*(in.get(0).asList());

            if (input.size()>=7)
            {
                yDebug("[demoAvoidance] got command (%s)",input.toString().c_str());
                string arm=iCub::skinDynLib::SkinPart_s[input.get(0).asInt()];

                if (arm == "r_hand" || arm == "r_forearm")
                {
                    arm = "right";
                }
                else if (arm == "l_hand" || arm == "l_forearm")
                {
                    arm = "left";
                }

                transform(arm.begin(),arm.end(),arm.begin(),::tolower);
                mutex.lock();
                map<string,Data>::iterator it=data.find(arm);
                if (it!=data.end())
                {
                    Data &d=it->second;
                    d.point[0]=input.get(7).asDouble();
                    d.point[1]=input.get(8).asDouble();
                    d.point[2]=input.get(9).asDouble();
                    d.dir[0]=input.get(10).asDouble();
                    d.dir[1]=input.get(11).asDouble();
                    d.dir[2]=input.get(12).asDouble();
                    d.persistence=PPS_AVOIDANCE_PERSISTENCE;
                    d.timeout=PPS_AVOIDANCE_TIMEOUT;
                }
                mutex.unlock();
            }
        }

        return true;
    }
Exemple #15
0
bool NetworkClock::read(ConnectionReader& reader) {
    Bottle bot;
    bool ok = bot.read(reader);

    if(closing)
    {
        _time = -1;
        return false;
    }

    if (!ok && !closing)
    {
        YARP_ERROR(Logger::get(), "Error reading clock port");
        return false;
    }

    timeMutex.lock();
    sec = bot.get(0).asInt32();
    nsec = bot.get(1).asInt32();
    _time = sec + (nsec*1e-9);
    initted = true;
    timeMutex.unlock();

    listMutex.lock();
    Waiters* waiters = static_cast<Waiters*>(pwaiters);
    Waiters::iterator waiter_i;

    waiter_i = waiters->begin();
    while (waiter_i != waiters->end())
    {
        if (waiter_i->first - _time < 1E-12 )
        {
            Semaphore *waiterSemaphore = waiter_i->second;
            waiter_i = waiters->erase(waiter_i);
            if (waiterSemaphore)
                waiterSemaphore->post();
        }
        else
            ++waiter_i;
    }
    listMutex.unlock();
    return true;
}
Exemple #16
0
bool CalibModule::load()
{
    string fileName=rf->findFile("calibrationFile").c_str();
    if (fileName.empty())
    {
        yWarning("calibration file not found");
        return false;
    }

    Property data; data.fromConfigFile(fileName.c_str()); 
    Bottle b; b.read(data);

    mutex.lock();
    yInfo("loading experts from file: %s",fileName.c_str());
    for (int i=0; i<b.size(); i++)
        factory(b.get(i));
    mutex.unlock();

    return true;
}
Exemple #17
0
 virtual bool read(ConnectionReader& connection) {
     double now = Time::now();
     Bottle b;
     bool ok = b.read(connection);
     if (ok) {
         mutex.wait();
         ct++;
         if (ct>1) {
             double dt = now - lastTime;
             period.add(dt);
         }
         lastTime = now;
         printf("Period is %g +/- %g (%d)\n",
                period.mean(),
                period.deviation(),
                ct);
         mutex.post();
     }
     return ok;
 }
Exemple #18
0
bool yarp::os::impl::HttpCarrier::write(Protocol& proto, SizedWriter& writer) {
    DummyConnector con;
    con.setTextMode(true);
    for (size_t i=writer.headerLength(); i<writer.length(); i++) {
        con.getWriter().appendBlock(writer.data(i),writer.length(i));
    }
    Bottle b;
    b.read(con.getReader());

    ConstString body = b.find("web").toString();
    if (body.length()!=0) {
        ConstString header;
        header += NetType::toHexString(body.length()).c_str();
        header += "\r\n";

        Bytes b2((char*)header.c_str(),header.length());
        proto.os().write(b2);

        Bytes b3((char*)body.c_str(),body.length());
        proto.os().write(b3);

        proto.os().write('\r');
        proto.os().write('\n');

    } else {
        ConstString txt = b.toString() + "\r\n";
        ConstString header;
        header += NetType::toHexString(txt.length()).c_str();
        header += "\r\n";
        Bytes b2((char*)header.c_str(),header.length());
        proto.os().write(b2);
        Bytes b3((char*)txt.c_str(),txt.length());
        proto.os().write(b3);
        proto.os().write('\r');
        proto.os().write('\n');
    }
    proto.os().flush();
    return proto.os().isOk();
}
Exemple #19
0
bool YarpNameSpace::writeToNameServer(PortWriter& cmd,
                                      PortReader& reply,
                                      const ContactStyle& style) {
    Contact srv = getNameServerContact();
    String cmd0 = "NAME_SERVER";

    DummyConnector con0;
    cmd.write(con0.getWriter());
    Bottle in;
    in.read(con0.getReader());
    for (int i=0; i<in.size(); i++) {
        cmd0 += " ";
        cmd0 += in.get(i).toString().c_str();
    }
    NameClient& nic = HELPER(this);
    String result = nic.send(cmd0);
    Bottle reply2;
    reply2.addString(result.c_str());
    DummyConnector con;
    reply2.write(con.getWriter());
    reply.read(con.getReader());
    return result!="";
}
Exemple #20
0
    void checkPair() {
        report(0,"checking portable-pair serialization...");
        // potential problem reported by Miguel Sarabia Del Castillo

        Matrix m;
        size_t rr = 10;
        size_t cc = 5;
        makeTestMatrix(m,rr,cc);

        double value = 3.14;

        yarp::os::PortablePair<yarp::sig::Matrix, yarp::os::Value> msg, msg2;
        msg.head = m;
        msg.body = yarp::os::Value(value);

        DummyConnector con;
        msg.write(con.getWriter());
        ConnectionReader& reader = con.getReader();
        msg2.read(reader);
        checkEqual(msg.head.rows(),msg2.head.rows(),"matrix row match");
        checkEqual(msg.head.cols(),msg2.head.cols(),"matrix col match");
        checkEqualish(msg.body.asFloat64(),msg2.body.asFloat64(),"value match");

        Bottle bot;
        bot.read(msg);
        Bottle *bot1 = bot.get(0).asList();
        Bottle *bot2 = bot.get(1).asList();
        checkTrue(bot1!=nullptr&&bot2!=nullptr,"got head/body");
        if (bot1==nullptr || bot2==nullptr) return;
        checkEqual((size_t) bot1->get(0).asInt32(),rr,"row count matches");
        checkEqual((size_t) bot1->get(1).asInt32(),cc,"column count matches");
        Bottle *lst = bot1->get(2).asList();
        checkTrue(lst!=nullptr,"have data");
        if (!lst) return;
        checkEqual(lst->size(),(rr*cc),"data length matches");
        checkEqualish(bot2->get(0).asFloat64(),value,"value match");
    }
Exemple #21
0
 void stack(PortWriter& msg, const ConstString& tag) {
     mutex.wait();
     msgs.push_back(Bottle());
     if (tag!="") {
         Bottle b;
         b.read(msg);
         Bottle& back = msgs.back();
         back.clear();
         back.addString(tag);
         back.append(b);
     } else {
         msgs.back().read(msg);
     }
     if (available_threads==0) {
         if (threads.size()<max_threads || max_threads == 0) {
             available_threads++;
             threads.push_back(new MessageStackThread(*this));
             threads.back()->start();
         }
     }
     available_threads--;
     mutex.post();
     produce.post();
 }
Exemple #22
0
    virtual bool read(ConnectionReader& reader) {
        YTRACE("NameServer::read start");
        ConstString ref = "NAME_SERVER ";
        bool ok = true;
        ConstString msg = "?";
        bool haveMessage = false;
        if (ok) {
            if (reader.isTextMode()) {
                msg = reader.expectText().c_str();
            } else {
                // migrate to binary mode support, eventually optimize
                Bottle b;
                b.read(reader);
                msg = b.toString().c_str();
            }
            haveMessage = (msg!="");
            msg = ref + msg;
        }
        if (reader.isActive()&&haveMessage) {
            YARP_DEBUG(Logger::get(),ConstString("name server got message ") + msg);
            size_t index = msg.find("NAME_SERVER");
            if (index==0) {
                Contact remote = reader.getRemoteContact();
                YARP_DEBUG(Logger::get(),
                           ConstString("name server receiving from ") +
                           remote.toURI());
                YARP_DEBUG(Logger::get(),
                           ConstString("name server request is ") + msg);
                ConstString result = server->apply(msg,remote);
                ConnectionWriter *os = reader.getWriter();
                if (os!=YARP_NULLPTR) {
                    if (result=="") {
                        result = ns_terminate(ConstString("unknown command ") +
                                              msg + "\n");
                    }
                    // This change is just to make Microsoft Telnet happy
                    ConstString tmp;
                    for (unsigned int i=0; i<result.length(); i++) {
                        if (result[i]=='\n') {
                            tmp += '\r';
                        }
                        tmp += result[i];
                    }
                    tmp += '\r';
                    os->appendString(tmp.c_str(),'\n');

                    YARP_DEBUG(Logger::get(),
                               ConstString("name server reply is ") + result);
                    ConstString resultSparse = result;
                    size_t end = resultSparse.find("\n*** end of message");
                    if (end!=ConstString::npos) {
                        resultSparse[end] = '\0';
                    }
                    YARP_INFO(Logger::get(),resultSparse);
                }
            } else {
                YARP_INFO(Logger::get(),
                          ConstString("Name server ignoring unknown command: ")+msg);
                ConnectionWriter *os = reader.getWriter();
                if (os!=YARP_NULLPTR) {
                    // this result is necessary for YARP1 support
                    os->appendString("???????????????????????????????????????",'\n');
                    //os->flush();
                    //os->close();
                }
            }
        }
        YTRACE("NameServer::read stop");
        return true;
    }
Exemple #23
0
bool RosNameSpace::writeToNameServer(PortWriter& cmd,
                                     PortReader& reply,
                                     const ContactStyle& style) {
    DummyConnector con0;
    cmd.write(con0.getWriter());
    Bottle in;
    in.read(con0.getReader());
    ConstString key = in.get(0).asString();
    ConstString arg1 = in.get(1).asString();

    Bottle cmd2, cache;
    if (key=="query") {
        Contact c = queryName(arg1.c_str());
        c.setName("");
        Bottle reply2;
        reply2.addString(arg1.c_str());
        reply2.addString(c.toString().c_str());
        DummyConnector con;
        reply2.write(con.getWriter());
        reply.read(con.getReader());
        return true;
    } else if (key=="list") {
        cmd2.addString("getSystemState");
        cmd2.addString("dummy_id");

        if (!NetworkBase::write(getNameServerContact(), cmd2, cache, style)) {
            fprintf(stderr, "Failed to contact ROS server\n");
            return false;
        }

        Bottle out;
        out.addVocab(Vocab::encode("many"));
        Bottle *parts = cache.get(2).asList();
        Property nodes;
        Property topics;
        Property services;
        if (parts) {
            for (int i=0; i<3; i++) {
                Bottle *part = parts->get(i).asList();
                if (!part) continue;
                for (int j=0; j<part->size(); j++) {
                    Bottle *unit = part->get(j).asList();
                    if (!unit) continue;
                    ConstString stem = unit->get(0).asString();
                    Bottle *links = unit->get(1).asList();
                    if (!links) continue;
                    if (i<2) {
                        topics.put(stem, 1);
                    } else {
                        services.put(stem, 1);
                    }
                    for (int j=0; j<links->size(); j++) {
                        nodes.put(links->get(j).asString(), 1);
                    }
                }
            }
            Property *props[3] = {&nodes, &topics, &services};
            const char *title[3] = {"node", "topic", "service"};
            for (int p=0; p<3; p++) {
                Bottle blist;
                blist.read(*props[p]);
                for (int i=0; i<blist.size(); i++) {
                    ConstString name = blist.get(i).asList()->get(0).asString();
                    Bottle& info = out.addList();
                    info.addString(title[p]);
                    info.addString(name);
                }
            }
        }
        out.write(reply);
        return true;
    } else {
        return false;
    }

}
Exemple #24
0
 virtual bool read(ConnectionReader& connection) {
     receive.read(connection);
     ct++;
     return true;
 }
Exemple #25
0
    virtual bool read(ConnectionReader& reader) {
        // called by comms code
        readBlock.wait();

        if (!reader.isValid()) {
            // termination
            stateMutex.wait();
            if (readDelegate!=NULL) {
                readResult = readDelegate->read(reader);
            }
            stateMutex.post();
            produce.post();
            readBlock.post();
            return false;
        }

        // wait for happy consumer - don't want to miss a packet
        if (!readBackground) {
            consume.wait();
        }

        if (closed) {
            YARP_DEBUG(Logger::get(),"Port::read shutting down");
            readBlock.post();
            return false;
        }

        stateMutex.wait();
        readResult = false;
        if (readDelegate!=NULL) {
            readResult = readDelegate->read(reader);
        } else {
            // read and ignore
            YARP_DEBUG(Logger::get(),"data received in Port, no reader for it");
            Bottle b;
            b.read(reader);
        }
        if (!readBackground) {
            readDelegate = NULL;
            writeDelegate = NULL;
        }
        bool result = readResult;
        stateMutex.post();
        if (!readBackground) {
            produce.post();
        }
        if (result&&willReply) {
            consume.wait();
            if (closed) {
                YARP_DEBUG(Logger::get(),"Port::read shutting down");
                readBlock.post();
                return false;
            }
            if (writeDelegate!=NULL) {
                stateMutex.wait();
                ConnectionWriter *writer = reader.getWriter();
                if (writer!=NULL) {
                    result = readResult = writeDelegate->write(*writer);
                }
                stateMutex.post();
            }
            if (dropDue) {
                reader.requestDrop();
            }
            produce.post();
        }
        readBlock.post();
        return result;
    }
Exemple #26
0
bool testSequence(char *seq, int len, const char *fmt, Bottle ref,
                  bool testWrite = true) {
    printf("\n");
    printf("================================================\n");
    printf(" READ %s\n", fmt);
    Bytes b1(seq,len);
    WireTwiddler tt;
    tt.configure(fmt);
    printf(">>> %s\n", tt.toString().c_str());
    Bottle bot;
    if (!tt.read(bot,b1)) {
        fprintf(stderr,"Read failed\n");
        return 1;
    }
    if (bot!=ref) {
        printf("%s: read %s, expected %s\n", fmt, 
               bot.toString().c_str(),
               ref.toString().c_str());
        printf("MISMATCH\n");
        exit(1);
        return false;
    }
    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;
    StreamConnectionReader reader2;
    reader2.reset(twiddled_input,NULL,route,0,false);
    bot.clear();
    twiddled_input.reset();
    bot.read(reader2);

    if (bot!=ref) {
        printf("%s: read %s, expected %s\n", fmt, 
               bot.toString().c_str(),
               ref.toString().c_str());
        printf("MISMATCH\n");
        exit(1);
        return false;
    }
    printf("[2] %s: read %s as expected\n", fmt, bot.toString().c_str());


    StreamConnectionReader reader3;
    reader3.reset(twiddled_input,NULL,route,0,false);
    bot.clear();
    twiddled_input.reset();
    bot.read(reader3);

    if (bot!=ref) {
        printf("%s: read %s, expected %s\n", fmt, 
               bot.toString().c_str(),
               ref.toString().c_str());
        printf("MISMATCH\n");
        exit(1);
        return false;
    }
    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;
        tt.write(ref,output);
        if (output.length()!=(size_t)len) {
            fprintf(stderr,"WRITE MISMATCH, length %d, expected %d\n",
                    (int)output.length(), len);
            exit(1);
            return false;
        }
        for (size_t i=0; i<output.length(); i++) {
            if (output.get()[i] != seq[i]) {
                fprintf(stderr,"WRITE MISMATCH, at %d, have [%d:%c] expected [%d:%c]\n",
                        (int)i, output.get()[i], output.get()[i], seq[i], seq[i]);
                exit(1);
                return false;
            }
        }
        printf("[4] %s: wrote %s as expected\n", fmt, bot.toString().c_str());
    }

    return true;
}
Exemple #27
0
 virtual bool read(ConnectionReader& con) {
     Bottle bot;
     bot.read(con);
     printf("Got %s\n", bot.toString().c_str());
     return true;
 }
bool HapticDeviceWrapper::read(ConnectionReader &connection)
{
    Bottle cmd;
    if (!cmd.read(connection))
        return false;
    int tag=cmd.get(0).asVocab();

    Bottle rep;
    if (device!=NULL)
    {
        LockGuard lg(mutex);

        if (tag==hapticdevice::set_transformation)
        {
            if (cmd.size()>=2)
            {
                if (Bottle *payload=cmd.get(1).asList())
                {
                    Matrix T(payload->get(0).asInt(),
                             payload->get(1).asInt());

                    if (Bottle *vals=payload->get(2).asList())
                    {
                        for (int r=0; r<T.rows(); r++)
                            for (int c=0; c<T.cols(); c++)
                                T(r,c)=vals->get(T.rows()*r+c).asDouble();

                        if (device->setTransformation(T))
                            rep.addVocab(hapticdevice::ack);
                        else
                            rep.addVocab(hapticdevice::nack);
                    }
                }
            }
        }
        else if (tag==hapticdevice::get_transformation)
        {
            Matrix T;
            if (device->getTransformation(T))
            {
                rep.addVocab(hapticdevice::ack);
                rep.addList().read(T);
            }
            else
                rep.addVocab(hapticdevice::nack);
        }
        else if (tag==hapticdevice::stop_feedback)
        {
            fdbck=0.0;
            device->stopFeedback();
            applyFdbck=false;
            rep.addVocab(hapticdevice::ack);
        }
        else if (tag==hapticdevice::is_cartesian)
        {
            bool ret;
            if (device->isCartesianForceModeEnabled(ret))
            {
                rep.addVocab(hapticdevice::ack);
                rep.addInt(ret?1:0);
            }
            else
                rep.addVocab(hapticdevice::nack);
        }
        else if (tag==hapticdevice::set_cartesian)
        {
            rep.addVocab(device->setCartesianForceMode()?
                         hapticdevice::ack:hapticdevice::nack);
        }
        else if (tag==hapticdevice::set_joint)
        {
            rep.addVocab(device->setJointTorqueMode()?
                         hapticdevice::ack:hapticdevice::nack);
        }
        else if (tag==hapticdevice::get_max)
        {
            Vector max;
            if (device->getMaxFeedback(max))
            {
                rep.addVocab(hapticdevice::ack);
                rep.addList().read(max);
            }
            else
                rep.addVocab(hapticdevice::nack);
        }
    }

    if (rep.size()==0)
        rep.addVocab(hapticdevice::nack);

    ConnectionWriter *writer=connection.getWriter();
    if (writer!=NULL)
        rep.write(*writer);

    return true;
}
Exemple #29
0
bool yarp::os::impl::HttpCarrier::reply(Protocol& proto, SizedWriter& writer) {
    DummyConnector con;
    con.setTextMode(true);
    for (size_t i=writer.headerLength(); i<writer.length(); i++) {
        con.getWriter().appendBlock(writer.data(i),writer.length(i));
    }
    Bottle b;
    b.read(con.getReader());

    ConstString mime = b.check("mime",Value("text/html")).asString();

    ConstString body;

    bool using_json = false;
    if (stream!=NULL) {
        if (stream->useJson()) {
            mime = "text/json";
            asJson(body,&b,stream->typeHint());
            using_json = true;
        }
    }

    if (b.check("web")&&!using_json) {
        body = b.find("web").toString();
    }

    if (b.check("stream")&&!using_json) {
        String header("HTTP/1.1 200 OK\r\nContent-Type: ");
        header += mime;
        header += "\r\n";
        header += "Transfer-Encoding: chunked\r\n";
        header += "\r\n";
        int N = 2*1024;
        header += NetType::toHexString(body.length()+N);
        header += "\r\n";

        Bytes b2((char*)header.c_str(),header.length());
        proto.os().write(b2);

        // chrome etc won't render until enough chars are received.
        for (int i=0; i<N; i++) {
            proto.os().write(' ');
        }

        Bytes b3((char*)body.c_str(),body.length());
        proto.os().write(b3);

        proto.os().write('\r');
        proto.os().write('\n');


        if (stream!=NULL) {
            stream->flip();
        }
        return true;
    }

    if (stream!=NULL) {
        stream->finish();
    }

    // Could check response codes, mime types here.

    if (body.length()!=0 || using_json) {
        ConstString mime = b.check("mime",Value("text/html")).asString();
        String header("HTTP/1.1 200 OK\nContent-Type: ");
        header += mime;
        header += "\n";
        header += "Access-Control-Allow-Origin: *\n";
        header += "\n";
        Bytes b2((char*)header.c_str(),header.length());
        proto.os().write(b2);

        //body = b.toString();
        Bytes b3((char*)body.c_str(),body.length());
        proto.os().write(b3);
    } else {
        writer.write(proto.os());
    }
    proto.os().flush();
    return proto.os().isOk();
}