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;
}
void RosNameSpace::run() {
int pct = 0;
do {
mutex.wait();
pct = pending.size();
mutex.post();
if (pct>0) {
mutex.wait();
Bottle *bot = pending.get(0).asList();
Bottle curr = *bot;
mutex.post();
dbg_printf("ROS connection begins: %s\n", curr.toString().c_str());
ContactStyle style;
style.admin = true;
style.carrier = "tcp";
Bottle cmd = curr.tail();
Contact contact = Contact::fromString(curr.get(0).asString());
contact = contact.addName("");
Bottle reply;
NetworkBase::write(contact, cmd, reply, style);
dbg_printf("ROS connection ends: %s\n", curr.toString().c_str());
mutex.wait();
pending = pending.tail();
pct = pending.size();
mutex.post();
}
} while (pct>0);
}
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;
}
int main(int argc, char *argv[]) {
Network yarp;
Rand::init();
yarp.setLocalMode(true);
Property config;
config.fromCommand(argc,argv);
Contact contact = Contact::bySocket("tcp","localhost",
DEFAULT_NAME_PORT_NUMBER);
contact = contact.addName("/root");
WideNameService wide;
NameServerManager manager(wide);
Port server;
manager.setPort(server);
server.setReaderCreator(manager);
bool ok = server.open(contact,false);
if (!ok) {
fprintf(stderr, "Name server failed to open\n");
return 1;
}
while (true) {
Time::delay(600);
printf("Name server running happily\n");
}
return 0;
}
Contact YarpNameSpace::detectNameServer(bool useDetectedServer,
bool& scanNeeded,
bool& serverUsed) {
NameConfig nc;
NameClient& nic = HELPER(this);
nic.setFakeMode(false);
nic.updateAddress();
nic.setScan();
if (useDetectedServer) {
nic.setSave();
}
nic.send("ping",false);
scanNeeded = nic.didScan();
serverUsed = nic.didSave();
Contact c = nic.getAddress();
c = c.addName(nc.getNamespace().c_str());
//Contact c = nic.getAddress().toContact();
// if (scanNeeded) {
// Address addr = nic.getAddress();
// c = c.addSocket("tcp",addr.getName().c_str(),addr.getPort());
////}
//c = c.addName(nc.getNamespace().c_str());
return c;
}
void publisherUpdate(NodeArgs& na) {
ConstString topic = na.args.get(0).asString();
Contact c = lookup(topic);
if (!c.isValid()) {
na.fail("Cannot find topic");
return;
}
c = c.addName("");
// just pass the message along, YARP ports know what to do with it
ContactStyle style;
style.admin = true;
style.carrier = "tcp";
Bottle reply;
if (!NetworkBase::write(c,na.request,reply,style)) {
na.fail("Cannot communicate with local port");
return;
}
na.fromExternal(reply);
//printf("DONE with passing on publisherUpdate\n");
}
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();
}
Contact Contact::fromString(const ConstString& txt) {
ConstString str(txt);
Contact c;
ConstString::size_type start = 0;
ConstString::size_type base = str.find("://");
ConstString::size_type offset = 2;
if (base==ConstString::npos) {
base = str.find(":/");
offset = 1;
}
if (base==ConstString::npos) {
if (str.length()>0 && str[0] == '/') {
base = 0;
offset = 0;
}
}
if (base!=ConstString::npos) {
c = Contact::byCarrier(str.substr(0,base));
start = base+offset;
// check if we have a direct machine:NNN syntax
ConstString::size_type colon = ConstString::npos;
int mode = 0;
int nums = 0;
ConstString::size_type i;
for (i=start+1; i<str.length(); i++) {
char ch = str[i];
if (ch==':') {
if (mode == 0) {
colon = i;
mode = 1;
continue;
} else {
mode = -1;
break;
}
}
if (ch=='/') {
break;
}
if (mode==1) {
if (ch>='0'&&ch<='9') {
nums++;
continue;
} else {
mode = -1;
break;
}
}
}
if (mode==1 && nums>=1) {
// yes, machine:nnn
ConstString machine = str.substr(start+1,colon-start-1);
ConstString portnum = str.substr(colon+1, nums);
c = c.addSocket(c.getCarrier()==""?"tcp":c.getCarrier().c_str(),
machine,
atoi(portnum.c_str()));
start = i;
}
}
ConstString rname = str.substr(start);
if (rname!="/") {
c = c.addName(rname.c_str());
}
return c;
}
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);
}
