int DgramTwoWayStream::restrictMcast(ACE_SOCK_Dgram_Mcast * dmcast,
const Contact& group,
const Contact& ipLocal,
bool add) {
restrictInterfaceIp = ipLocal;
YARP_INFO(Logger::get(),
String("multicast connection ") + group.getHost() + " on network interface for " + ipLocal.getHost());
int result = -1;
// There's some major damage in ACE mcast interfaces.
// Most require interface names, yet provide no way to query
// these names - and in the end, convert to IP addresses.
// Here we try to do an end run around ACE.
// based on: ACE_SOCK_Dgram::set_nic
ip_mreq multicast_address;
ACE_INET_Addr group_addr(group.getPort(),
group.getHost().c_str());
ACE_INET_Addr interface_addr(ipLocal.getPort(),
ipLocal.getHost().c_str());
multicast_address.imr_interface.s_addr =
htonl (interface_addr.get_ip_address ());
multicast_address.imr_multiaddr.s_addr =
htonl (group_addr.get_ip_address ());
if (add) {
YARP_DEBUG(Logger::get(),"Trying to correct mcast membership...\n");
result =
((ACE_SOCK*)dmcast)->set_option (IPPROTO_IP,
IP_ADD_MEMBERSHIP,
&multicast_address,
sizeof (struct ip_mreq));
} else {
YARP_DEBUG(Logger::get(),"Trying to correct mcast output...");
result =
((ACE_SOCK*)dmcast)->set_option (IPPROTO_IP,
IP_MULTICAST_IF,
&multicast_address.imr_interface.s_addr,
sizeof (struct in_addr));
}
if (result!=0) {
int num = errno;
YARP_DEBUG(Logger::get(),
String("mcast result: ") +
strerror(num));
if (num==98) {
// our membership is already correct / Address already in use
result = 0;
}
result = 0; // in fact, best to proceed for Windows.
}
return result;
}
int SocketTwoWayStream::open(const Contact& address) {
if (address.getPort()==-1) {
return -1;
}
std::string host = address.getHost();
yarp::os::impl::TcpConnector connector;
#ifdef YARP_HAS_ACE
if (address.getHost() == "localhost") {
// ACE does not like localhost. At all.
NameConfig config;
host = config.getHostName(true);
}
ACE_INET_Addr addr(address.getPort(), host.c_str());
YARP_timeval openTimeout;
YARP_timeval *timeout = nullptr;
if (address.hasTimeout()) {
openTimeout.set(address.getTimeout());
timeout = &openTimeout;
}
int result = connector.connect(stream, addr, timeout, ACE_Addr::sap_any, 1);
#else
int result;
if (address.hasTimeout())
{
YARP_timeval timeout;
/* set timeout seconds and microseconds */
timeout.tv_sec = (int) address.getTimeout();
timeout.tv_usec = (address.getTimeout() - (int) address.getTimeout()) * 1000000;
result = connector.connect(stream, address, &timeout);
}
else
{
result = connector.connect(stream, address, nullptr);
}
#endif
if (result>=0) {
happy = true;
} else {
YARP_SPRINTF2(Logger::get(),
debug,
"TCP connection to tcp://%s:%d failed to open",
host.c_str(),
address.getPort());
}
updateAddresses();
return result;
}
ConstString NameServer::apply(const ConstString& txt, const Contact& remote) {
ConstString result = "no command given";
mutex.wait();
SplitString ss(txt.c_str());
if (ss.size()>=2) {
ConstString key = ss.get(1);
//YARP_DEBUG(Logger::get(),ConstString("dispatching to ") + key);
ss.set(1,remote.getHost().c_str());
result = dispatcher.dispatch(this,key.c_str(),ss.size()-1,
(char **)(ss.get()+1));
if (result == "") {
Bottle b = ndispatcher.dispatch(this,key.c_str(),ss.size()-1,
(char **)(ss.get()+1));
result = b.toString().c_str();
if (result!="") {
result = result + "\n";
result = terminate(result);
}
}
//YARP_DEBUG(Logger::get(), ConstString("name server request -- ") + txt);
//YARP_DEBUG(Logger::get(), ConstString("name server result -- ") + result);
}
mutex.post();
return result;
}
Bottle NameServer::botify(const Contact& address) {
Bottle result;
if (address.isValid()) {
Bottle bname;
bname.addString("name");
bname.addString(address.getRegName().c_str());
Bottle bip;
bip.addString("ip");
bip.addString(address.getHost().c_str());
Bottle bnum;
bnum.addString("port_number");
bnum.addInt(address.getPort());
Bottle bcarrier;
bcarrier.addString("carrier");
bcarrier.addString(address.getCarrier().c_str());
result.addString("port");
result.addList() = bname;
result.addList() = bip;
result.addList() = bnum;
result.addList() = bcarrier;
} else {
Bottle bstate;
bstate.addString("error");
bstate.addInt(-2);
bstate.addString("port not known");
result.addString("port");
result.addList() = bstate;
}
return result;
}
int ShmemHybridStream::open(const Contact& yarp_address,bool sender)
{
m_bLinked=false;
ACE_INET_Addr ace_address(yarp_address.getPort(),yarp_address.getHost().c_str());
if (sender)
{
return connect(ace_address);
}
else
{
ACE_INET_Addr ace_server_addr(ace_address.get_port_number());
int result = m_Acceptor.open(ace_server_addr);
if (result<0)
{
YARP_ERROR(Logger::get(),ConstString("ShmemHybridStream open result")+NetType::toString(result));
return result;
}
m_Acceptor.get_local_addr(ace_server_addr);
m_LocalAddress = Contact(ace_server_addr.get_host_addr(),ace_server_addr.get_port_number());
m_RemoteAddress = m_LocalAddress; // finalized in call to accept()
return result;
}
return 1;
}
Contact NameServer::unregisterName(const ConstString& name) {
Contact prev = queryName(name);
if (prev.isValid()) {
if (prev.getPort()!=-1) {
NameRecord& rec = getNameRecord(prev.getRegName());
if (rec.isReusablePort()) {
HostRecord& host = getHostRecord(prev.getHost());
host.release(prev.getPort());
}
if (rec.isReusableIp()) {
if (rec.getAddress().getCarrier()=="mcast") {
mcastRecord.releaseAddress(rec.getAddress().getHost().c_str());
}
}
rec.clear();
tmpNames.release(name);
Bottle event;
event.addVocab(Vocab::encode("del"));
event.addString(name.c_str());
onEvent(event);
}
}
return queryName(name);
}
bool DgramTwoWayStream::join(const Contact& group, bool sender,
const Contact& ipLocal) {
#ifdef YARP_HAS_ACE
YARP_DEBUG(Logger::get(),String("subscribing to mcast address ") +
group.toURI() + " for " +
(sender?"writing":"reading"));
multiMode = true;
if (sender) {
if (ipLocal.isValid()) {
return openMcast(group,ipLocal);
} else {
// just use udp as normal
return open(group);
}
//return;
}
ACE_SOCK_Dgram_Mcast *dmcast = new ACE_SOCK_Dgram_Mcast;
//possible flags: ((ACE_SOCK_Dgram_Mcast::options)(ACE_SOCK_Dgram_Mcast::OPT_NULLIFACE_ALL | ACE_SOCK_Dgram_Mcast::OPT_BINDADDR_YES));
dgram = dmcast;
mgram = dmcast;
yAssert(dgram!=NULL);
ACE_INET_Addr addr(group.getPort(),group.getHost().c_str());
int result = -1;
if (ipLocal.isValid()) {
result = 0;
result = dmcast->join(addr,1);
if (result==0) {
result = restrictMcast(dmcast,group,ipLocal,true);
}
} else {
result = dmcast->join(addr,1);
}
configureSystemBuffers();
if (result!=0) {
YARP_ERROR(Logger::get(),"cannot connect to multi-cast address");
happy = false;
return false;
}
localAddress = group;
remoteAddress = group;
localHandle.set(localAddress.getPort(),localAddress.getHost().c_str());
remoteHandle.set(remoteAddress.getPort(),remoteAddress.getHost().c_str());
allocate();
return true;
#else
return false;
#endif
}
yarp::os::Contact NameServiceOnTriples::query(const ConstString& port) {
Contact check = Contact::fromString(port);
if (check.getHost()!="") return check;
Bottle cmd,reply,event;
Contact remote;
TripleSource& mem = *db;
NameTripleState act(cmd,reply,event,remote,mem);
return query(port,act,"");
}
bool yarp::os::impl::HttpCarrier::sendHeader(ConnectionState& proto) {
ConstString target = "GET / HTTP/1.0\r\n";
ConstString path = proto.getRoute().getToName();
if (path.size()>=2) {
target = "GET " + path + " HTTP/1.0\r\n";
}
Contact host = proto.getRoute().getToContact();
if (host.getHost()!="") {
target += "Host: ";
target += host.getHost();
target += "\r\n";
}
target += "\r\n";
Bytes b((char*)target.c_str(),target.length());
proto.os().write(b);
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
}
static bool needsLookup(const Contact& contact)
{
if (contact.getHost() != "") {
return false;
}
if (contact.getCarrier() == "topic") {
return false;
}
return true;
}
Contact RosNameSpace::rosify(const Contact& contact) {
ConstString carrier = ((contact.getCarrier() == "rosrpc") ? "rosrpc" : "http");
ConstString hostname = contact.getHost();
if (yarp::os::impl::NameConfig::isLocalName(hostname)) {
char hn[HOST_NAME_MAX];
yarp::os::gethostname(hn, sizeof(hn));
hostname = hn;
}
return Contact(carrier, hostname, contact.getPort());
}
bool MjpegCarrier::sendHeader(ConnectionState& proto) {
Name n(proto.getRoute().getCarrierName() + "://test");
ConstString pathValue = n.getCarrierModifier("path");
ConstString target = "GET /?action=stream\n\n";
if (pathValue!="") {
target = "GET /";
target += pathValue;
}
target += " HTTP/1.1\n";
Contact host = proto.getRoute().getToContact();
if (host.getHost()!="") {
target += "Host: ";
target += host.getHost();
target += "\r\n";
}
target += "\n";
Bytes b((char*)target.c_str(),target.length());
proto.os().write(b);
return true;
}
void requestTopic(NodeArgs& na) {
ConstString topic = na.args.get(0).asString();
Contact c = lookup(topic);
if (!c.isValid()) {
na.fail("Cannot find topic");
return;
}
na.reply.addString("TCPROS");
na.reply.addString(c.getHost());
na.reply.addInt(c.getPort());
na.success();
}
void appendEntry(yarp::os::Bottle& reply, const Contact& c) {
Bottle& info = reply.addList();
info.addString("registration");
info.addString("name");
info.addString(c.getName().c_str());
info.addString("ip");
info.addString(c.getHost().c_str());
info.addString("port");
info.addInt(c.getPort());
info.addString("type");
info.addString(c.getCarrier().c_str());
}
Contact RosNameSpace::unregisterContact(const Contact& contact) {
// Remainder of method is supporting older /name+#/foo syntax
Bottle cmd, reply;
cmd.addString("unregisterSubscriber");
cmd.addString(contact.getName());
cmd.addString("/yarp/registration");
Contact c("http", contact.getHost().c_str(), contact.getPort());
cmd.addString(c.toString());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) return Contact();
return Contact();
}
/**
* Connect to server
*/
int TcpConnector::connect(TcpStream &new_stream, const Contact& address) {
// printf("TCP/IP start in client mode\n");
// sockets.set_as_client();
// sockets.set_client_sockfd(sockfd);
if (open (new_stream) == -1)
return -1;
// Write sockaddr struct with given address...
sockaddr_in servAddr;
servAddr.sin_addr.s_addr = INADDR_ANY;
servAddr.sin_family = AF_INET;
servAddr.sin_port = htons(address.getPort());
memset(servAddr.sin_zero, '\0', sizeof servAddr.sin_zero);
struct hostent *hostInfo = gethostbyname(address.getHost().c_str());
if (hostInfo) {
bcopy(hostInfo->h_addr,(char *)(&servAddr.sin_addr),hostInfo->h_length);
} else {
inet_aton(address.getHost().c_str(), &servAddr.sin_addr);
}
// Address servAddress,
// servAddress = Address(inet_ntoa(servAddr.sin_addr),servAddr.sin_port);
YARP_ASSERT (new_stream.get_handle() != -1);
int result = ::connect(new_stream.get_handle(), (sockaddr*) &servAddr, sizeof(servAddr));
// std::cout << "Connect [handle=" << new_stream.get_handle() << "] at " << inet_ntoa(servAddr.sin_addr) << ":" << servAddr.sin_port << std::endl;
if (result < 0) {
perror("TcpConnector::connect fail");
std::cerr << "Connect [handle=" << new_stream.get_handle() << "] at " << inet_ntoa(servAddr.sin_addr) << ":" << servAddr.sin_port << std::endl;
}
return result;
}
int SocketTwoWayStream::open(const Contact& address) {
if (address.getPort()==-1) {
return -1;
}
String host = address.getHost();
#ifdef YARP_HAS_ACE
ACE_SOCK_Connector connector;
if (address.getHost() == "localhost") {
// ACE does not like localhost. At all.
NameConfig config;
host = config.getHostName(true);
}
ACE_INET_Addr addr(address.getPort(),host.c_str());
ACE_Time_Value openTimeout;
ACE_Time_Value *timeout = NULL;
if (address.hasTimeout()) {
openTimeout.set(address.getTimeout());
timeout = &openTimeout;
}
int result = connector.connect(stream,addr,timeout,ACE_Addr::sap_any,1);
#else
TcpConnector connector;
int result = connector.connect(stream, address);
#endif
if (result>=0) {
happy = true;
} else {
YARP_SPRINTF2(Logger::get(),
debug,
"TCP connection to tcp://%s:%d failed to open",
host.c_str(),
address.getPort());
}
updateAddresses();
return result;
}
int yarp_link(const char *from, const char *to) {
//TODO: will it ever be possible to hard link ports?
// If possible, it might be an alias for yarp_simlink, as with YARP
// there isn't a sym/hard linking difference
//Create the new Contact
Contact src = Network::queryName(from);
printf("source [%s] is %s\n", from, src.toString().c_str());
Contact dest = Contact::byName(to).addSocket(src.getCarrier(),src.getHost(),src.getPort());
printf("dest [%s] should be %s\n", to, src.toString().c_str());
return 0;
}
/**
* Open the server port and listen for clients
*/
int TcpAcceptor::shared_open(const Contact& address) {
struct sockaddr_in servAddr;
servAddr.sin_addr.s_addr = INADDR_ANY;
servAddr.sin_family = AF_INET;
servAddr.sin_port = htons((address.getPort()>0)?address.getPort():0);
inet_aton(address.getHost().c_str(), &servAddr.sin_addr);
memset(servAddr.sin_zero, '\0', sizeof servAddr.sin_zero);
// servAddress = Address(inet_ntoa(servAddr.sin_addr),servAddr.sin_port);
if (bind(get_handle(), (struct sockaddr *)&servAddr,
sizeof (struct sockaddr)) == -1) {
perror("At bind(sockfd) there was an error...");
return -1;
}
if (listen(get_handle(), BACKLOG) == -1) {
perror("At listen(sockfd) there was an error...");
return -1;
}
struct sigaction sa;
sa.sa_handler = sigchld_handler;
sigemptyset(&sa.sa_mask);
sa.sa_flags = SA_RESTART;
if (sigaction(SIGCHLD, &sa, NULL) == -1) {
perror("At sigaction(address) there was an error...");
return -1;
}
struct sockaddr_in sin;
socklen_t addrlen = sizeof(sin);
if (getsockname(get_handle(), (struct sockaddr *)&sin, &addrlen) == 0 &&
sin.sin_family == AF_INET &&
addrlen == sizeof(sin)) {
port_number = (int)ntohs(sin.sin_port);
} else {
perror("At getsockname(address) there was an error...");
return -1;
}
return 1;
}
bool DgramTwoWayStream::openMcast(const Contact& group,
const Contact& ipLocal) {
#ifdef YARP_HAS_ACE
multiMode = true;
localAddress = ipLocal;
localHandle = ACE_INET_Addr((u_short)(localAddress.getPort()),
(ACE_UINT32)INADDR_ANY);
ACE_SOCK_Dgram_Mcast *dmcast = new ACE_SOCK_Dgram_Mcast;
dgram = dmcast;
mgram = dmcast;
yAssert(dgram!=NULL);
int result = -1;
ACE_INET_Addr addr(group.getPort(),group.getHost().c_str());
if (ipLocal.isValid()) {
result = dmcast->open(addr,NULL,1);
if (result==0) {
result = restrictMcast(dmcast,group,ipLocal,false);
}
} else {
result = dmcast->open(addr,NULL,1);
}
if (result!=0) {
YARP_ERROR(Logger::get(),"could not open multicast datagram socket");
return false;
}
configureSystemBuffers();
remoteAddress = group;
localHandle.set(localAddress.getPort(),localAddress.getHost().c_str());
remoteHandle.set(remoteAddress.getPort(),remoteAddress.getHost().c_str());
allocate();
return true;
#else
return false;
#endif
}
ConstString NameServer::textify(const Contact& address) {
ConstString result = "";
if (address.isValid()) {
if (address.getPort()>=0) {
result = "registration name ";
result = result + address.getRegName() +
" ip " + address.getHost() + " port " +
NetType::toString(address.getPort()) + " type " +
address.getCarrier() + "\n";
} else {
result = "registration name ";
result = result + address.getRegName() +
" ip " + "none" + " port " +
"none" + " type " +
address.getCarrier() + "\n";
}
}
return result;
}
void checkTimeoutNetworkExists() {
ContactStyle style;
style.timeout = 2.0;
report(0,"checking Network::exists timeout");
Port p;
p.open("/tcp");
Contact c = p.where();
bool ok = Network::exists("/tcp",style);
checkTrue(ok,"a yarp port");
p.close();
TcpFace face;
Contact address(c.getHost(),c.getPort());
checkTrue(face.open(address),"open server socket, timeout check proceeds");
Network::registerContact(c);
ok = Network::exists("/tcp",style);
Network::unregisterContact(c);
checkFalse(ok,"not a yarp port");
face.close();
}
int yarp_rename(const char *from, const char *to) {
//TODO: the current code just renames ports, eg:
// /read can become /rd, but /read/rd1 cannot become /rd/rd1
// every subport/subdirectory should have to be renamed
YPath ypath(from);
if (!ypath.isPort()) { //Check that the path exists? Is it right?
return -ENOENT;
}
//Create the new Contact
Contact src = Network::queryName(from);
Network::unregisterContact(src);
Contact dest(to, src.getCarrier(), src.getHost(), src.getPort());
Network::registerContact(dest);
return 0;
}
bool yarp::os::impl::McastCarrier::sendHeader(ConnectionState& proto) {
// need to do more than the default
bool ok = defaultSendHeader(proto);
if (!ok) return false;
YARP_DEBUG(Logger::get(),"Adding extra mcast header");
Contact addr;
Contact alt = proto.getStreams().getLocalAddress();
ConstString altKey =
proto.getRoute().getFromName() +
"/net=" + alt.getHost();
McastCarrier *elect = getCaster().getElect(altKey);
if (elect!=YARP_NULLPTR) {
YARP_DEBUG(Logger::get(),"picking up peer mcast name");
addr = elect->mcastAddress;
mcastName = elect->mcastName;
} else {
// fetch an mcast address
Contact target("...", "mcast", "...", 0);
addr = NetworkBase::registerContact(target);
mcastName = addr.getRegName();
if (addr.isValid()) {
// mark owner of mcast address
NetworkBase::setProperty(proto.getRoute().getFromName().c_str(),
"owns",
Value(mcastName.c_str()));
}
}
int ip[] = { 224, 3, 1, 1 };
int port = 11000;
if (addr.isValid()) {
SplitString ss(addr.getHost().c_str(),'.');
if (ss.size()!=4) {
addr = Contact();
} else {
yAssert(ss.size()==4);
for (int i=0; i<4; i++) {
ip[i] = NetType::toInt(ss.get(i));
}
port = addr.getPort();
}
}
if (!addr.isValid()) {
YARP_ERROR(Logger::get(), "Name server not responding helpfully, setting mcast name arbitrarily.");
YARP_ERROR(Logger::get(), "Only a single mcast address supported in this mode.");
addr = Contact("/tmp/mcast", "mcast", "224.3.1.1", 11000);
}
ManagedBytes block(6);
for (int i=0; i<4; i++) {
((unsigned char*)block.get())[i] = (unsigned char)ip[i];
}
block.get()[5] = (char)(port%256);
block.get()[4] = (char)(port/256);
proto.os().write(block.bytes());
mcastAddress = addr;
return true;
}
bool Port::open(const Contact& contact, bool registerName,
const char *fakeName) {
Contact contact2 = contact;
if (!NetworkBase::initialized()) {
YARP_ERROR(Logger::get(), "YARP not initialized; create a yarp::os::Network object before using ports");
return false;
}
ConstString n = contact2.getName();
bool local = false;
if (n == "" && contact2.getPort()<=0) {
local = true;
registerName = false;
n = "...";
}
NestedContact nc(n);
if (nc.getNestedName()!="") {
if (nc.getNodeName() == "") {
Nodes& nodes = NameClient::getNameClient().getNodes();
nodes.requireActiveName();
ConstString node_name = nodes.getActiveName();
if (node_name!="") {
n = n + node_name;
}
}
}
if (n!="" && n[0]!='/' && n[0]!='=' && n!="..." && n.substr(0,3)!="...") {
if (fakeName==NULL) {
Nodes& nodes = NameClient::getNameClient().getNodes();
ConstString node_name = nodes.getActiveName();
if (node_name!="") {
// n = node_name + "=/" + n;
n = "/" + n + "@" + node_name;
}
}
}
if (n!="" && n[0]!='/' && n[0]!='=' && n!="..." && n.substr(0,3)!="...") {
if (fakeName==NULL) {
YARP_SPRINTF1(Logger::get(),error,
"Port name '%s' needs to start with a '/' character",
n.c_str());
return false;
}
}
if (n!="" && n!="..." && n[0]!='=' && n.substr(0,3)!="...") {
if (fakeName==NULL) {
ConstString prefix = NetworkBase::getEnvironment("YARP_PORT_PREFIX");
if (prefix!="") {
n = prefix + n;
contact2 = contact2.addName(n);
}
}
}
PortCoreAdapter *currentCore = &(HELPER(implementation));
if (currentCore!=NULL) {
NestedContact nc;
nc.fromString(n);
if (nc.getNestedName()!="") {
if (nc.getCategory()=="") {
// we need to add in a category
ConstString cat = "";
if (currentCore->commitToRead) {
cat = "-";
} else if (currentCore->commitToWrite) {
cat = "+";
}
if (cat!="") {
if (currentCore->commitToRpc) {
cat += "1";
}
contact2 = contact2.addName(nc.getNestedName() +
cat +
"@" +
nc.getNodeName());
} else {
YARP_SPRINTF1(Logger::get(),error,
"Error: Port '%s' is not committed to being either an input or output port.",
n.c_str());
YARP_SPRINTF0(Logger::get(),error,
"YARP does not mind, but we are trying to register with a name server that does.");
YARP_SPRINTF0(Logger::get(),error,
"You can call Port::setWriteOnly() or Port::setReadOnly(), OR rename the port.");
NestedContact nc2 = nc;
nc2.setCategoryWrite();
YARP_SPRINTF1(Logger::get(),error,
"For an output port, call it: %s (+ adds data)",
nc2.toString().c_str());
nc2.setCategoryRead();
YARP_SPRINTF1(Logger::get(),error,
"For an input port, call it: %s (- takes data)",
nc2.toString().c_str());
return false;
}
}
}
}
// Allow for open() to be called safely many times on the same Port
if (currentCore->isOpened()) {
PortCoreAdapter *newCore = new PortCoreAdapter(*this);
YARP_ASSERT(newCore!=NULL);
// copy state that should survive in a new open()
if (currentCore->checkPortReader()!=NULL) {
newCore->configReader(*(currentCore->checkPortReader()));
}
if (currentCore->checkReadCreator()!=NULL) {
newCore->configReadCreator(*(currentCore->checkReadCreator()));
}
if (currentCore->checkWaitAfterSend()>=0) {
newCore->configWaitAfterSend(currentCore->checkWaitAfterSend());
}
close();
delete ((PortCoreAdapter*)implementation);
implementation = newCore;
}
PortCoreAdapter& core = HELPER(implementation);
core.openable();
if (NetworkBase::localNetworkAllocation()&&contact2.getPort()<=0) {
YARP_DEBUG(Logger::get(),"local network allocation needed");
local = true;
}
bool success = true;
Contact caddress = Contact::bySocket(contact2.getCarrier(),
contact2.getHost(),
contact2.getPort())
.addName(contact2.getName());
caddress.setNested(contact2.getNested());
Contact address = caddress;
core.setReadHandler(core);
if (contact2.getPort()>0 && contact2.getHost()!="") {
registerName = false;
}
ConstString typ = getType().getName();
if (typ!="") {
NestedContact nc;
nc.fromString(contact2.getName());
nc.setTypeName(typ);
contact2.setNested(nc);
}
if (registerName&&!local) {
address = NetworkBase::registerContact(contact2);
}
core.setControlRegistration(registerName);
success = (address.isValid()||local)&&(fakeName==NULL);
if (success) {
// create a node if needed
Nodes& nodes = NameClient::getNameClient().getNodes();
nodes.prepare(address.getRegName().c_str());
}
// If we are a service client, go ahead and connect
if (success) {
NestedContact nc;
nc.fromString(address.getName());
if (nc.getNestedName()!="") {
if (nc.getCategory() == "+1") {
addOutput(nc.getNestedName());
}
}
}
ConstString blame = "invalid address";
if (success) {
success = core.listen(address,registerName);
blame = "address conflict";
if (success) {
success = core.start();
blame = "manager did not start";
}
}
if (success) {
address = core.getAddress();
if (registerName&&local) {
contact2 = contact2.addSocket(address.getCarrier(),
address.getHost(),
address.getPort());
contact2 = contact2.addName(address.getRegName().c_str());
Contact newName = NetworkBase::registerContact(contact2);
core.resetPortName(newName.getName());
address = core.getAddress();
} else if (core.getAddress().getRegName()=="" && !registerName) {
core.resetPortName(core.getAddress().addCarrier("").toURI());
core.setName(core.getAddress().getRegName());
}
if (core.getVerbosity()>=1) {
if (address.getRegName()=="") {
YARP_INFO(Logger::get(),
String("Anonymous port active at ") +
address.toURI());
} else {
YARP_INFO(Logger::get(),
String("Port ") +
address.getRegName() +
" active at " +
address.toURI());
}
}
}
if (fakeName!=NULL) {
success = core.manualStart(fakeName);
blame = "unmanaged port failed to start";
}
if (!success) {
YARP_ERROR(Logger::get(),
String("Port ") +
(address.isValid()?(address.getRegName().c_str()):(contact2.getName().c_str())) +
" failed to activate" +
(address.isValid()?" at ":"") +
(address.isValid()?address.toURI():String("")) +
" (" +
blame.c_str() +
")");
}
if (success) {
// create a node if needed
Nodes& nodes = NameClient::getNameClient().getNodes();
nodes.add(*this);
}
return success;
}
bool NameServiceOnTriples::cmdQuery(NameTripleState& act, bool nested) {
ConstString port = act.cmd.get(1).asString();
ParseName parser;
parser.apply(port.c_str());
port = parser.getPortName();
/*
// port names may be prefixed - sort that out
ConstString base = port;
ConstString pat = "";
if (base.find("/net=") == 0 || base.find("/NET=") == 0) {
int patStart = 5;
int patEnd = base.find('/',patStart);
if (patEnd>=patStart) {
pat = base.substr(patStart,patEnd-patStart);
base = base.substr(patEnd);
}
port = base;
}
*/
if (act.reply.size()==0 && !act.bottleMode) {
act.reply.addString("old");
}
Bottle& q=(act.bottleMode&&!act.nestedMode)?
act.reply :
act.reply.addList();
Contact c = query(port, act, parser.getNetworkChoice(), nested);
ConstString host = c.getHost();
ConstString carrier = c.getCarrier();
int sock = c.getPort();
if (c.isValid()) {
if (!act.bottleMode) {
q.addString("registration");
q.addString("name");
q.addString(port);
q.addString("ip");
q.addString(host);
q.addString("port");
q.addInt(sock);
q.addString("type");
q.addString(carrier);
} else {
Bottle bname;
bname.addString("name");
bname.addString(port);
Bottle bip;
bip.addString("ip");
bip.addString(host);
Bottle bnum;
bnum.addString("port_number");
bnum.addInt(sock);
Bottle bcarrier;
bcarrier.addString("carrier");
bcarrier.addString(carrier);
q.addString("port");
q.addList() = bname;
q.addList() = bip;
q.addList() = bnum;
q.addList() = bcarrier;
}
} else {
if (act.bottleMode) {
Bottle bstate;
bstate.addString("error");
bstate.addInt(-2);
bstate.addString("port not known");
q.addString("port");
q.addList() = bstate;
}
}
return true;
}
bool DgramTwoWayStream::open(const Contact& local, const Contact& remote) {
localAddress = local;
remoteAddress = remote;
#ifdef YARP_HAS_ACE
localHandle = ACE_INET_Addr((u_short)(localAddress.getPort()),(ACE_UINT32)INADDR_ANY);
if (remote.isValid()) {
remoteHandle.set(remoteAddress.getPort(),remoteAddress.getHost().c_str());
}
dgram = new ACE_SOCK_Dgram;
yAssert(dgram!=NULL);
int result = dgram->open(localHandle,
ACE_PROTOCOL_FAMILY_INET,
0,
1);
#else
dgram = NULL;
dgram_sockfd = -1;
int s = -1;
if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
exit(1);
}
struct sockaddr_in dgram_sin;
memset((char *) &dgram_sin, 0, sizeof(dgram_sin));
dgram_sin.sin_family = AF_INET;
dgram_sin.sin_addr.s_addr = htonl(INADDR_ANY);
dgram_sin.sin_port = htons(remote.getPort());
if (local.isValid()) {
if (inet_aton(remote.getHost().c_str(), &dgram_sin.sin_addr)==0) {
YARP_ERROR(Logger::get(), "could not set up udp client\n");
exit(1);
}
if (connect(s, (struct sockaddr *)&dgram_sin, sizeof(dgram_sin))==-1) {
YARP_ERROR(Logger::get(), "could not connect udp client\n");
exit(1);
}
} else {
if (bind(s, (struct sockaddr *)&dgram_sin, sizeof(dgram_sin))==-1) {
YARP_ERROR(Logger::get(), "could not create udp server\n");
exit(1);
}
}
dgram_sockfd = s;
dgram = this;
int result = -1;
int local_port = -1;
struct sockaddr_in sin;
socklen_t len = sizeof(sin);
if (getsockname(dgram_sockfd, (struct sockaddr *)&sin, &len) == 0 &&
sin.sin_family == AF_INET) {
result = 0;
local_port = ntohs(sin.sin_port);
}
#endif
if (result!=0) {
YARP_ERROR(Logger::get(),"could not open datagram socket");
return false;
}
configureSystemBuffers();
#ifdef YARP_HAS_ACE
dgram->get_local_addr(localHandle);
YARP_DEBUG(Logger::get(),String("starting DGRAM entity on port number ") + NetType::toString(localHandle.get_port_number()));
localAddress = Contact("127.0.0.1",
localHandle.get_port_number());
#else
localAddress = Contact("127.0.0.1",local_port);
#endif
YARP_DEBUG(Logger::get(),String("Update: DGRAM from ") +
localAddress.toURI() +
" to " + remoteAddress.toURI());
allocate();
return true;
}
bool BootstrapServer::configFileBootstrap(yarp::os::Contact& contact,
bool configFileRequired,
bool mayEditConfigFile) {
Contact suggest = contact;
// see what address is lying around
Contact prev;
NameConfig conf;
if (conf.fromFile()) {
prev = conf.getAddress();
} else if (configFileRequired) {
fprintf(stderr,"Could not read configuration file %s\n",
conf.getConfigFileName().c_str());
return false;
}
// merge
if (prev.isValid()) {
if (suggest.getHost() == "...") {
suggest.setHost(prev.getHost());
}
if (suggest.getCarrier() == "...") {
suggest.setCarrier(prev.getCarrier());
}
if (suggest.getPort() == 0) {
suggest.setPort(prev.getPort());
}
}
if (suggest.getRegName() == "...") {
suggest.setName(conf.getNamespace());
}
// still something not set?
if (suggest.getPort() == 0) {
suggest.setPort(10000);
}
if (suggest.getHost() == "...") {
// should get my IP
suggest.setHost(conf.getHostName());
}
if (!configFileRequired) {
// finally, should make sure IP is local, and if not, correct it
if (!conf.isLocalName(suggest.getHost())) {
fprintf(stderr,"Overriding non-local address for name server\n");
suggest.setHost(conf.getHostName());
} else {
// Let's just check we're not a loopback
ConstString betterHost = conf.getHostName(false,suggest.getHost());
if (betterHost!=suggest.getHost()) {
fprintf(stderr,"Overriding loopback address for name server\n");
suggest.setHost(betterHost);
}
}
}
else
{
if (!conf.isLocalName(conf.getHostName())) {
fprintf(stderr,"The address written in config file doesn't belong any interface \n");
return false;
}
suggest.setHost(conf.getHostName());
}
bool changed = false;
if (prev.isValid()) {
changed = (prev.getHost() != suggest.getHost()) ||
(prev.getPort() != suggest.getPort()) ||
(conf.getMode() != "yarp" && conf.getMode() != "");
}
if (changed && !mayEditConfigFile) {
fprintf(stderr,"PROBLEM: need to change settings in %s\n",
conf.getConfigFileName().c_str());
fprintf(stderr," Current settings: host %s port %d family %s\n",
prev.getHost().c_str(), prev.getPort(),
(conf.getMode()=="")?"yarp":conf.getMode().c_str());
fprintf(stderr," Desired settings: host %s port %d family %s\n",
suggest.getHost().c_str(), suggest.getPort(), "yarp");
fprintf(stderr,"Please specify '--write' if it is ok to overwrite current settings, or\n");
if(!configFileRequired)
fprintf(stderr,"Please specify '--read' to use the current settings, or\n");
else
fprintf(stderr,"Please set an existing address in config file, or\n");
fprintf(stderr,"delete %s\n", conf.getConfigFileName().c_str());
return false;
}
bool shouldSave = changed || !prev.isValid();
if (shouldSave) {
// and save
conf.setAddress(suggest);
if (!conf.toFile()) {
fprintf(stderr,"Could not save configuration file %s\n",
conf.getConfigFileName().c_str());
}
}
contact = suggest;
return true;
}
Contact RosNameSpace::unregisterAdvanced(const ConstString& name, NameStore *store) {
NestedContact nc;
nc.fromString(name);
ConstString cat = nc.getCategory();
if (nc.getNestedName()!="") {
if (cat == "-1") {
Nodes& nodes = NameClient::getNameClient().getNodes();
Contact c = nodes.getURI(name);
c.setCarrier("rosrpc");
c = rosify(c);
Bottle cmd, reply;
cmd.clear();
cmd.addString("unregisterService");
cmd.addString(toRosNodeName(nc.getNodeName()));
cmd.addString(nc.getNestedName());
cmd.addString(c.toURI());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) return Contact();
} else if (cat == "+" || cat== "-") {
Bottle cmd, reply;
cmd.clear();
cmd.addString((cat=="+")?"unregisterPublisher":"unregisterSubscriber");
cmd.addString(toRosNodeName(nc.getNodeName()));
cmd.addString(nc.getNestedName());
Contact c;
if (store) {
c = rosify(store->query(nc.getNodeName()));
} else {
Nodes& nodes = NameClient::getNameClient().getNodes();
c = rosify(nodes.getParent(name));
}
cmd.addString(c.toString());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) return Contact();
}
return Contact();
}
// Remainder of method is supporting older /name+#/foo syntax
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());
}
if (sub_idx!=ConstString::npos) {
node = name.substr(0, sub_idx);
sub = name.substr(sub_idx+2, name.length());
}
if (node=="") {
node = name;
}
YARP_SPRINTF3(Logger::get(), debug, "Name [%s] sub [%s] pub [%s]\n",
name.c_str(), sub.c_str(), pub.c_str());
if (pub!="") {
NetworkBase::disconnect(name, ConstString("topic:/") + pub);
}
if (sub!="") {
NetworkBase::disconnect(ConstString("topic:/") + sub, name);
}
Contact contact = NetworkBase::queryName(name);
Bottle cmd, reply;
cmd.addString("unregisterPublisher");
cmd.addString(name);
cmd.addString("/yarp/registration");
Contact c("http", contact.getHost().c_str(), contact.getPort());
cmd.addString(c.toString());
bool ok = NetworkBase::write(getNameServerContact(),
cmd, reply);
if (!ok) return Contact();
return Contact();
}
