bool XmlRpcCarrier::shouldInterpretRosMessages(ConnectionState& proto) {
// We need to set the interpretRos flag, which controls
// whether ROS-style admin messages are treated as
// admin messages or data messages in YARP.
// In the future, they should always be data messages.
// For now, they should be admin messages for all ports
// except ports tagged as corresponding to ros nodes.
bool nodelike = false;
Contactable *port = proto.getContactable();
Property opt;
if (port) {
Property *pport = port->acquireProperties(true);
if (pport) {
opt = *pport;
}
port->releaseProperties(pport);
}
if (opt.check("node_like")) {
nodelike = true;
}
Name n(proto.getRoute().getCarrierName() + "://test");
ConstString rospass = n.getCarrierModifier("ros");
interpretRos = !nodelike;
if (rospass=="1"||rospass=="on") {
interpretRos = true;
}
if (rospass=="0"||rospass=="off") {
interpretRos = false;
}
return interpretRos;
}
ConstString TcpRosCarrier::getRosType(ConnectionState& proto) {
ConstString typ = "";
ConstString rtyp = "";
if (proto.getContactable()) {
Type t = proto.getContactable()->getType();
typ = t.getName();
md5sum = t.readProperties().find("md5sum").asString();
message_definition = t.readProperties().find("message_definition").asString();
user_type = typ;
if (typ=="yarp/image") {
wire_type = "sensor_msgs/Image";
rtyp = "";
} else if (typ=="yarp/bottle") {
rtyp = proto.getContactable()->getType().getNameOnWire();
if (rtyp=="yarp/image") rtyp = "sensor_msgs/Image";
wire_type = rtyp;
} else if (typ!="") {
rtyp = typ;
wire_type = typ;
}
}
Name n(proto.getRoute().getCarrierName() + "://test");
ConstString mode = "topic";
ConstString modeValue = n.getCarrierModifier("topic");
if (modeValue=="") {
mode = "service";
modeValue = n.getCarrierModifier("service");
}
if (modeValue!="") {
ConstString package = n.getCarrierModifier("package");
if (package!="") {
rtyp = package + "/" + modeValue;
}
}
dbg_printf("USER TYPE %s\n", user_type.c_str());
dbg_printf("WIRE TYPE %s\n", wire_type.c_str());
return rtyp;
}
bool TcpRosCarrier::write(ConnectionState& proto, SizedWriter& writer) {
SizedWriter *flex_writer = &writer;
ConstString typ = "";
if (proto.getContactable()) {
typ = proto.getContactable()->getType().getName();
}
if (raw!=2) {
// At startup, we check for what kind of messages are going
// through, and prepare an appropriate byte-rejiggering if
// needed.
if (translate==TCPROS_TRANSLATE_UNKNOWN) {
dbg_printf("* TCPROS_TRANSLATE_UNKNOWN\n");
FlexImage *img = NULL;
if (typ=="yarp/image"||typ=="yarp/bottle") {
img = wi.checkForImage(writer);
}
if (img) {
translate = TCPROS_TRANSLATE_IMAGE;
ConstString frame = "/frame";
ri.init(*img,frame);
} else {
if (WireBottle::extractBlobFromBottle(writer,wt)) {
translate = TCPROS_TRANSLATE_BOTTLE_BLOB;
} else {
translate = TCPROS_TRANSLATE_INHIBIT;
}
}
}
} else {
translate = TCPROS_TRANSLATE_INHIBIT;
}
// Apply byte-rejiggering if needed.
switch (translate) {
case TCPROS_TRANSLATE_IMAGE:
{
dbg_printf("* TCPROS_TRANSLATE_IMAGE\n");
FlexImage *img = wi.checkForImage(writer);
if (img==NULL) {
fprintf(stderr, "TCPROS Expected an image, but did not get one.\n");
return false;
}
ri.update(img,seq,Time::now());
seq++;
flex_writer = &ri;
}
break;
case TCPROS_TRANSLATE_BOTTLE_BLOB:
{
dbg_printf("* TCPROS_TRANSLATE_BOTTLE_BLOB\n");
if (!WireBottle::extractBlobFromBottle(writer,wt)) {
fprintf(stderr, "TCPROS Expected a bottle blob, but did not get one.\n");
return false;
}
flex_writer = &wt;
}
break;
case TCPROS_TRANSLATE_TWIDDLER:
{
dbg_printf("* TCPROS_TRANSLATE_TWIDDLER\n");
twiddler_output.attach(writer,twiddler);
if (twiddler_output.update()) {
flex_writer = &twiddler_output;
} else {
flex_writer = NULL;
}
}
break;
case TCPROS_TRANSLATE_INHIBIT:
dbg_printf("* TCPROS_TRANSLATE_INHIBIT\n");
break;
default:
dbg_printf("* TCPROS_TRANSLATE_OTHER\n");
break;
}
if (flex_writer == NULL) {
return false;
}
int len = 0;
for (size_t i=0; i<flex_writer->length(); i++) {
len += (int)flex_writer->length(i);
}
dbg_printf("Prepping to write %d blocks (%d bytes)\n",
(int)flex_writer->length(),
len);
string header_len(4,'\0');
char *at = (char*)header_len.c_str();
RosHeader::appendInt(at,len);
Bytes b1((char*)header_len.c_str(),header_len.length());
proto.os().write(b1);
flex_writer->write(proto.os());
dbg_printf("done sending\n");
if (isService) {
if (!sender) {
if (!persistent) {
proto.os().close();
}
}
}
return proto.getStreams().isOk();
}
