void send_net_data(JOCTET *data, int len, void *client) {
dbg_printf("Send %d bytes\n", len);
ConnectionState *p = (ConnectionState *)client;
char hdr[1000];
sprintf(hdr,"\n");
const char *brk = "\n";
if (hdr[1]=='\0') {
brk = "\r\n";
}
dbg_printf("Using terminator %s\n",(hdr[1]=='\0')?"\\r\\n":"\\n");
sprintf(hdr,"Content-Type: image/jpeg%s\
Content-Length: %d%s%s", brk, len, brk, brk);
Bytes hbuf(hdr,strlen(hdr));
p->os().write(hbuf);
Bytes buf((char *)data,len);
/*
// add corruption now and then, for testing.
static int ct = 0;
ct++;
if (ct==50) {
printf("Adding corruption\n");
buf.get()[0] = 'z';
ct = 0;
}
*/
p->os().write(buf);
sprintf(hdr,"%s--boundarydonotcross%s",brk,brk);
Bytes hbuf2(hdr,strlen(hdr));
p->os().write(hbuf2);
}
bool AbstractCarrier::write(ConnectionState& proto, SizedWriter& writer) {
bool ok = sendIndex(proto,writer);
if (!ok) {
return false;
}
writer.write(proto.os());
proto.os().flush();
return proto.os().isOk();
}
bool yarp::os::impl::TextCarrier::respondToHeader(ConnectionState& proto)
{
std::string from = "Welcome ";
from += proto.getRoute().getFromName();
from += "\r\n";
yarp::os::Bytes b2((char*)from.c_str(), from.length());
proto.os().write(b2);
proto.os().flush();
return proto.os().isOk();
}
bool yarp::os::impl::TextCarrier::sendAck(ConnectionState& proto)
{
if (ackVariant) {
std::string from = "<ACK>\r\n";
Bytes b2((char*)from.c_str(), from.length());
proto.os().write(b2);
proto.os().flush();
}
return proto.os().isOk();
}
bool yarp::os::impl::TextCarrier::sendHeader(ConnectionState& proto)
{
std::string target = getSpecifierName();
yarp::os::Bytes b((char*)target.c_str(), 8);
proto.os().write(b);
std::string from = proto.getSenderSpecifier();
yarp::os::Bytes b2((char*)from.c_str(), from.length());
proto.os().write(b2);
proto.os().write('\r');
proto.os().write('\n');
proto.os().flush();
return proto.os().isOk();
}
bool MpiCarrier::sendHeader(ConnectionState& proto) {
// Send the "magic number" for this carrier
ManagedBytes header(8);
getHeader(header.bytes());
proto.os().write(header.bytes());
if (!proto.os().isOk()) return false;
// Now we can do whatever we want, as long as somehow
// we also send the name of the originating port
name = proto.getRoute().getFromName();
other = proto.getRoute().getToName();
Bytes b2((char*)name.c_str(),name.length());
proto.os().write(b2);
proto.os().write('\r');
proto.os().write('\n');
// Sender
route = name + "->" + other;
createStream(true);
if (!MpiControl) return false;
if (! MpiControl->isRunning())
return false;
comm->openPort();
char* port = comm->port_name;
char* uid = comm->unique_id;
#ifdef MPI_DEBUG
printf("[MpiCarrier @ %s] setting up MpiPort '%s'\n", route.c_str(), port);
#endif
Bytes b4(uid,strlen(uid));
proto.os().write(b4);
proto.os().write('\r');
proto.os().write('\n');
Bytes b3(port,strlen(port));
proto.os().write(b3);
proto.os().write('\r');
proto.os().write('\n');
proto.os().flush();
#ifdef MPI_DEBUG
printf("[MpiCarrier @ %s] Header sent\n", route.c_str());
#endif
return proto.os().isOk();
}
void AbstractCarrier::writeYarpInt(int n, ConnectionState& proto)
{
char buf[8];
Bytes header(&(buf[0]), sizeof(buf));
createYarpNumber(n, header);
proto.os().write(header);
}
bool TcpRosCarrier::reply(ConnectionState& proto, SizedWriter& writer) {
char twiddle[1];
twiddle[0] = 1;
Bytes twiddle_buf(twiddle,1);
proto.os().write(twiddle_buf);
return write(proto,writer);
}
bool AbstractCarrier::sendConnectionStateSpecifier(ConnectionState& proto) {
char buf[8];
Bytes header((char*)&buf[0],sizeof(buf));
OutputStream& os = proto.os();
proto.getConnection().getHeader(header);
os.write(header);
os.flush();
return os.isOk();
}
bool MjpegCarrier::sendHeader(ConnectionState& proto) {
Name n(proto.getRoute().getCarrierName() + "://test");
ConstString pathValue = n.getCarrierModifier("path");
ConstString target = "GET /?action=stream\n\n";
if (pathValue!="") {
target = "GET /";
target += pathValue;
}
target += " HTTP/1.1\n\n";
Bytes b((char*)target.c_str(),target.length());
proto.os().write(b);
return true;
}
bool AbstractCarrier::sendSenderSpecifier(ConnectionState& proto) {
NetInt32 numberSrc;
Bytes number((char*)&numberSrc,sizeof(NetInt32));
const String senderName = proto.getSenderSpecifier();
//const String& senderName = getRoute().getFromName();
NetType::netInt((int)senderName.length()+1,number);
OutputStream& os = proto.os();
os.write(number);
Bytes b((char*)senderName.c_str(),senderName.length()+1);
os.write(b);
os.flush();
return os.isOk();
}
bool MpiCarrier::expectReplyToHeader(ConnectionState& proto) {
// SWITCH TO NEW STREAM TYPE
if (!comm->accept()) {
delete stream;
return false;
}
proto.takeStreams(stream);
#ifdef MPI_DEBUG
printf("[MpiCarrier @ %s] MpiStream successfully setup \n", route.c_str() );
#endif
return proto.os().isOk();
}
bool XmlRpcCarrier::sendHeader(ConnectionState& proto) {
shouldInterpretRosMessages(proto);
ConstString target = "POST /RPC2";
Name n(proto.getRoute().getCarrierName() + "://test");
ConstString pathValue = n.getCarrierModifier("path");
if (pathValue!="") {
target = "POST /";
target += pathValue;
// on the wider web, we should provide real host names
host = NetworkBase::queryName(proto.getRoute().getToName());
}
target += " HTTP/1.1\n";
http = target;
Bytes b((char*)target.c_str(),target.length());
proto.os().write(b);
return true;
}
bool yarp::os::impl::HttpCarrier::sendHeader(ConnectionState& proto) {
ConstString target = "GET / HTTP/1.0\r\n";
ConstString path = proto.getRoute().getToName();
if (path.size()>=2) {
target = "GET " + path + " HTTP/1.0\r\n";
}
Contact host = proto.getRoute().getToContact();
if (host.getHost()!="") {
target += "Host: ";
target += host.getHost();
target += "\r\n";
}
target += "\r\n";
Bytes b((char*)target.c_str(),target.length());
proto.os().write(b);
return true;
}
bool AbstractCarrier::defaultSendIndex(ConnectionState& proto, SizedWriter& writer)
{
writeYarpInt(10, proto);
int len = (int)writer.length();
char lens[] = { (char)len, (char)1,
(char)-1, (char)-1, (char)-1, (char)-1,
(char)-1, (char)-1, (char)-1, (char)-1 };
Bytes b(lens, 10);
OutputStream& os = proto.os();
os.write(b);
NetInt32 numberSrc;
Bytes number((char*)&numberSrc, sizeof(NetInt32));
for (int i=0; i<len; i++) {
NetType::netInt((int)writer.length(i), number);
os.write(number);
}
NetType::netInt(0, number);
os.write(number);
return os.isOk();
}
bool yarp::os::impl::HttpCarrier::write(ConnectionState& proto, SizedWriter& writer) {
DummyConnector con;
con.setTextMode(true);
for (size_t i=writer.headerLength(); i<writer.length(); i++) {
con.getWriter().appendBlock(writer.data(i),writer.length(i));
}
Bottle b;
b.read(con.getReader());
ConstString body = b.find("web").toString();
if (body.length()!=0) {
ConstString header;
header += NetType::toHexString(body.length()).c_str();
header += "\r\n";
Bytes b2((char*)header.c_str(),header.length());
proto.os().write(b2);
Bytes b3((char*)body.c_str(),body.length());
proto.os().write(b3);
proto.os().write('\r');
proto.os().write('\n');
} else {
ConstString txt = b.toString() + "\r\n";
ConstString header;
header += NetType::toHexString(txt.length()).c_str();
header += "\r\n";
Bytes b2((char*)header.c_str(),header.length());
proto.os().write(b2);
Bytes b3((char*)txt.c_str(),txt.length());
proto.os().write(b3);
proto.os().write('\r');
proto.os().write('\n');
}
proto.os().flush();
return proto.os().isOk();
}
bool HumanCarrier::sendHeader(ConnectionState& proto) {
// Send the "magic number" for this carrier
ManagedBytes header(8);
getHeader(header.bytes());
proto.os().write(header.bytes());
if (!proto.os().isOk()) return false;
// Now we can do whatever we want, as long as somehow
// we also send the name of the originating port
// let's just send the port name in plain text terminated with a
// carriage-return / line-feed
ConstString from = proto.getRoute().getFromName();
Bytes b2((char*)from.c_str(),from.length());
proto.os().write(b2);
proto.os().write('\r');
proto.os().write('\n');
proto.os().flush();
return proto.os().isOk();
}
bool Carrier::reply(ConnectionState& proto, SizedWriter& writer) {
writer.write(proto.os());
return proto.os().isOk();
}
bool TcpRosCarrier::expectSenderSpecifier(ConnectionState& proto) {
proto.setRoute(proto.getRoute().addFromName("tcpros"));
dbg_printf("Trying for tcpros header\n");
ManagedBytes m(headerLen1);
Bytes mrem(m.get()+4,m.length()-4);
NetInt32 ni = headerLen2;
memcpy(m.get(),(char*)(&ni), 4);
dbg_printf("reading %d bytes\n", (int)mrem.length());
int res = proto.is().readFull(mrem);
dbg_printf("read %d bytes\n", res);
if (res!=(int)mrem.length()) {
if (res>=0) {
fprintf(stderr,"TCPROS header failure, expected %d bytes, got %d bytes\n",
(int)mrem.length(),res);
} else {
fprintf(stderr,"TCPROS connection has gone terribly wrong\n");
}
return false;
}
RosHeader header;
header.readHeader(string(m.get(),m.length()));
dbg_printf("Got header %s\n", header.toString().c_str());
ConstString rosname = "";
if (header.data.find("type")!=header.data.end()) {
rosname = header.data["type"].c_str();
}
ConstString rtyp = getRosType(proto);
if (rtyp!="") {
rosname = rtyp;
header.data["type"] = rosname;
header.data["md5sum"] = (md5sum!="")?md5sum:"*";
if (message_definition!="") {
header.data["message_definition"] = message_definition;
}
}
dbg_printf("<outgoing> Type of data is %s\n", rosname.c_str());
if (header.data.find("callerid")!=header.data.end()) {
proto.setRoute(proto.getRoute().addFromName(header.data["callerid"].c_str()));
} else {
proto.setRoute(proto.getRoute().addFromName("tcpros"));
}
// Let's just ignore everything that is sane and holy, and
// send the same header right back.
// **UPDATE** Oh, ok, let's modify the callerid. Begrudgingly.
NestedContact nc(proto.getRoute().getToName());
header.data["callerid"] = nc.getNodeName().c_str();
string header_serial = header.writeHeader();
string header_len(4,'\0');
char *at = (char*)header_len.c_str();
RosHeader::appendInt(at,header_serial.length());
dbg_printf("Writing %s -- %d bytes\n",
RosHeader::showMessage(header_len).c_str(),
(int)header_len.length());
Bytes b1((char*)header_len.c_str(),header_len.length());
proto.os().write(b1);
dbg_printf("Writing %s -- %d bytes\n",
RosHeader::showMessage(header_serial).c_str(),
(int)header_serial.length());
Bytes b2((char*)header_serial.c_str(),header_serial.length());
proto.os().write(b2);
if (header.data.find("probe")!=header.data.end()) {
dbg_printf("================PROBE===============\n");
return false;
}
if (!isService) {
isService = (header.data.find("service")!=header.data.end());
}
if (rosname!="" && (user_type != wire_type || user_type == "")) {
if (wire_type!="sensor_msgs/Image") { // currently using a custom method for images
kind = TcpRosStream::rosToKind(rosname.c_str()).c_str();
TcpRosStream::configureTwiddler(twiddler,kind.c_str(),rosname.c_str(),true,true);
translate = TCPROS_TRANSLATE_TWIDDLER;
}
} else {
rosname = "";
}
sender = isService;
processRosHeader(header);
if (isService) {
TcpRosStream *stream = new TcpRosStream(proto.giveStreams(),sender,
false,
isService,raw,rosname.c_str());
if (stream==NULL) { return false; }
proto.takeStreams(stream);
return proto.is().isOk();
}
return true;
}
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();
}
bool XmlRpcCarrier::write(ConnectionState& proto, SizedWriter& writer) {
StringOutputStream sos;
StringInputStream sis;
writer.write(sos);
sis.reset(sos.toString());
ConstString header;
if (sender) {
header = sis.readLine();
}
ConstString body = sis.readLine();
Value v;
if (header.length()>0 && header[0]=='q') {
body = "yarp.quit";
// XMLRPC does not need a quit message, this should get stripped
return false;
}
Bottle *bot = v.asList();
bot->fromString(body.c_str());
ConstString methodName;
if (sender) {
methodName = bot->get(0).toString();
*bot = bot->tail();
}
XmlRpcValue args;
if (bot->size()==1) {
toXmlRpcValue(bot->get(0),args);
} else {
toXmlRpcValue(v,args);
}
std::string req;
if (sender) {
const Contact& addr = host.isValid()?host:proto.getStreams().getRemoteAddress();
XmlRpcClient c(addr.getHost().c_str(),(addr.getPort()>0)?addr.getPort():80);
c.generateRequest(methodName.c_str(),args);
req = c.getRequest();
} else {
XmlRpcServerConnection c(0,NULL);
c.generateResponse(args.toXml());
req = c.getResponse();
}
int start = 0;
if (sender) {
if (req.length()<8) {
fprintf(stderr, "XmlRpcCarrier fail, %s:%d\n", __FILE__, __LINE__);
return false;
}
for (int i=0; i<(int)req.length(); i++) {
if (req[i] == '\n') {
start++;
break;
}
start++;
}
if (!firstRound) {
Bytes b((char*)http.c_str(),http.length());
proto.os().write(b);
}
firstRound = false;
}
Bytes b((char*)req.c_str()+start,req.length()-start);
proto.os().write(b);
return proto.os().isOk();
}
bool yarp::os::impl::HttpCarrier::reply(ConnectionState& proto, SizedWriter& writer) {
DummyConnector con;
con.setTextMode(true);
for (size_t i=writer.headerLength(); i<writer.length(); i++) {
con.getWriter().appendBlock(writer.data(i),writer.length(i));
}
Bottle b;
b.read(con.getReader());
ConstString mime = b.check("mime",Value("text/html")).asString();
ConstString body;
bool using_json = false;
if (stream != YARP_NULLPTR) {
if (stream->useJson()) {
mime = "text/json";
asJson(body,&b,stream->typeHint());
using_json = true;
}
}
if (b.check("web")&&!using_json) {
body = b.find("web").toString();
}
if (b.check("stream")&&!using_json) {
ConstString header("HTTP/1.1 200 OK\r\nContent-Type: ");
header += mime;
header += "\r\n";
header += "Transfer-Encoding: chunked\r\n";
header += "\r\n";
int N = 2*1024;
header += NetType::toHexString(body.length()+N);
header += "\r\n";
Bytes b2((char*)header.c_str(),header.length());
proto.os().write(b2);
// chrome etc won't render until enough chars are received.
for (int i=0; i<N; i++) {
proto.os().write(' ');
}
Bytes b3((char*)body.c_str(),body.length());
proto.os().write(b3);
proto.os().write('\r');
proto.os().write('\n');
if (stream != YARP_NULLPTR) {
stream->flip();
}
return true;
}
if (stream != YARP_NULLPTR) {
stream->finish();
}
// Could check response codes, mime types here.
if (body.length()!=0 || using_json) {
ConstString mime = b.check("mime",Value(using_json?"application/json":"text/html")).asString();
ConstString header("HTTP/1.1 200 OK\nContent-Type: ");
header += mime;
header += "\n";
header += "Access-Control-Allow-Origin: *\n";
header += "\n";
Bytes b2((char*)header.c_str(),header.length());
proto.os().write(b2);
//body = b.toString();
Bytes b3((char*)body.c_str(),body.length());
proto.os().write(b3);
} else {
writer.write(proto.os());
}
proto.os().flush();
return proto.os().isOk();
}
bool yarp::os::impl::HttpCarrier::expectSenderSpecifier(ConnectionState& proto) {
proto.setRoute(proto.getRoute().addFromName("web"));
ConstString remainder = proto.is().readLine();
if (!urlDone) {
for (unsigned int i=0; i<remainder.length(); i++) {
if (remainder[i]!=' ') {
url += remainder[i];
} else {
break;
}
}
}
bool done = false;
expectPost = false;
contentLength = 0;
while (!done) {
ConstString result = proto.is().readLine();
if (result == "") {
done = true;
} else {
//printf(">>> %s\n", result.c_str());
Bottle b;
b.fromString(result.c_str());
if (b.get(0).asString()=="Content-Length:") {
//printf("]]] got length %d\n", b.get(1).asInt());
contentLength = b.get(1).asInt();
}
if (b.get(0).asString()=="Content-Type:") {
//printf("]]] got type %s\n", b.get(1).asString());
if (b.get(1).asString()=="application/x-www-form-urlencoded") {
expectPost = true;
}
}
}
}
if (expectPost) {
//printf("[[[this is a post message of length %d]]]\n", contentLength);
ManagedBytes blk(contentLength+1);
Bytes start(blk.get(),contentLength);
proto.is().readFull(start);
blk.get()[contentLength] = '\0';
//printf("message: %s\n", blk.get());
input = blk.get();
} else {
//printf("message: %s\n", url.c_str());
input = url;
}
prop.fromQuery(input.c_str());
prop.put("REQUEST_URI",url.c_str());
//printf("Property %s\n",prop.toString().c_str());
Contact home = NetworkBase::getNameServerContact();
Contact me = proto.getStreams().getLocalAddress();
ConstString from = "<html><head><link href=\"http://";
from += home.getHost();
from += ":";
from += NetType::toString(home.getPort());
from += "/web/main.css\" rel=\"stylesheet\" type=\"text/css\"/></head><body bgcolor='#ffffcc'><h1>yarp port ";
from += proto.getRoute().getToName();
from += "</h1>\n";
from += "<p>(<a href=\"http://";
from += home.getHost();
from += ":";
from += NetType::toString(home.getPort());
from += "/data=list\">All ports</a>) \n";
from += "(<a href=\"http://";
from += me.getHost();
from += ":";
from += NetType::toString(me.getPort());
from += "/\">connections</a>) \n";
from += "(<a href=\"http://";
from += me.getHost();
from += ":";
from += NetType::toString(me.getPort());
from += "/data=help\">help</a>) \n";
from += "(<a href=\"http://";
from += me.getHost();
from += ":";
from += NetType::toString(me.getPort());
from += "/r\">read</a>) \n";
from += "</p>\n";
from += "<p>\n";
from += "<form method=\"post\" action=\"http://";
from += me.getHost();
from += ":";
from += NetType::toString(me.getPort());
from += "/form\">";
prefix = from;
//Bytes b2((char*)from.c_str(),from.length());
//proto.os().write(b2);
//proto.os().flush();
// Message gets finished by the stream
return proto.os().isOk();
}
bool yarp::os::impl::McastCarrier::sendHeader(ConnectionState& proto) {
// need to do more than the default
bool ok = defaultSendHeader(proto);
if (!ok) return false;
YARP_DEBUG(Logger::get(),"Adding extra mcast header");
Contact addr;
Contact alt = proto.getStreams().getLocalAddress();
ConstString altKey =
proto.getRoute().getFromName() +
"/net=" + alt.getHost();
McastCarrier *elect = getCaster().getElect(altKey);
if (elect!=YARP_NULLPTR) {
YARP_DEBUG(Logger::get(),"picking up peer mcast name");
addr = elect->mcastAddress;
mcastName = elect->mcastName;
} else {
// fetch an mcast address
Contact target("...", "mcast", "...", 0);
addr = NetworkBase::registerContact(target);
mcastName = addr.getRegName();
if (addr.isValid()) {
// mark owner of mcast address
NetworkBase::setProperty(proto.getRoute().getFromName().c_str(),
"owns",
Value(mcastName.c_str()));
}
}
int ip[] = { 224, 3, 1, 1 };
int port = 11000;
if (addr.isValid()) {
SplitString ss(addr.getHost().c_str(),'.');
if (ss.size()!=4) {
addr = Contact();
} else {
yAssert(ss.size()==4);
for (int i=0; i<4; i++) {
ip[i] = NetType::toInt(ss.get(i));
}
port = addr.getPort();
}
}
if (!addr.isValid()) {
YARP_ERROR(Logger::get(), "Name server not responding helpfully, setting mcast name arbitrarily.");
YARP_ERROR(Logger::get(), "Only a single mcast address supported in this mode.");
addr = Contact("/tmp/mcast", "mcast", "224.3.1.1", 11000);
}
ManagedBytes block(6);
for (int i=0; i<4; i++) {
((unsigned char*)block.get())[i] = (unsigned char)ip[i];
}
block.get()[5] = (char)(port%256);
block.get()[4] = (char)(port/256);
proto.os().write(block.bytes());
mcastAddress = addr;
return true;
}
bool TcpRosCarrier::sendHeader(ConnectionState& proto) {
dbg_printf("Route is %s\n", proto.getRoute().toString().c_str());
Name n(proto.getRoute().getCarrierName() + "://test");
ConstString mode = "topic";
ConstString modeValue = n.getCarrierModifier("topic");
if (modeValue=="") {
mode = "service";
modeValue = n.getCarrierModifier("service");
isService = true;
}
if (modeValue=="") {
printf("*** no topic or service specified!\n");
mode = "topic";
modeValue = "notopic";
isService = false;
}
ConstString rawValue = n.getCarrierModifier("raw");
if (rawValue=="2") {
raw = 2;
dbg_printf("ROS-native mode requested\n");
} else if (rawValue=="1") {
raw = 1;
dbg_printf("Raw mode requested\n");
} else if (rawValue=="0") {
raw = 0;
dbg_printf("Cooked mode requested\n");
}
RosHeader header;
dbg_printf("Writing to %s\n", proto.getStreams().getRemoteAddress().toString().c_str());
dbg_printf("Writing from %s\n", proto.getStreams().getLocalAddress().toString().c_str());
ConstString rtyp = getRosType(proto);
if (rtyp!="") {
header.data["type"] = rtyp.c_str();
}
header.data[mode.c_str()] = modeValue.c_str();
header.data["md5sum"] = (md5sum!="")?md5sum:"*";
if (message_definition!="") {
header.data["message_definition"] = message_definition;
}
NestedContact nc(proto.getRoute().getFromName());
header.data["callerid"] = nc.getNodeName();
header.data["persistent"] = "1";
string header_serial = header.writeHeader();
string header_len(4,'\0');
char *at = (char*)header_len.c_str();
RosHeader::appendInt(at,header_serial.length());
dbg_printf("Writing %s -- %d bytes\n",
RosHeader::showMessage(header_len).c_str(),
(int)header_len.length());
Bytes b1((char*)header_len.c_str(),header_len.length());
proto.os().write(b1);
dbg_printf("Writing %s -- %d bytes\n",
RosHeader::showMessage(header_serial).c_str(),
(int)header_serial.length());
Bytes b2((char*)header_serial.c_str(),header_serial.length());
proto.os().write(b2);
return proto.os().isOk();
}
