bool configure(ResourceFinder &rf)
{
Property config;
config.fromConfigFile(rf.getContext() + "config.ini");
Bottle &bGeneral = config.findGroup("host_computer");
ip_address = bGeneral.find("ip_address").asString().c_str();
cout << "Host computer ip address: " << ip_address << endl;
gtw_contactTactileLeftInputPort = gtw_contactTactileLeftInputPort.addName("/gtw/telepresence/tactile/left:i");
gtw_contactTactileLeftInputPort = gtw_contactTactileLeftInputPort.addCarrier("tcp");
gtw_contactTactileLeftInputPort = gtw_contactTactileLeftInputPort.addHost(ip_address);
gtw_contactTactileLeftInputPort = gtw_contactTactileLeftInputPort.addPort(80005);
gtw_contactTactileRightInputPort = gtw_contactTactileRightInputPort.addName("/gtw/telepresence/tactile/right:i");
gtw_contactTactileRightInputPort = gtw_contactTactileRightInputPort.addCarrier("tcp");
gtw_contactTactileRightInputPort = gtw_contactTactileRightInputPort.addHost(ip_address);
gtw_contactTactileRightInputPort = gtw_contactTactileRightInputPort.addPort(80006);
gtw_contactTactileLeftOutputPort = gtw_contactTactileLeftOutputPort.addName("/gtw/telepresence/tactile/left:o");
gtw_contactTactileLeftOutputPort = gtw_contactTactileLeftOutputPort.addCarrier("tcp");
gtw_contactTactileLeftOutputPort = gtw_contactTactileLeftOutputPort.addHost(ip_address);
gtw_contactTactileLeftOutputPort = gtw_contactTactileLeftOutputPort.addPort(80007);
gtw_contactTactileRightOutputPort = gtw_contactTactileRightOutputPort.addName("/gtw/telepresence/tactile/right:o");
gtw_contactTactileRightOutputPort = gtw_contactTactileRightOutputPort.addCarrier("tcp");
gtw_contactTactileRightOutputPort = gtw_contactTactileRightOutputPort.addHost(ip_address);
gtw_contactTactileRightOutputPort = gtw_contactTactileRightOutputPort.addPort(80008);
Network::registerContact(gtw_contactTactileLeftInputPort);
Network::registerContact(gtw_contactTactileRightInputPort);
Network::registerContact(gtw_contactTactileLeftOutputPort);
Network::registerContact(gtw_contactTactileRightOutputPort);
gtw_tactileLeftInputPort.open(gtw_contactTactileLeftInputPort, true); // give the port a name
gtw_tactileRightInputPort.open(gtw_contactTactileRightInputPort, true); // give the port a name
gtw_tactileLeftOutputPort.open(gtw_contactTactileLeftOutputPort, true); // give the port a name
gtw_tactileRightOutputPort.open(gtw_contactTactileRightOutputPort, true); // give the port a name
cout << "Waiting for connection: /icub/skin/left_hand_comp -> /gtw/telepresence/tactile/left:i" << endl;
while( !Network::isConnected("/icub/skin/left_hand_comp", "/gtw/telepresence/tactile/left:i") )
{}
cout << "Connection ready: /icub/skin/left_hand_comp -> /gtw/telepresence/tactile/left:i" << endl;
cout << "Waiting for connection: /icub/skin/right_hand_comp -> /gtw/telepresence/tactile/right:i" << endl;
while( !Network::isConnected("/icub/skin/right_hand_comp", "/gtw/telepresence/tactile/right:i") )
{}
cout << "Connection ready: /icub/skin/right_hand_comp -> /gtw/telepresence/tactile/right:i" << endl;
return true;
}
Contact RosNameSpace::detectNameServer(bool useDetectedServer,
bool& scanNeeded,
bool& serverUsed) {
NameConfig nc;
nc.fromFile();
Contact c = nc.getAddress();
scanNeeded = false;
serverUsed = false;
if (!c.isValid()) {
scanNeeded = true;
fprintf(stderr,"Checking for ROS_MASTER_URI...\n");
ConstString addr = NetworkBase::getEnvironment("ROS_MASTER_URI");
c = Contact::fromString(addr.c_str());
if (c.isValid()) {
c = c.addCarrier("xmlrpc");
c = c.addName(nc.getNamespace().c_str());
NameConfig nc;
nc.setAddress(c);
nc.setMode("ros");
nc.toFile();
serverUsed = true;
}
}
return c;
}
bool RosNameSpace::connectTopic(Bottle& cmd,
bool srcIsTopic,
const Contact& src,
const Contact& dest,
ContactStyle style,
bool activeRegistration) {
Bottle reply;
Contact dynamicSrc = src;
Contact dynamicDest = dest;
if (style.carrier!="") {
if (srcIsTopic) {
dynamicDest = dynamicDest.addCarrier(style.carrier);
} else {
dynamicSrc = dynamicSrc.addCarrier(style.carrier);
}
}
Contact base = getNameServerContact();
bool ok = NetworkBase::write(base,
cmd,
reply);
bool fail = (reply.check("faultCode",Value(0)).asInt()!=0)||!ok;
if (fail) {
if (!style.quiet) {
fprintf(stderr,"Failure: name server did not accept connection to topic.\n");
if (reply.check("faultString")) {
fprintf(stderr,"Cause: %s\n", reply.check("faultString",Value("")).asString().c_str());
}
}
}
if (!fail) {
if (activeRegistration) {
Bottle *lst = reply.get(2).asList();
Bottle cmd2;
if (lst!=NULL) {
cmd2.addString("publisherUpdate");
cmd2.addString("/yarp");
cmd2.addString(dynamicSrc.getName());
cmd2.addList() = *lst;
NetworkBase::write(dynamicDest,
cmd2,
reply,
true);
}
}
}
return !fail;
}
yarp::os::Contact RosLookup::getRosCoreAddressFromEnv() {
ConstString addr = NetworkBase::getEnvironment("ROS_MASTER_URI");
Contact c = Contact::fromString(addr.c_str());
if (c.isValid()) {
c = c.addCarrier("xmlrpc");
}
return c;
}
Contact RosNameSpace::queryName(const ConstString& name) {
dbg_printf("ROSNameSpace queryName(%s)\n", name.c_str());
NestedContact nc(name);
ConstString full = name;
ConstString node = nc.getNodeName();
ConstString srv = nc.getNestedName();
ConstString cat = nc.getCategory();
bool is_service = false;
Bottle cmd,reply;
if (cat.find("-1")==ConstString::npos) {
cmd.addString("lookupNode");
cmd.addString("dummy_id");
cmd.addString(toRosNodeName(node));
NetworkBase::write(getNameServerContact(),
cmd, reply);
}
Contact contact;
if (reply.get(0).asInt()!=1) {
cmd.clear();
reply.clear();
cmd.addString("lookupService");
cmd.addString("dummy_id");
cmd.addString(toRosNodeName(node));
NetworkBase::write(getNameServerContact(),
cmd, reply);
is_service = true;
}
contact = Contact::fromString(reply.get(2).asString());
// unfortunate differences in labeling carriers
if (contact.getCarrier()=="rosrpc") {
contact = contact.addCarrier(ConstString("rossrv+service.") + name);
} else {
contact = contact.addCarrier("xmlrpc");
}
contact = contact.addName(name);
if (srv == "" || !is_service) return contact;
return Contact();
}
bool NetworkBase::write(const Contact& contact,
PortWriter& cmd,
PortReader& reply,
const ContactStyle& style) {
if (!getNameSpace().serverAllocatesPortNumbers()) {
// switch to more up-to-date method
Port port;
port.setAdminMode(style.admin);
port.openFake("network_write");
Contact ec = contact;
if (style.carrier!="") {
ec = ec.addCarrier(style.carrier);
}
if (!port.addOutput(ec)) {
if (!style.quiet) {
ACE_OS::fprintf(stderr, "Cannot make connection to '%s'\n",
ec.toString().c_str());
}
return false;
}
bool ok = port.write(cmd,reply);
return ok;
}
const char *connectionName = "admin";
ConstString name = contact.getName();
const char *targetName = name.c_str(); // use carefully!
Contact address = contact;
if (!address.isValid()) {
address = getNameSpace().queryName(targetName);
}
if (!address.isValid()) {
if (!style.quiet) {
YARP_SPRINTF1(Logger::get(),error,
"cannot find port %s",
targetName);
}
return false;
}
if (style.timeout>0) {
address.setTimeout((float)style.timeout);
}
OutputProtocol *out = Carriers::connect(address);
if (out==NULL) {
if (!style.quiet) {
YARP_SPRINTF1(Logger::get(),error,
"Cannot connect to port %s",
targetName);
}
return false;
}
if (style.timeout>0) {
out->setTimeout(style.timeout);
}
Route r(connectionName,targetName,
(style.carrier!="")?style.carrier.c_str():"text_ack");
out->open(r);
PortCommand pc(0,style.admin?"a":"d");
BufferedConnectionWriter bw(out->getConnection().isTextMode(),
out->getConnection().isBareMode());
bool ok = true;
if (out->getConnection().canEscape()) {
ok = pc.write(bw);
}
if (!ok) {
if (!style.quiet) {
YARP_ERROR(Logger::get(),"could not write to connection");
}
if (out!=NULL) delete out;
return false;
}
ok = cmd.write(bw);
if (!ok) {
if (!style.quiet) {
YARP_ERROR(Logger::get(),"could not write to connection");
}
if (out!=NULL) delete out;
return false;
}
if (style.expectReply) {
bw.setReplyHandler(reply);
}
out->write(bw);
if (out!=NULL) {
delete out;
out = NULL;
}
return true;
}
static int metaConnect(const ConstString& src,
const ConstString& dest,
ContactStyle style,
int mode) {
YARP_SPRINTF3(Logger::get(),debug,
"working on connection %s to %s (%s)",
src.c_str(),
dest.c_str(),
(mode==YARP_ENACT_CONNECT)?"connect":((mode==YARP_ENACT_DISCONNECT)?"disconnect":"check")
);
// get the expressed contacts, without name server input
Contact dynamicSrc = Contact::fromString(src);
Contact dynamicDest = Contact::fromString(dest);
bool topical = style.persistent;
if (dynamicSrc.getCarrier()=="topic" ||
dynamicDest.getCarrier()=="topic") {
topical = true;
}
bool topicalNeedsLookup = !getNameSpace().connectionHasNameOfEndpoints();
// fetch completed contacts from name server, if needed
Contact staticSrc;
Contact staticDest;
if (needsLookup(dynamicSrc)&&(topicalNeedsLookup||!topical)) {
staticSrc = NetworkBase::queryName(dynamicSrc.getName());
if (!staticSrc.isValid()) {
if (!style.persistent) {
if (!style.quiet) {
fprintf(stderr, "Failure: could not find source port %s\n",
src.c_str());
}
return 1;
} else {
staticSrc = dynamicSrc;
}
}
} else {
staticSrc = dynamicSrc;
}
if (staticSrc.getCarrier()=="") {
staticSrc = staticSrc.addCarrier("tcp");
}
if (staticDest.getCarrier()=="") {
staticDest = staticDest.addCarrier("tcp");
}
if (needsLookup(dynamicDest)&&(topicalNeedsLookup||!topical)) {
staticDest = NetworkBase::queryName(dynamicDest.getName());
if (!staticDest.isValid()) {
if (!style.persistent) {
if (!style.quiet) {
fprintf(stderr, "Failure: could not find destination port %s\n",
dest.c_str());
}
return 1;
} else {
staticDest = dynamicDest;
}
}
} else {
staticDest = dynamicDest;
}
if (staticSrc.getCarrier()=="xmlrpc" &&
(staticDest.getCarrier()=="xmlrpc"||(staticDest.getCarrier().find("rossrv")==0))&&
mode==YARP_ENACT_CONNECT) {
// Unconnectable in general
// Let's assume the first part is a YARP port, and use "tcp" instead
staticSrc = staticSrc.addCarrier("tcp");
staticDest = staticDest.addCarrier("tcp");
}
ConstString carrierConstraint = "";
// see if we can do business with the source port
bool srcIsCompetent = false;
bool srcIsTopic = false;
if (staticSrc.getCarrier()!="topic") {
if (!topical) {
Carrier *srcCarrier = NULL;
if (staticSrc.getCarrier()!="") {
srcCarrier = Carriers::chooseCarrier(staticSrc.getCarrier().c_str());
}
if (srcCarrier!=NULL) {
String srcBootstrap = srcCarrier->getBootstrapCarrierName();
if (srcBootstrap!="") {
srcIsCompetent = true;
} else {
carrierConstraint = staticSrc.getCarrier();
}
delete srcCarrier;
srcCarrier = NULL;
}
}
} else {
srcIsTopic = true;
}
// see if we can do business with the destination port
bool destIsCompetent = false;
bool destIsTopic = false;
if (staticDest.getCarrier()!="topic") {
if (!topical) {
Carrier *destCarrier = NULL;
if (staticDest.getCarrier()!="") {
destCarrier = Carriers::chooseCarrier(staticDest.getCarrier().c_str());
}
if (destCarrier!=NULL) {
String destBootstrap = destCarrier->getBootstrapCarrierName();
if (destBootstrap!="") {
destIsCompetent = true;
} else {
carrierConstraint = staticDest.getCarrier();
}
delete destCarrier;
destCarrier = NULL;
}
}
} else {
destIsTopic = true;
}
if (srcIsTopic||destIsTopic) {
Bottle cmd, reply;
NameSpace& ns = getNameSpace();
bool ok = false;
if (srcIsTopic) {
if (mode==YARP_ENACT_CONNECT) {
ok = ns.connectTopicToPort(staticSrc,staticDest,style);
} else if (mode==YARP_ENACT_DISCONNECT) {
ok = ns.disconnectTopicFromPort(staticSrc,staticDest,style);
} else {
fprintf(stderr,"Failure: cannot check subscriptions yet\n");
return 1;
}
} else {
if (mode==YARP_ENACT_CONNECT) {
ok = ns.connectPortToTopic(staticSrc,staticDest,style);
} else if (mode==YARP_ENACT_DISCONNECT) {
ok = ns.disconnectPortFromTopic(staticSrc,staticDest,style);
} else {
fprintf(stderr,"Failure: cannot check subscriptions yet\n");
return 1;
}
}
if (!ok) {
return 1;
}
if (!style.quiet) {
if (style.verboseOnSuccess) {
fprintf(stderr,"Success: connection to topic %s.\n", mode==YARP_ENACT_CONNECT ? "added" : "removed");
}
}
return 0;
}
if (dynamicSrc.getCarrier()!="") {
style.carrier = dynamicSrc.getCarrier();
}
if (dynamicDest.getCarrier()!="") {
style.carrier = dynamicDest.getCarrier();
}
if (style.carrier!="" && carrierConstraint!="") {
if (style.carrier!=carrierConstraint) {
fprintf(stderr,"Failure: conflict between %s and %s\n",
style.carrier.c_str(),
carrierConstraint.c_str());
return 1;
}
}
if (carrierConstraint!="") {
style.carrier = carrierConstraint;
}
if (style.carrier=="") {
style.carrier = staticDest.getCarrier();
}
if (style.carrier=="") {
style.carrier = staticSrc.getCarrier();
}
bool connectionIsPush = false;
bool connectionIsPull = false;
Carrier *connectionCarrier = NULL;
if (style.carrier!="topic") {
connectionCarrier = Carriers::chooseCarrier(style.carrier.c_str());
if (connectionCarrier!=NULL) {
connectionIsPush = connectionCarrier->isPush();
connectionIsPull = !connectionIsPush;
}
}
int result = -1;
if ((srcIsCompetent&&connectionIsPush)||topical) {
// Classic case.
Contact c = Contact::fromString(dest);
if (connectionCarrier!=NULL) delete connectionCarrier;
return enactConnection(staticSrc,c,style,mode,false);
}
if (destIsCompetent&&connectionIsPull) {
Contact c = Contact::fromString(src);
if (connectionCarrier!=NULL) delete connectionCarrier;
return enactConnection(staticDest,c,style,mode,true);
}
if (connectionCarrier!=NULL) {
if (!connectionIsPull) {
Contact c = Contact::fromString(dest);
result = connectionCarrier->connect(staticSrc,c,style,mode,false);
} else {
Contact c = Contact::fromString(src);
result = connectionCarrier->connect(staticDest,c,style,mode,true);
}
}
if (connectionCarrier!=NULL) {
delete connectionCarrier;
connectionCarrier = NULL;
}
if (result!=-1) {
if (!style.quiet) {
if (result==0) {
if (style.verboseOnSuccess) {
printf("Success: added connection using custom carrier method\n");
}
} else {
printf("Failure: custom carrier method did not work\n");
}
}
return result;
}
if (mode!=YARP_ENACT_DISCONNECT) {
fprintf(stderr,"Failure: no way to make connection %s->%s\n", src.c_str(), dest.c_str());
}
return 1;
}
static int enactConnection(const Contact& src,
const Contact& dest,
const ContactStyle& style,
int mode,
bool reversed) {
ContactStyle rpc;
rpc.admin = true;
rpc.quiet = style.quiet;
rpc.timeout = style.timeout;
if (style.persistent) {
bool ok = false;
// we don't talk to the ports, we talk to the nameserver
NameSpace& ns = getNameSpace();
if (mode==YARP_ENACT_CONNECT) {
ok = ns.connectPortToPortPersistently(src,dest,style);
} else if (mode==YARP_ENACT_DISCONNECT) {
ok = ns.disconnectPortToPortPersistently(src,dest,style);
} else {
fprintf(stderr,"Failure: cannot check subscriptions yet\n");
return 1;
}
if (!ok) {
return 1;
}
if (!style.quiet) {
fprintf(stderr,"Success: port-to-port persistent connection added.\n");
}
return 0;
}
if (mode==YARP_ENACT_EXISTS) {
Bottle cmd, reply;
cmd.addVocab(Vocab::encode("list"));
cmd.addVocab(Vocab::encode(reversed?"in":"out"));
cmd.addString(dest.getName().c_str());
YARP_SPRINTF2(Logger::get(),debug,"asking %s: %s",
src.toString().c_str(), cmd.toString().c_str());
bool ok = NetworkBase::write(src,cmd,reply,rpc);
if (!ok) {
noteDud(src);
return 1;
}
if (reply.check("carrier")) {
if (!style.quiet) {
printf("Connection found between %s and %s\n",
src.getName().c_str(), dest.getName().c_str());
}
return 0;
}
return 1;
}
int act = (mode==YARP_ENACT_DISCONNECT)?VOCAB3('d','e','l'):VOCAB3('a','d','d');
// Let's ask the destination to connect/disconnect to the source.
// We assume the YARP carrier will reverse the connection if
// appropriate when connecting.
Bottle cmd, reply;
cmd.addVocab(act);
Contact c = dest;
if (style.carrier!="") {
c = c.addCarrier(style.carrier);
}
if (mode!=YARP_ENACT_DISCONNECT) {
cmd.addString(c.toString());
} else {
cmd.addString(c.getName());
}
Contact c2 = src;
if (c2.getPort()<=0) {
c2 = NetworkBase::queryName(c2.getName());
}
if (c2.getCarrier()!="tcp") {
YARP_SPRINTF2(Logger::get(),debug,"would have asked %s: %s",
src.toString().c_str(), cmd.toString().c_str());
return 1;
}
YARP_SPRINTF2(Logger::get(),debug,"** asking %s: %s",
src.toString().c_str(), cmd.toString().c_str());
bool ok = NetworkBase::write(c2,cmd,reply,rpc);
if (!ok) {
noteDud(src);
return 1;
}
ok = false;
ConstString msg = "";
if (reply.get(0).isInt()) {
ok = (reply.get(0).asInt()==0);
msg = reply.get(1).asString();
} else {
// older protocol
msg = reply.get(0).asString();
ok = msg[0]=='A'||msg[0]=='R';
}
if (mode==YARP_ENACT_DISCONNECT && !ok) {
msg = "no such connection\n";
}
if (mode==YARP_ENACT_CONNECT && !ok) {
noteDud(dest);
}
if (!style.quiet) {
if (style.verboseOnSuccess||!ok) {
fprintf(stderr,"%s %s",
ok?"Success:":"Failure:",
msg.c_str());
}
}
return ok?0:1;
}
Contact RosNameSpace::registerAdvanced(const Contact& contact, NameStore *store) {
dbg_printf("ROSNameSpace registerContact(%s / %s)\n",
contact.toString().c_str(),
contact.toURI().c_str());
NestedContact nc = contact.getNested();
if (nc.getNestedName()=="") {
nc.fromString(contact.getName());
}
ConstString cat = nc.getCategory();
if (nc.getNestedName()!="") {
if (cat == "-1") {
Bottle cmd, reply;
cmd.clear();
cmd.addString("registerService");
cmd.addString(toRosNodeName(nc.getNodeName()));
cmd.addString(toRosName(nc.getNestedName()));
Contact rosrpc = contact.addCarrier("rosrpc");
cmd.addString(rosrpc.toURI());
Contact c;
if (store) {
c = rosify(store->query(nc.getNodeName()));
} else {
Nodes& nodes = NameClient::getNameClient().getNodes();
c = rosify(nodes.getParent(contact.getName()));
}
cmd.addString(c.toString());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) return Contact();
} else if (cat == "+" || cat== "-") {
Bottle cmd, reply;
cmd.clear();
cmd.addString((cat=="+")?"registerPublisher":"registerSubscriber");
cmd.addString(toRosNodeName(nc.getNodeName()));
cmd.addString(toRosName(nc.getNestedName()));
ConstString typ = nc.getTypeNameStar();
if (typ!="*"&&typ!="") {
// remap some basic native YARP types
if (typ=="yarp/image") {
typ = "sensor_msgs/Image";
} else if (typ=="yarp/vector") {
typ = "std_msgs/Float64MultiArray";
}
if (typ.find("/")==ConstString::npos) {
typ = ConstString("yarp/") + typ;
}
}
cmd.addString(typ);
Contact c;
if (store) {
c = rosify(store->query(nc.getNodeName()));
} else {
Nodes& nodes = NameClient::getNameClient().getNodes();
c = rosify(nodes.getParent(contact.getName()));
}
//Contact c = rosify(contact);
cmd.addString(c.toString());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) {
fprintf(stderr, "ROS registration error: %s\n", reply.toString().c_str());
return Contact();
}
if (cat=="-") {
Bottle *publishers = reply.get(2).asList();
if (publishers && publishers->size()>=1) {
cmd.clear();
cmd.addString(contact.toURI());
cmd.addString("publisherUpdate");
cmd.addString("/yarp/RosNameSpace");
cmd.addString(toRosName(nc.getNestedName()));
cmd.addList() = *publishers;
mutex.wait();
bool need_start = false;
if (pending.size()==0) {
mutex.post();
stop();
need_start = true;
mutex.wait();
}
pending.addList() = cmd;
if (need_start) {
start();
}
mutex.post();
}
}
}
return contact;
}
// Remainder of method is supporting older /name+#/foo syntax
ConstString full = contact.getName();
ConstString name = full;
size_t pub_idx = name.find("+#");
size_t sub_idx = name.find("-#");
ConstString node = "";
ConstString pub = "";
ConstString sub = "";
if (pub_idx!=ConstString::npos) {
node = name.substr(0,pub_idx);
pub = name.substr(pub_idx+2,name.length());
YARP_SPRINTF1(Logger::get(),debug,"Publish to %s",pub.c_str());
}
if (sub_idx!=ConstString::npos) {
node = name.substr(0,sub_idx);
sub = name.substr(sub_idx+2,name.length());
YARP_SPRINTF1(Logger::get(),debug,"Subscribe to %s",sub.c_str());
}
if (node=="") {
node = name;
}
YARP_SPRINTF4(Logger::get(),debug,"Name [%s] Node [%s] sub [%s] pub [%s]",
name.c_str(), node.c_str(), sub.c_str(), pub.c_str());
{
Bottle cmd, reply;
// for ROS, we fake port name registrations by
// registering them as nodes that publish to an arbitrary
// topic
cmd.clear();
cmd.addString("registerPublisher");
cmd.addString(toRosNodeName(node));
cmd.addString("/yarp/registration");
cmd.addString("*");
Contact c = rosify(contact);
cmd.addString(c.toString());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) return Contact();
}
if (pub!="") {
NetworkBase::connect(node, ConstString("topic:/") + pub);
}
if (sub!="") {
NetworkBase::connect(ConstString("topic:/") + sub, node);
}
return contact.addName(node);
}
