bool AbstractCarrier::expectSenderSpecifier(ConnectionState& proto) {
NetInt32 numberSrc;
Bytes number((char*)&numberSrc,sizeof(NetInt32));
int len = 0;
YARP_SSIZE_T r = proto.is().readFull(number);
if ((size_t)r!=number.length()) {
YARP_DEBUG(Logger::get(),"did not get sender name length");
return false;
}
len = NetType::netInt(number);
if (len>1000) len = 1000;
if (len<1) len = 1;
// expect a string -- these days null terminated, but not in YARP1
ManagedBytes b(len+1);
r = proto.is().readFull(Bytes(b.get(),len));
if ((int)r!=len) {
YARP_DEBUG(Logger::get(),"did not get sender name");
return false;
}
// add null termination for YARP1
b.get()[len] = '\0';
String s = b.get();
proto.setRoute(proto.getRoute().addFromName(s));
return true;
}
bool AbstractCarrier::expectSenderSpecifier(ConnectionState& proto)
{
NetInt32 numberSrc;
Bytes number((char*)&numberSrc, sizeof(NetInt32));
int len = 0;
YARP_SSIZE_T r = proto.is().readFull(number);
if ((size_t)r!=number.length()) {
YARP_DEBUG(Logger::get(), "did not get sender name length");
return false;
}
len = NetType::netInt(number);
if (len>1000) {
len = 1000;
}
if (len<1) {
len = 1;
}
ManagedBytes b(len+1);
r = proto.is().readFull(Bytes(b.get(), len));
if ((int)r!=len) {
YARP_DEBUG(Logger::get(), "did not get sender name");
return false;
}
ConstString s = b.get();
Route route = proto.getRoute();
route.setFromName(s);
proto.setRoute(route);
return true;
}
bool MpiCarrier::expectSenderSpecifier(ConnectionState& proto) {
// interpret everything that sendHeader wrote
name = proto.getRoute().getToName();
#ifdef MPI_DEBUG
printf("[MpiCarrier @ %s] Waiting for header\n", route.c_str());
#endif
other = proto.is().readLine();
proto.setRoute(proto.getRoute().addFromName(other));
// Receiver
route = name + "<-" + other;
createStream(false);
if (!MpiControl) return false;
if (! MpiControl->isRunning())
return false;
ConstString other_id = proto.is().readLine();
bool notLocal = comm->notLocal(other_id);
port = proto.is().readLine();
#ifdef MPI_DEBUG
printf("[MpiCarrier @ %s] Header received\n", route.c_str());
#endif
return notLocal && proto.is().isOk();
}
bool yarp::os::impl::McastCarrier::expectExtraHeader(ConnectionState& proto) {
YARP_DEBUG(Logger::get(),"Expecting extra mcast header");
ManagedBytes block(6);
YARP_SSIZE_T len = proto.is().readFull(block.bytes());
if ((size_t)len!=block.length()) {
YARP_ERROR(Logger::get(),"problem with MCAST header");
return false;
}
int ip[] = { 0, 0, 0, 0 };
int port = -1;
unsigned char *base = (unsigned char *)block.get();
ConstString add;
for (int i=0; i<4; i++) {
ip[i] = base[i];
if (i!=0) { add += "."; }
char buf[100];
ACE_OS::sprintf(buf,"%d",ip[i]);
add += buf;
}
port = 256*base[4]+base[5];
Contact addr("mcast", add, port);
YARP_DEBUG(Logger::get(),ConstString("got mcast header ") + addr.toURI());
mcastAddress = addr;
return true;
}
bool yarp::os::impl::TextCarrier::expectReplyToHeader(ConnectionState& proto)
{
if (ackVariant) {
// expect and ignore welcome line
std::string result = proto.is().readLine();
}
return true;
}
bool yarp::os::impl::TextCarrier::expectAck(ConnectionState& proto)
{
if (ackVariant) {
// expect and ignore acknowledgement
std::string result = proto.is().readLine();
}
return true;
}
bool yarp::os::impl::TextCarrier::expectSenderSpecifier(ConnectionState& proto)
{
YARP_SPRINTF0(Logger::get(), debug, "TextCarrier::expectSenderSpecifier");
Route route = proto.getRoute();
route.setFromName(proto.is().readLine());
proto.setRoute(route);
return true;
}
int AbstractCarrier::readYarpInt(ConnectionState& proto) {
char buf[8];
Bytes header((char*)&buf[0],sizeof(buf));
YARP_SSIZE_T len = proto.is().readFull(header);
if ((size_t)len!=header.length()) {
YARP_DEBUG(proto.getLog(),"data stream died");
return -1;
}
return interpretYarpNumber(header);
}
bool AbstractCarrier::defaultExpectAck(ConnectionState& proto) {
if (proto.getConnection().requireAck()) {
char buf[8];
Bytes header((char*)&buf[0],sizeof(buf));
YARP_SSIZE_T hdr = proto.is().readFull(header);
if ((size_t)hdr!=header.length()) {
YARP_DEBUG(proto.getLog(),"did not get acknowledgement header");
return false;
}
int len = interpretYarpNumber(header);
if (len<0) {
YARP_DEBUG(proto.getLog(),"acknowledgement header is bad");
return false;
}
size_t len2 = proto.is().readDiscard(len);
if ((size_t)len!=len2) {
YARP_DEBUG(proto.getLog(),"did not get an acknowledgement of the promised length");
return false;
}
}
return true;
}
bool yarp::os::impl::LocalCarrier::expectIndex(ConnectionState& proto) {
YARP_DEBUG(Logger::get(), "local recv: wait send");
sent.wait();
YARP_DEBUG(Logger::get(), "local recv: got send");
proto.setReference(ref);
received.post();
if (ref != nullptr) {
YARP_DEBUG(Logger::get(), "local recv: received");
} else {
YARP_DEBUG(Logger::get(), "local recv: shutdown");
proto.is().interrupt();
return false;
}
return true;
}
bool MpiCarrier::respondToHeader(ConnectionState& proto) {
// SWITCH TO NEW STREAM TYPE
#ifdef MPI_DEBUG
printf("[MpiCarrier @ %s] trying to connect to MpiPort '%s'\n", route.c_str(), port.c_str());
#endif
if (!comm->connect(port)) {
delete stream;
return false;
}
proto.takeStreams(stream);
#ifdef MPI_DEBUG
printf("[MpiCarrier @ %s] MpiStream successfully setup \n", route.c_str() );
#endif
return proto.is().isOk();
}
bool yarp::os::impl::HttpCarrier::expectReplyToHeader(ConnectionState& proto) {
input = "";
YARP_SSIZE_T len = 1;
while (len>0) {
char buf[2];
Bytes b((char *)&buf[0],1);
len = proto.is().read(b);
if (len>0) {
buf[len] = '\0';
input += ConstString(buf,len);
}
}
stream = new HttpTwoWayStream(proto.giveStreams(),
input.c_str(),
prefix.c_str(),
prop,
true);
proto.takeStreams(stream);
return true;
}
bool AbstractCarrier::defaultExpectIndex(ConnectionState& proto)
{
Log& log = proto.getLog();
YARP_DEBUG(Logger::get(), "expecting an index");
YARP_SPRINTF1(Logger::get(),
debug,
"ConnectionState::expectIndex for %s",
proto.getRoute().toString().c_str());
// expect index header
char buf[8];
Bytes header((char*)&buf[0], sizeof(buf));
YARP_SSIZE_T r = proto.is().readFull(header);
if ((size_t)r!=header.length()) {
YARP_DEBUG(log, "broken index");
return false;
}
int len = interpretYarpNumber(header);
if (len<0) {
YARP_DEBUG(log, "broken index - header is not a number");
return false;
}
if (len!=10) {
YARP_DEBUG(log, "broken index - header is wrong length");
return false;
}
YARP_DEBUG(Logger::get(), "index coming in happily...");
char buf2[10];
Bytes indexHeader((char*)&buf2[0], sizeof(buf2));
r = proto.is().readFull(indexHeader);
if ((size_t)r!=indexHeader.length()) {
YARP_DEBUG(log, "broken index, secondary header");
return false;
}
YARP_DEBUG(Logger::get(), "secondary header came in happily...");
int inLen = (unsigned char)(indexHeader.get()[0]);
int outLen = (unsigned char)(indexHeader.get()[1]);
// Big limit on number of blocks here! Inherited from QNX.
// should make it go away if it hurts someone.
int total = 0;
NetInt32 numberSrc;
Bytes number((char*)&numberSrc, sizeof(NetInt32));
for (int i=0; i<inLen; i++) {
YARP_SSIZE_T l = proto.is().readFull(number);
if ((size_t)l!=number.length()) {
YARP_DEBUG(log, "bad input block length");
return false;
}
int x = NetType::netInt(number);
total += x;
}
for (int i2=0; i2<outLen; i2++) {
YARP_SSIZE_T l = proto.is().readFull(number);
if ((size_t)l!=number.length()) {
YARP_DEBUG(log, "bad output block length");
return false;
}
int x = NetType::netInt(number);
total += x;
}
proto.setRemainingLength(total);
YARP_SPRINTF1(Logger::get(),
debug,
"Total message length: %d",
total);
return true;
}
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 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::expectReplyToHeader(ConnectionState& proto) {
RosHeader header;
char mlen[4];
Bytes mlen_buf(mlen,4);
int res = proto.is().readFull(mlen_buf);
if (res<4) {
printf("Fail %s %d\n", __FILE__, __LINE__);
return false;
}
int len = NetType::netInt(mlen_buf);
dbg_printf("Len %d\n", len);
if (len>10000) {
printf("not ready for serious messages\n");
return false;
}
ManagedBytes m(len);
res = proto.is().readFull(m.bytes());
if (res!=len) {
printf("Fail %s %d\n", __FILE__, __LINE__);
return false;
}
header.readHeader(string(m.get(),m.length()));
dbg_printf("Message header: %s\n", header.toString().c_str());
ConstString rosname = "";
if (header.data.find("type")!=header.data.end()) {
rosname = header.data["type"].c_str();
}
dbg_printf("<incoming> Type of data is [%s]s\n", rosname.c_str());
if (header.data.find("callerid")!=header.data.end()) {
string name = header.data["callerid"];
dbg_printf("<incoming> callerid is %s\n", name.c_str());
dbg_printf("Route was %s\n", proto.getRoute().toString().c_str());
proto.setRoute(proto.getRoute().addToName(name.c_str()));
dbg_printf("Route is now %s\n", proto.getRoute().toString().c_str());
}
if (!isService) {
isService = (header.data.find("request_type")!=header.data.end());
}
if (rosname!="" && (user_type != wire_type || user_type == "")) {
kind = TcpRosStream::rosToKind(rosname.c_str()).c_str();
TcpRosStream::configureTwiddler(twiddler,kind.c_str(),rosname.c_str(),false,false);
translate = TCPROS_TRANSLATE_TWIDDLER;
} else {
rosname = "";
}
dbg_printf("tcpros %s mode\n", isService?"service":"topic");
// we may be a pull stream
sender = isService;
processRosHeader(header);
TcpRosStream *stream = new TcpRosStream(proto.giveStreams(),sender,
sender,
isService,raw,rosname.c_str());
if (stream==NULL) { return false; }
dbg_printf("Getting ready to hand off streams...\n");
proto.takeStreams(stream);
return proto.is().isOk();
}
