C++ os::ConnectionReader类代码示例

bool Matrix::read(yarp::os::ConnectionReader& connection) {
    // auto-convert text mode interaction
    connection.convertTextMode();
    MatrixPortContentHeader header;
    bool ok = connection.expectBlock((char*)&header, sizeof(header));
    if (!ok) return false;
    int r=rows();
    int c=cols();
    if (header.listLen > 0)
    {
        if ( r != (int)(header.rows) || c!=(int)(header.cols))
        {
            resize(header.rows, header.cols);
        }

        int l=0;
        double *tmp=data();
        for(l=0;l<header.listLen;l++)
            tmp[l]=connection.expectDouble();
    }
    else
        return false;

    return true;
}

bool C_sendingBuffer::read(yarp::os::ConnectionReader& connection)
{
	connection.convertTextMode(); // if connection is text-mode, convert!
	int tag = connection.expectInt();
	if (tag!=BOTTLE_TAG_LIST+BOTTLE_TAG_BLOB+BOTTLE_TAG_INT)
		return false;
	int ct = connection.expectInt();
	if (ct!=2)
		return false;
	size_of_the_packet = connection.expectInt();
	connection.expectBlock(packet, SIZE_OF_DATA);
	return true;
}

bool PortCoreInputUnit::skipIncomingData(yarp::os::ConnectionReader& reader) {
    size_t pending = reader.getSize();
    if (pending>0) {
        while (pending>0) {
            char buf[10000];
            size_t next = (pending<sizeof(buf))?pending:sizeof(buf);
            reader.expectBlock(&buf[0],next);
            pending -= next;
        }
        return true;
    }
    return false;
}

bool eventBuffer::read(yarp::os::ConnectionReader& connection)
{
    connection.convertTextMode();   // if connection is text-mode, convert!
    int tag = connection.expectInt();
    if (tag != BOTTLE_TAG_LIST+BOTTLE_TAG_BLOB+BOTTLE_TAG_INT)
        return false;
    int ct = connection.expectInt();
    if (ct!=2)
        return false;
    size_of_the_packet = connection.expectInt();
    int ceilSizeOfPacket = (size_of_the_packet+7)/8*8; // the nearest multiple of 8 greater or equal to size_of_the_packet
    memset(packet, 0, SIZE_OF_DATA);
    connection.expectBlock(packet, ceilSizeOfPacket);
    return true;
}

bool AnalogServerHandler::read(yarp::os::ConnectionReader& connection)
{
    yarp::os::Bottle in;
    yarp::os::Bottle out;
    bool ok=in.read(connection);
    if (!ok) return false;

    // parse in, prepare out
    int code = in.get(0).asVocab();
    bool ret=false;
    if (code==VOCAB_IANALOG)
    {
        ret=_handleIAnalog(in, out);
    }

    if (!ret)
    {
        out.clear();
        out.addVocab(VOCAB_FAILED);
    }

    yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
    if (returnToSender!=NULL) {
        out.write(*returnToSender);
    }
    return true;
}

bool eventBottle::read(yarp::os::ConnectionReader& connection) {
    connection.convertTextMode();   // if connection is text-mode, convert!
    int tag = connection.expectInt();
    if (tag != BOTTLE_TAG_LIST + BOTTLE_TAG_BLOB + BOTTLE_TAG_INT) {
       return false;
    }
    int ct = connection.expectInt();
    if (ct!=2) {
        return false;
    }
    bytes_of_the_packet = connection.expectInt();
    size_of_the_packet = bytes_of_the_packet / wordDimension;      // number of 32 bit word times 4bytes
    connection.expectBlock(packetPointer,bytes_of_the_packet);

    //printf(" eventBottle::read:size_of_the_packet %d bytes_of_the_packet %d \n", size_of_the_packet, bytes_of_the_packet);


    // ---------------------------------------------------------------------------------------------------------
    // ------------------------ deserialisation of the bottle -------------------------------------
    //printf("bytes of the packet %d \n",bytes_of_the_packet );
    int word;
    char* i_data  = packetPointer;
    packet->clear(); // clear the bottle before any other adding

    unsigned char tmpChar;
    for(int i = 0 ; i < bytes_of_the_packet;) {
        word = 0;
        for (int j = 0 ; j < 4 ; j++){
            tmpChar = (unsigned char) *i_data;
            //printf("%02x ", *i_data );
            int value =  tmpChar << (8 * j);
            word = word | value;
            i_data++;
            i++;
        }
        packet->addInt(word);
        //printf("= %08x \n", word);

    }

    //----------------------------------------------------------------------------------------------------------

    //packet->fromBinary(packetPointer,bytes_of_the_packet);
    size_of_the_bottle = packet->size();
    return true;
}

