bool TcpRosCarrier::expectReplyToHeader(Protocol& proto) {
RosHeader header;
char mlen[4];
Bytes mlen_buf(mlen,4);
int res = NetType::readFull(proto.is(),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 = NetType::readFull(proto.is(),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());
}
#ifdef FORCE_ROS_NATIVE
rosname = ""; // forget
#endif
isService = (header.data.find("request_type")!=header.data.end());
dbg_printf("tcpros %s mode\n", isService?"service":"topic");
// we may be a pull stream
sender = isService;
TcpRosStream *stream = new TcpRosStream(proto.giveStreams(),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();
}
bool TcpRosCarrier::sendHeader(Protocol& proto) {
dbg_printf("Route is %s\n", proto.getRoute().toString().c_str());
Name n(proto.getRoute().getCarrierName() + "://test");
String mode = "topic";
String 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;
}
String rawValue = n.getCarrierModifier("raw");
#ifdef FORCE_ROS_NATIVE
raw = 2;
#endif
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());
//header.data["type"] = "std_msgs/String";
header.data[mode.c_str()] = modeValue.c_str();
header.data["md5sum"] = "*";
header.data["callerid"] = proto.getRoute().getFromName().c_str();
//"/not/valid/at/all/i/am/afraid/old/chum";
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();
}
bool TcpRosCarrier::expectSenderSpecifier(Protocol& 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 = NetType::readFull(proto.is(),mrem);
dbg_printf("read %d bytes\n", res);
if (res!=(int)mrem.length()) {
printf("Fail %s %d\n", __FILE__, __LINE__);
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();
}
dbg_printf("<outgoing> Type of data is %s\n", rosname.c_str());
kind = TcpRosStream::rosToKind(rosname.c_str()).c_str();
dbg_printf("Loose translation [%s]\n", kind.c_str());
#ifdef FORCE_ROS_NATIVE
kind = "";
#endif
if (kind!="") {
twiddler.configure(kind.c_str());
translate = TCPROS_TRANSLATE_TWIDDLER;
}
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.
// Oh, ok, let's modify the callerid. Begrudgingly.
header.data["callerid"] = proto.getRoute().getToName().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);
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();
}
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();
}
