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(); }