bool Vector::read(yarp::os::ConnectionReader& connection) {
    // auto-convert text mode interaction
    connection.convertTextMode();
    VectorPortContentHeader header;
    bool ok = connection.expectBlock((char*)&header, sizeof(header));
    if (!ok) return false;
    if (header.listLen > 0 && 
        header.listTag == BOTTLE_TAG_LIST + BOTTLE_TAG_DOUBLE) {
        if (size() != (size_t)(header.listLen))
            resize(header.listLen);
        
        int k=0;
        for (k=0;k<header.listLen;k++)
            (*this)[k]=connection.expectDouble();
    } else {
        return false;
    }

    return !connection.isError();
}

bool Quaternion::read(yarp::os::ConnectionReader& connection)
{
    // auto-convert text mode interaction
    connection.convertTextMode();
    QuaternionPortContentHeader header;
    bool ok = connection.expectBlock((char*)&header, sizeof(header));
    if (!ok) return false;

    if (header.listLen == 4 &&  header.listTag == (BOTTLE_TAG_LIST | BOTTLE_TAG_DOUBLE))
    {
        this->internal_data[0] = connection.expectDouble();
        this->internal_data[1] = connection.expectDouble();
        this->internal_data[2] = connection.expectDouble();
        this->internal_data[3] = connection.expectDouble();
    }
    else
    {
        return false;
    }

    return !connection.isError();
}

bool BatteryWrapper::read(yarp::os::ConnectionReader& connection)
{
    yarp::os::Bottle in;
    yarp::os::Bottle out;
    bool ok = in.read(connection);
    if (!ok) return false;

    // parse in, prepare out
    int code = in.get(0).asVocab();
    bool ret = false;
    if (code == VOCAB_IBATTERY)
    {
        int cmd = in.get(1).asVocab();
        if (cmd == VOCAB_BATTERY_INFO)
        {
            if (battery_p)
            {
                yarp::os::ConstString info;
                battery_p->getBatteryInfo(info);
                out.addVocab(VOCAB_IS);
                out.addVocab(cmd);
                out.addString(info);
                ret = true;
            }
        }
        else
        {
            yError("Invalid vocab received in BatteryWrapper");
        }
    }
    else
    {
        yError("Invalid vocab received in BatteryWrapper");
    }

    if (!ret)
    {
        out.clear();
        out.addVocab(VOCAB_FAILED);
    }

    yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
    if (returnToSender != NULL) {
        out.write(*returnToSender);
    }
    return true;
}

bool TaskYarpInterface::RpcMessageCallback::read(yarp::os::ConnectionReader& connection)
{
    yarp::os::Bottle input;
    yarp::os::Bottle reply;

    if (!input.read(connection)){
        return false;
    }
    else{
        tmBase.parseIncomingMessage(input, reply);
        yarp::os::ConnectionWriter* returnToSender = connection.getWriter();
        if (returnToSender!=NULL) {
            if (!reply.write(*returnToSender)) {
                OCRA_ERROR("Reply was not successfully written");
                return false;
            }
        }
        return true;
    }
}

bool RGBDSensorWrapper::read(yarp::os::ConnectionReader& connection)
{
    yarp::os::Bottle in;
    yarp::os::Bottle out;
    bool ok = in.read(connection);
    if (!ok) return false;

    // parse in, prepare out
//     int action = in.get(0).asVocab();
//     int inter  = in.get(1).asVocab();
    bool ret = false;

    if (!ret)
    {
        out.clear();
        out.addVocab(VOCAB_FAILED);
    }

    yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
    if (returnToSender != NULL) {
        out.write(*returnToSender);
    }
    return true;
}

