bool yarp::os::impl::UdpCarrier::expectReplyToHeader(ConnectionState& proto) {
// I am the sender
int myPort = proto.getStreams().getLocalAddress().getPort();
ConstString myName = proto.getStreams().getLocalAddress().getHost();
ConstString altName = proto.getStreams().getRemoteAddress().getHost();
int altPort = readYarpInt(proto);
if (altPort==-1) {
return false;
}
DgramTwoWayStream *stream = new DgramTwoWayStream();
yAssert(stream!=YARP_NULLPTR);
proto.takeStreams(YARP_NULLPTR); // free up port from tcp
bool ok =
stream->open(Contact(myName,myPort),Contact(altName,altPort));
if (!ok) {
delete stream;
return false;
}
proto.takeStreams(stream);
return true;
}
bool yarp::os::impl::McastCarrier::becomeMcast(ConnectionState& proto, bool sender) {
#ifndef YARP_HAS_ACE
return false;
#else
YARP_UNUSED(sender);
DgramTwoWayStream *stream = new DgramTwoWayStream();
yAssert(stream!=YARP_NULLPTR);
Contact remote = proto.getStreams().getRemoteAddress();
Contact local;
local = proto.getStreams().getLocalAddress();
bool test = true;
//(yarp::NameConfig::getEnv("YARP_MCAST_TEST")!="");
/*
if (test) {
printf(" MULTICAST is being extended; some temporary status messages added\n");
printf(" Local: %s\n", local.toString().c_str());
printf(" Remote: %s\n", remote.toString().c_str());
}
*/
proto.takeStreams(YARP_NULLPTR); // free up port from tcp
if (sender) {
/*
Multicast behavior seems a bit variable.
We assume here that if packages need to be broadcast
to targets via different network interfaces, that
we'll need to send independently on those two
interfaces. This may or may not always be the case,
the author doesn't know, so is being cautious.
*/
key = proto.getRoute().getFromName();
if (test) {
key += "/net=";
key += local.getHost();
}
YARP_DEBUG(Logger::get(),
ConstString("multicast key: ") + key);
addSender(key);
}
bool ok = true;
if (isElect()||!sender) {
if (test) {
ok = stream->join(mcastAddress,sender,local);
} else {
ok = stream->join(mcastAddress,sender);
}
}
if (!ok) {
delete stream;
return false;
}
proto.takeStreams(stream);
return true;
#endif
}
bool yarp::os::impl::HttpCarrier::respondToHeader(ConnectionState& proto) {
stream = new HttpTwoWayStream(proto.giveStreams(),
input.c_str(),
prefix.c_str(),
prop,
false);
proto.takeStreams(stream);
return true;
}
bool yarp::os::impl::LocalCarrier::becomeLocal(ConnectionState& proto, bool sender) {
LocalCarrierStream *stream = new LocalCarrierStream();
if (stream != nullptr) {
stream->attach(this, sender);
}
proto.takeStreams(stream);
//YARP_ERROR(Logger::get(), "*** don't trust local carrier yet ****");
//exit(1);
return true;
}
bool XmlRpcCarrier::respondToHeader(ConnectionState& proto) {
shouldInterpretRosMessages(proto);
sender = false;
XmlRpcStream *stream = new XmlRpcStream(proto.giveStreams(),
sender,
interpretRos);
if (stream==NULL) { return false; }
proto.takeStreams(stream);
return true;
}
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 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::UdpCarrier::respondToHeader(ConnectionState& proto) {
// I am the receiver
// issue: need a fresh port number...
DgramTwoWayStream *stream = new DgramTwoWayStream();
yAssert(stream!=YARP_NULLPTR);
Contact remote = proto.getStreams().getRemoteAddress();
bool ok = stream->open(remote);
if (!ok) {
delete stream;
return false;
}
int myPort = stream->getLocalAddress().getPort();
writeYarpInt(myPort,proto);
proto.takeStreams(stream);
return true;
}
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 H264Carrier::expectReplyToHeader(ConnectionState& proto)
{
// I'm the receiver...
cfgParams.remotePort = proto.getRoute().getToContact().getPort();
auto* stream = new H264Stream(cfgParams);
if (stream==nullptr) { return false; }
yarp::os::Contact remote = proto.getStreams().getRemoteAddress();
bool ok = stream->open(remote);
//std::cout << "Remote contact info: host=" << proto.getRoute().getToContact().getHost() << " port= " << proto.getRoute().getToContact().getPort() <<std::endl;
if (!ok)
{
delete stream;
return false;
}
stream->start();
proto.takeStreams(stream);
return true;
}
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();
}
