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 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 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 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 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();
}
YARP_END_PACK
bool VectorBase::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_t)getListSize() != (size_t)(header.listLen))
resize(header.listLen);
const char *ptr = getMemoryBlock();
yAssert(ptr != 0);
int elemSize=getElementSize();
ok = connection.expectBlock(ptr, elemSize*header.listLen);
if (!ok) return false;
} else {
return false;
}
return !connection.isError();
}
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;
//first check that the received image size is reasonable
if (header.width == 0 || header.height == 0)
{
// I maintain the prevous logic, although we should probably return false
return !connection.isError();
}
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;
}
}
// handle easy case, received and current image are compatible, no conversion needed
if (getPixelCode() == header.id && q == header.quantum)
{
return readFromConnection(*this, header, connection);
}
// image is bayer 8 bits, current image is MONO, copy as is (keep raw format)
if (getPixelCode() == VOCAB_PIXEL_MONO && isBayer8(header.id))
{
return readFromConnection(*this, header, connection);
}
// image is bayer 16 bits, current image is MONO16, copy as is (keep raw format)
if (getPixelCode() == VOCAB_PIXEL_MONO16 && isBayer16(header.id))
{
return readFromConnection(*this, header, connection);
}
////////////////////
// Received and current images are binary incompatible do our best to convert
//
// handle here all bayer encoding 8 bits
if (isBayer8(header.id))
{
FlexImage flex;
flex.setPixelCode(VOCAB_PIXEL_MONO);
flex.setQuantum(header.quantum);
bool ok = readFromConnection(flex, header, connection);
if (!ok)
return false;
if (getPixelCode() == VOCAB_PIXEL_BGR && header.id==VOCAB_PIXEL_ENCODING_BAYER_GRBG8)
return deBayer_GRBG8_TO_BGR(flex, *this, 3);
else if (getPixelCode() == VOCAB_PIXEL_BGRA && header.id == VOCAB_PIXEL_ENCODING_BAYER_GRBG8)
return deBayer_GRBG8_TO_BGR(flex, *this, 4);
if (getPixelCode() == VOCAB_PIXEL_RGB && header.id==VOCAB_PIXEL_ENCODING_BAYER_GRBG8)
return deBayer_GRBG8_TO_RGB(flex, *this, 3);
if (getPixelCode() == VOCAB_PIXEL_RGBA && header.id == VOCAB_PIXEL_ENCODING_BAYER_GRBG8)
return deBayer_GRBG8_TO_RGB(flex, *this, 4);
else
{
YARP_FIXME_NOTIMPLEMENTED("Conversion from bayer encoding not yet implemented\n");
return false;
}
}
// handle here all bayer encodings 16 bits
if (isBayer16(header.id))
{
// as bayer16 seems unlikely we defer implementation for later
YARP_FIXME_NOTIMPLEMENTED("Conversion from bayer encoding 16 bits not yet implemented\n");
return false;
}
// Received image has valid YARP pixels and can be converted using Image primitives
// prepare a FlexImage, set it to be compatible with the received image
// read new image into FlexImage then copy from it.
FlexImage flex;
flex.setPixelCode(header.id);
flex.setQuantum(header.quantum);
ok = readFromConnection(flex, header, connection);
if (ok)
copy(flex);
return ok;
}
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();
}