bool TeoXRpcResponder::read(yarp::os::ConnectionReader& connection) {
    yarp::os::Bottle in, out;
    in.read(connection);
    printf("[xRpcResponder] Got %s\n", in.toString().c_str());
    out.clear();
    yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
    if (returnToSender==NULL) return false;
    if ((in.get(0).asString() == "help")||(in.get(0).asVocab() == VOCAB_HELP))  // help //
    {
        out.addString("Available commands: [help] [load] [stat]");
        return out.write(*returnToSender);
    }
    else if ((in.get(0).asString() == "load")||(in.get(0).asVocab() == VOCAB_LOAD))  // load //
    {
        if ( in.size() != 2 )
        {
            CD_ERROR("in.size() != 2\n");
            out.addVocab(VOCAB_FAILED);
            return out.write(*returnToSender);
        }
        if( ! cartesianRateThread->load( in.get(1).asString() ) )
        {
            CD_ERROR("cartesianRateThread->load failed\n");
            out.addVocab(VOCAB_FAILED);
            return out.write(*returnToSender);
        }
        out.addVocab(VOCAB_OK);
        return out.write(*returnToSender);
    }
    else if ((in.get(0).asString() == "stat")||(in.get(0).asVocab() == VOCAB_STAT))  // stat //
    {
        std::vector<double> stat;
        if( ! cartesianRateThread->stat(stat) )
            out.addVocab(VOCAB_FAILED);
        else
            for(int i=0;i<stat.size();i++)
                out.addDouble(stat[i]);
        return out.write(*returnToSender);
    }
    else if  ((in.get(0).asString() == "inv")||(in.get(0).asVocab() == VOCAB_INV))  // inv //
    {
        std::vector<double> xd, q;
        for(int i=1;i<in.size();i++)
            xd.push_back(in.get(i).asDouble());
        if( ! cartesianRateThread->inv(xd,q) )
            out.addVocab(VOCAB_FAILED);
        else
            for(int i=0;i<q.size();i++)
                out.addDouble(q[i]);
        return out.write(*returnToSender);
    }
    else if  ((in.get(0).asString() == "movj")||(in.get(0).asVocab() == VOCAB_MOVJ))  // movj //
    {
        std::vector<double> xd, q;
        for(int i=1;i<in.size();i++)
            xd.push_back(in.get(i).asDouble());
        if( ! cartesianRateThread->movj(xd) )
            out.addVocab(VOCAB_FAILED);
        else
        {
            if(in.check("wait"))
            {
                CD_SUCCESS("Waiting\n");
                bool done = false;
                while(!done) {
                    cartesianRateThread->checkMotionDone(&done);
                    printf(".");
                    fflush(stdout);
                    yarp::os::Time::delay(0.5);
                }
                printf("\n");
            }
            out.addVocab(VOCAB_OK);
        }
        return out.write(*returnToSender);
    }
    else
    {
        fprintf(stderr,"[xRpcResponder] fail: Unknown command (use 'help' if needed).\n");
        out.addVocab(VOCAB_FAILED);
        return out.write(*returnToSender);
    }
}

bool Image::read(yarp::os::ConnectionReader& connection) {

    // auto-convert text mode interaction
    connection.convertTextMode();
    
    ImageNetworkHeader header;
    
    bool ok = connection.expectBlock((char*)&header,sizeof(header));
    if (!ok) return false;
        
    imgPixelCode = header.id;
    
    int q = getQuantum();
    if (q==0) {
        //q = YARP_IMAGE_ALIGN;
        setQuantum(header.quantum);
        q = getQuantum();
    }
    if (q!=header.quantum) {
        if ((header.depth*header.width)%header.quantum==0 &&
            (header.depth*header.width)%q==0) {
            header.quantum = q;
        }
    }
        
    if (getPixelCode()!=header.id||q!=header.quantum) {
        // we're trying to read an incompatible image type
        // rather than just fail, we'll read it (inefficiently)
        FlexImage flex;
        flex.setPixelCode(header.id);
        flex.setQuantum(header.quantum);
        flex.resize(header.width,header.height);
        if (header.width!=0&&header.height!=0) {
            unsigned char *mem = flex.getRawImage();
            yAssert(mem!=NULL);
            if (flex.getRawImageSize()!=header.imgSize) {
                printf("There is a problem reading an image\n");
                printf("incoming: width %d, height %d, code %d, quantum %d, size %d\n",
                       (int)header.width, (int)header.height, 
                       (int)header.id,
                       (int)header.quantum, (int)header.imgSize);
                printf("my space: width %d, height %d, code %d, quantum %d, size %d\n",
                       flex.width(), flex.height(), flex.getPixelCode(),
                       flex.getQuantum(), 
                       flex.getRawImageSize());
            }
            yAssert(flex.getRawImageSize()==header.imgSize);
            ok = connection.expectBlock((char *)flex.getRawImage(),
                                        flex.getRawImageSize());
            if (!ok) return false;
        }
        copy(flex);
    } else {
        yAssert(getPixelCode()==header.id);
        resize(header.width,header.height);
        unsigned char *mem = getRawImage();
        if (header.width!=0&&header.height!=0) {
            yAssert(mem!=NULL);
            if (getRawImageSize()!=header.imgSize) {
                printf("There is a problem reading an image\n");
                printf("incoming: width %d, height %d, code %d, quantum %d, size %d\n",
                       (int)header.width, (int)header.height, 
                       (int)header.id,
                       (int)header.quantum, (int)header.imgSize);
                printf("my space: width %d, height %d, code %d, quantum %d, size %d\n",
                       width(), height(), getPixelCode(), getQuantum(), getRawImageSize());
            }
            yAssert(getRawImageSize()==header.imgSize);
            ok = connection.expectBlock((char *)getRawImage(),
                                        getRawImageSize());
            if (!ok) return false;
        }
    }
    
    return !connection.isError();
}

