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();
}
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 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))
{
out.addVocab(VOCAB_IS);
out.addVocab(cmd);
out.addDouble(rate);
ret = true;
}
else
{
ret = false;
}
}
}
else
{
yError("Invalid command received in Rangefinder2DWrapper");
}
}
else if (action == VOCAB_SET)
{
int cmd = in.get(2).asVocab();
if (cmd == VOCAB_LASER_DISTANCE_RANGE)
{
if (sens_p)
{
double min = in.get(3).asInt();
double max = in.get(4).asInt();
sens_p->setDistanceRange(min, max);
ret = true;
}
}
else if (cmd == VOCAB_LASER_ANGULAR_RANGE)
{
if (sens_p)
{
double min = in.get(3).asInt();
double max = in.get(4).asInt();
sens_p->setScanLimits(min, max);
ret = true;
}
}
else if (cmd == VOCAB_LASER_SCAN_RATE)
{
if (sens_p)
{
double rate = in.get(3).asInt();
sens_p->setScanRate(rate);
ret = true;
}
}
else if (cmd == VOCAB_LASER_ANGULAR_STEP)
{
if (sens_p)
{
double step = in.get(3).asDouble();
sens_p->setHorizontalResolution(step);
ret = true;
}
}
else
{
yError("Invalid command received in Rangefinder2DWrapper");
}
}
else
{
yError("Invalid action received in Rangefinder2DWrapper");
}
}
else
{
yError("Invalid interface vocab received in Rangefinder2DWrapper");
}
if (!ret)
{
out.clear();
out.addVocab(VOCAB_FAILED);
}
yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
if (returnToSender != NULL) {
out.write(*returnToSender);
}
return true;
}
bool yarp::dev::LocationsServer::read(yarp::os::ConnectionReader& connection)
{
yarp::os::Bottle in;
yarp::os::Bottle out;
bool ret;
int code;
bool ok = in.read(connection);
if (!ok) return false;
// parse in, prepare out
if(in.get(0).isString())
{
if (in.get(0).asString() == "save" && in.get(1).isString())
{
if(save_locations(in.get(1).asString()))
{
out.addString(in.get(1).asString() + " successfully saved");
}
}
else if (in.get(0).asString() == "load" && in.get(1).isString())
{
if(load_locations(in.get(1).asString()))
{
out.addString(in.get(1).asString() + " successfully loaded");
}
}
else if(in.get(0).asString() == "list")
{
std::map<std::string, Map2DLocation>::iterator it;
for (it = m_locations.begin(); it != m_locations.end(); it++)
{
out.addString(it->first);
}
}
else if(in.get(0).asString() == "help")
{
out.addString("'save <full path filename>' to save locations on a file");
out.addString("'load <full path filename>' to load locations from a file");
out.addString("'list' to view locations stored");
}
else
{
out.addString("request not undestood, call 'help' to see a list of avaiable commands");
}
yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
if (returnToSender != NULL)
{
out.write(*returnToSender);
}
updateVizMarkers();
return true;
}
code = in.get(0).asVocab();
ret = false;
if (code == VOCAB_INAVIGATION)
{
int cmd = in.get(1).asVocab();
if (cmd == VOCAB_NAV_GET_LOCATION_LIST)
{
yarp::os::ConstString info;
out.addVocab(VOCAB_OK);
Bottle& l = out.addList();
std::map<std::string, Map2DLocation>::iterator it;
for (it = m_locations.begin(); it != m_locations.end(); it++)
{
l.addString(it->first);
}
ret = true;
}
else if (cmd == VOCAB_NAV_CLEAR)
{
m_locations.clear();
out.addVocab(VOCAB_OK);
ret = true;
}
else if (cmd == VOCAB_NAV_DELETE)
{
std::string name = in.get(2).asString();
std::map<std::string, Map2DLocation>::iterator it;
it = m_locations.find(name);
if (it != m_locations.end())
{
m_locations.erase(it);
out.addVocab(VOCAB_OK);
}
else
{
yError("User requested an invalid location name");
out.addVocab(VOCAB_ERR);
}
ret = true;
}
else if (cmd == VOCAB_NAV_GET_LOCATION)
{
std::string name = in.get(2).asString();
std::map<std::string, Map2DLocation>::iterator it;
it = m_locations.find(name);
if (it != m_locations.end())
{
out.addVocab(VOCAB_OK);
Map2DLocation loc = it->second;
out.addString(loc.map_id);
out.addDouble(loc.x);
out.addDouble(loc.y);
out.addDouble(loc.theta);
}
else
{
out.addVocab(VOCAB_ERR);
yError("User requested an invalid location name");
}
ret = true;
}
else if (cmd == VOCAB_NAV_STORE_ABS)
{
Map2DLocation location;
yarp::os::ConstString name = in.get(2).asString();
location.map_id = in.get(3).asString();
location.x = in.get(4).asDouble();
location.y = in.get(5).asDouble();
location.theta = in.get(6).asDouble();
m_locations.insert(std::pair<std::string, Map2DLocation>(name, location));
out.addVocab(VOCAB_OK);
ret = true;
}
else
{
yError("Invalid vocab received in LocationsServer");
}
}
else
{
yError("Invalid vocab received in LocationsServer");
}
if (!ret)
{
out.clear();
out.addVocab(VOCAB_FAILED);
}
yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
if (returnToSender != NULL)
{
out.write(*returnToSender);
}
updateVizMarkers();
return true;
}
bool FrameTransformServer::read(yarp::os::ConnectionReader& connection)
{
LockGuard lock(m_mutex);
yarp::os::Bottle in;
yarp::os::Bottle out;
bool ok = in.read(connection);
if (!ok) return false;
string request = in.get(0).asString();
// parse in, prepare out
int code = in.get(0).asVocab();
bool ret = false;
if (code == VOCAB_ITRANSFORM)
{
int cmd = in.get(1).asVocab();
if (cmd == VOCAB_TRANSFORM_SET)
{
if (in.size() != 12)
{
yError() << "FrameTransformServer::read() protocol error";
out.clear();
out.addVocab(VOCAB_FAILED);
}
else
{
FrameTransform t;
t.src_frame_id = in.get(2).asString();
t.dst_frame_id = in.get(3).asString();
double duration = in.get(4).asDouble();
t.translation.tX = in.get(5).asDouble();
t.translation.tY = in.get(6).asDouble();
t.translation.tZ = in.get(7).asDouble();
t.rotation.w() = in.get(8).asDouble();
t.rotation.x() = in.get(9).asDouble();
t.rotation.y() = in.get(10).asDouble();
t.rotation.z() = in.get(11).asDouble();
t.timestamp = yarp::os::Time::now();
bool static_transform;
if (duration > 0)
{
static_transform = false;
}
else
{
static_transform = true;
}
if (static_transform)
{
ret = m_yarp_static_transform_storage->set_transform(t);
}
else
{
ret = m_yarp_timed_transform_storage->set_transform(t);
}
if (ret == true)
{
out.clear();
out.addVocab(VOCAB_OK);
}
else
{
out.clear();
out.addVocab(VOCAB_FAILED);
yError() << "FrameTransformServer::read() something strange happened";
}
}
}
else if (cmd == VOCAB_TRANSFORM_DELETE_ALL)
{
m_yarp_timed_transform_storage->clear();
m_yarp_static_transform_storage->clear();
m_ros_timed_transform_storage->clear();
m_ros_static_transform_storage->clear();
out.clear();
out.addVocab(VOCAB_OK);
}
else if (cmd == VOCAB_TRANSFORM_DELETE)
{
string frame1 = in.get(2).asString();
string frame2 = in.get(3).asString();
bool ret1 = m_yarp_timed_transform_storage->delete_transform(frame1, frame2);
if (ret1 == true)
{
out.clear();
out.addVocab(VOCAB_OK);
}
else
{
bool ret2 = m_yarp_static_transform_storage->delete_transform(frame1, frame2);
if (ret2 == true)
{
out.clear();
out.addVocab(VOCAB_OK);
}
}
}
else
{
yError("Invalid vocab received in FrameTransformServer");
out.clear();
out.addVocab(VOCAB_ERR);
}
}
else if(request == "help")
{
out.addVocab(Vocab::encode("many"));
out.addString("'list': get all transforms stored");
out.addString("'delete_all': delete all transforms");
out.addString("'set_static_transform <src> <dst> <x> <y> <z> <roll> <pitch> <yaw>': create a static transform");
}
else if (request == "set_static_transform")
{
FrameTransform t;
t.src_frame_id = in.get(1).asString();
t.dst_frame_id = in.get(2).asString();
t.translation.tX = in.get(3).asDouble();
t.translation.tY = in.get(4).asDouble();
t.translation.tZ = in.get(5).asDouble();
t.rotFromRPY(in.get(6).asDouble(), in.get(7).asDouble(), in.get(8).asDouble());
t.timestamp = yarp::os::Time::now();
bool static_transform = true;
ret = m_yarp_static_transform_storage->set_transform(t);
if (ret == true)
{
yInfo() << "set_static_transform done";
out.addString("set_static_transform done");
}
else
{
yError() << "FrameTransformServer::read() something strange happened";
}
}
else if(request == "delete_all")
{
m_yarp_timed_transform_storage->clear();
m_yarp_static_transform_storage->clear();
m_ros_timed_transform_storage->clear();
m_ros_static_transform_storage->clear();
yInfo() << "delete_all done";
out.addString("delete_all done");
}
else if (request == "list")
{
out.addVocab(Vocab::encode("many"));
list_response(out);
}
else
{
yError("Invalid vocab received in FrameTransformServer");
out.clear();
out.addVocab(VOCAB_ERR);
}
yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
if (returnToSender != NULL)
{
out.write(*returnToSender);
}
else
{
yError() << "FrameTransformServer: invalid return to sender";
}
return true;
}
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 SystemInfoSerializer::read(yarp::os::ConnectionReader& connection)
{
// reading memory
memory.totalSpace = connection.expectInt();
memory.freeSpace = connection.expectInt();
// reading storage
storage.totalSpace = connection.expectInt();
storage.freeSpace = connection.expectInt();
// reading network
//network.mac = connection.expectText();
//network.ip4 = connection.expectText();
//network.ip6 = connection.expectText();
// reading processor
processor.architecture = connection.expectText();
processor.model = connection.expectText();
processor.vendor = connection.expectText();
processor.family = connection.expectInt();
processor.modelNumber = connection.expectInt();
processor.cores = connection.expectInt();
processor.siblings = connection.expectInt();
processor.frequency = connection.expectDouble();
// reading load
load.cpuLoad1 = connection.expectDouble();
load.cpuLoad5 = connection.expectDouble();
load.cpuLoad15 = connection.expectDouble();
load.cpuLoadInstant = connection.expectInt();
// reading platform
platform.name = connection.expectText();
platform.distribution = connection.expectText();
platform.release = connection.expectText();
platform.codename = connection.expectText();
platform.kernel = connection.expectText();
platform.environmentVars.fromString(connection.expectText());
// reading user
user.userName = connection.expectText();
user.realName = connection.expectText();
user.homeDir = connection.expectText();
user.userID = connection.expectInt();
return true;
}
//------------------------------------------------------------------------------------------------------------------------------
bool yarp::dev::FrameTransformClient::read(yarp::os::ConnectionReader& connection)
{
LockGuard lock (m_rpc_mutex);
yarp::os::Bottle in;
yarp::os::Bottle out;
bool ok = in.read(connection);
if (!ok) return false;
std::string request = in.get(0).asString();
if (request == "help")
{
out.addVocab(Vocab::encode("many"));
out.addString("'get_transform <src> <dst>: print the transform from <src> to <dst>");
out.addString("'list_frames: print all the available refence frames");
out.addString("'list_ports: print all the opened ports for tranform broadcasting");
out.addString("'publish_transform <src> <dst> <portname> <format>: opens a port to publish transform from src to dst");
out.addString("'unpublish_transform <portname>: closes a previously opened port to publish a transform");
out.addString("'unpublish_all <portname>: closes a all previously opened ports to publish a transform");
}
else if (request == "list_frames")
{
std::vector<std::string> v;
this->getAllFrameIds(v);
out.addVocab(Vocab::encode("many"));
out.addString("List of available reference frames:");
int count = 0;
for (auto vec = v.begin(); vec != v.end(); vec++)
{
count++;
std::string str = std::to_string(count) + "- " + *vec;
out.addString(str.c_str());
}
}
else if (request == "get_transform")
{
std::string src = in.get(1).asString();
std::string dst = in.get(2).asString();
out.addVocab(Vocab::encode("many"));
yarp::sig::Matrix m;
this->getTransform(src, dst, m);
out.addString("Tranform from " + src + " to " + dst + " is: ");
out.addString(m.toString());
}
else if (request == "list_ports")
{
out.addVocab(Vocab::encode("many"));
if (m_array_of_ports.size()==0)
{
out.addString("No ports are currently active");
}
for (auto it = m_array_of_ports.begin(); it != m_array_of_ports.end(); it++)
{
if (*it)
{
std::string s = (*it)->port.getName() + " "+ (*it)->transform_src + " -> " + (*it)->transform_dst;
out.addString(s);
}
}
}
else if (request == "publish_transform")
{
out.addVocab(Vocab::encode("many"));
std::string src = in.get(1).asString();
std::string dst = in.get(2).asString();
std::string port_name = in.get(3).asString();
std::string format = "matrix";
if (in.size() > 4)
{format= in.get(4).asString();}
if (port_name[0]=='/') port_name.erase(port_name.begin());
std::string full_port_name = m_local_name + "/" + port_name;
bool ret = true;
for (auto it = m_array_of_ports.begin(); it != m_array_of_ports.end(); it++)
{
if ((*it) && (*it)->port.getName() == full_port_name)
{
ret = false;
break;
}
}
if (this->frameExists(src)==false)
{
out.addString("Requested src frame " + src + " does not exists.");
yWarning("Requested src frame %s does not exists.", src.c_str());
}
if (this->frameExists(dst)==false)
{
out.addString("Requested dst frame " + dst + " does not exists.");
yWarning("Requested fst frame %s does not exists.", dst.c_str());
}
if (ret == true)
{
broadcast_port_t* b = new broadcast_port_t;
b->transform_src = src;
b->transform_dst = dst;
b->format = format;
bool pret = b->port.open(full_port_name);
if (pret)
{
out.addString("Operation complete. Port " + full_port_name + " opened.");
m_array_of_ports.push_back(b);
if (m_array_of_ports.size()==1) this->start();
}
else
{
delete b;
out.addString("Operation failed. Unable to open port " + full_port_name + ".");
}
}
else
{
out.addString("unable to perform operation");
}
}
else if (request == "unpublish_all")
{
for (auto it = m_array_of_ports.begin(); it != m_array_of_ports.end(); it++)
{
(*it)->port.close();
delete (*it);
(*it)=nullptr;
}
m_array_of_ports.clear();
if (m_array_of_ports.size()==0) this->askToStop();
out.addString("Operation complete");
}
else if (request == "unpublish_transform")
{
bool ret = false;
std::string port_name = in.get(1).asString();
if (port_name[0]=='/') port_name.erase(port_name.begin());
std::string full_port_name = m_local_name + "/" + port_name;
for (auto it = m_array_of_ports.begin(); it != m_array_of_ports.end(); it++)
{
if ((*it)->port.getName() == port_name)
{
(*it)->port.close();
delete (*it);
(*it)=nullptr;
m_array_of_ports.erase(it);
ret = true;
break;
}
}
if (ret)
{
out.addString("Port " + full_port_name + " has been closed.");
}
else
{
out.addString("Port " + full_port_name + " was not found.");
}
if (m_array_of_ports.size()==0) this->askToStop();
}
else
{
yError("Invalid vocab received in FrameTransformClient");
out.clear();
out.addVocab(VOCAB_ERR);
out.addString("Invalid command name");
}
yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
if (returnToSender != nullptr)
{
out.write(*returnToSender);
}
else
{
yError() << "FrameTransformClient: invalid return to sender";
}
return true;
}