bool MapGrid2D::read(yarp::os::ConnectionReader& connection)
{
    // auto-convert text mode interaction
    connection.convertTextMode();

    connection.expectInt();
    connection.expectInt();

    connection.expectInt();
    m_width = connection.expectInt();
    connection.expectInt();
    m_height = connection.expectInt();
    connection.expectInt();
    m_origin.x = connection.expectDouble();
    connection.expectInt();
    m_origin.y = connection.expectDouble();
    connection.expectInt();
    m_origin.theta = connection.expectDouble();
    connection.expectInt();
    m_resolution = connection.expectDouble();
    connection.expectInt();
    int siz = connection.expectInt();
    char buff[255]; memset(buff, 0, 255);
    connection.expectBlock((char*)buff, siz);
    m_map_name = buff;
    m_map_occupancy.resize(m_width, m_height);
    m_map_flags.resize(m_width, m_height);
    bool ok = true;
    unsigned char *mem = nullptr;
    int            memsize = 0;
    connection.expectInt();
    memsize = connection.expectInt();
    if (memsize != m_map_occupancy.getRawImageSize()) { return false; }
    mem = m_map_occupancy.getRawImage();
    ok &= connection.expectBlock((char*)mem, memsize);
    connection.expectInt();
    memsize = connection.expectInt();
    if (memsize != m_map_flags.getRawImageSize()) { return false; }
    mem = m_map_flags.getRawImage();
    ok &= connection.expectBlock((char*)mem, memsize);
    if (!ok) return false;

    return !connection.isError();
        return true;
}

bool Rangefinder2DWrapper::read(yarp::os::ConnectionReader& connection)
{
    yarp::os::Bottle in;
    yarp::os::Bottle out;
    bool ok = in.read(connection);
    if (!ok) return false;

    // parse in, prepare out
    int action = in.get(0).asVocab();
    int inter  = in.get(1).asVocab();
    bool ret = false;

    if (inter == VOCAB_ILASER2D)
    {
        if (action == VOCAB_GET)
        {
            int cmd = in.get(2).asVocab();
            if (cmd == VOCAB_DEVICE_INFO)
            {
                if (sens_p)
                {
                    yarp::os::ConstString info;
                    if (sens_p->getDeviceInfo(info))
                    {
                        out.addVocab(VOCAB_IS);
                        out.addVocab(cmd);
                        out.addString(info);
                        ret = true;
                    }
                    else
                    {
                        ret = false;
                    }
                }
            }
            else if (cmd == VOCAB_LASER_DISTANCE_RANGE)
            {
                if (sens_p)
                {
                    double max = 0;
                    double min = 0;
                    if (sens_p->getDistanceRange(min, max))
                    {
                        out.addVocab(VOCAB_IS);
                        out.addVocab(cmd);
                        out.addDouble(min);
                        out.addDouble(max);
                        ret = true;
                    }
                    else
                    {
                        ret = false;
                    }
                }
            }
            else if (cmd == VOCAB_LASER_ANGULAR_RANGE)
            {
                if (sens_p)
                {
                    double max = 0;
                    double min = 0;
                    if (sens_p->getScanLimits(min, max))
                    {
                        out.addVocab(VOCAB_IS);
                        out.addVocab(cmd);
                        out.addDouble(min);
                        out.addDouble(max);
                        ret = true;
                    }
                    else
                    {
                        ret = false;
                    }
                }
            }
            else if (cmd == VOCAB_LASER_ANGULAR_STEP)
            {
                if (sens_p)
                {
                    double step = 0;
                    if (sens_p->getHorizontalResolution(step))
                    {
                        out.addVocab(VOCAB_IS);
                        out.addVocab(cmd);
                        out.addDouble(step);
                        ret = true;
                    }
                    else
                    {
                        ret = false;
                    }
                }
            }
            else if (cmd == VOCAB_LASER_SCAN_RATE)
            {
                if (sens_p)
                {
                    double rate = 0;
                    if (sens_p->getScanRate(rate))
                    {
//.........这里部分代码省略.........