/**
* Start a query for a service.
*
* Remember service filter and send query to all connected ServiceManagers.
*/
void ServiceManager::startQuery(const ServiceFilter& filter, ResultSet<ServiceDescription>* listener) {
RScopeLock lock(_mutex);
_localQueries[filter] = listener;
Message* queryMsg = filter.toMessage();
queryMsg->putMeta("um.rpc.type", "startDiscovery");
queryMsg->putMeta("um.rpc.mgrId", _svcSub->getUUID());
_svcPub->send(queryMsg);
UM_LOG_INFO("Sending new query to %d ServiceManagers", _svcPub->waitForSubscribers(0));
delete queryMsg;
}
ServiceDescription ServiceManager::find(const ServiceFilter& svcFilter, int timeout) {
if(_svcPub->waitForSubscribers(1, timeout) < 1) {
// there is no other ServiceManager yet
UM_LOG_INFO("Failed to find another ServiceManager");
return ServiceDescription();
}
Message* findMsg = svcFilter.toMessage();
std::string reqId = UUID::getUUID();
findMsg->putMeta("um.rpc.type", "discover");
findMsg->putMeta("um.rpc.reqId", reqId.c_str());
findMsg->putMeta("um.rpc.mgrId", _svcSub->getUUID());
// Thread::sleepMs(1000);
if(_findRequests.find(reqId) != _findRequests.end())
UM_LOG_WARN("Find request %s already received", reqId.c_str());
_findRequests[reqId] = new Monitor();
_svcPub->send(findMsg);
delete findMsg;
RScopeLock lock(_mutex);
_findRequests[reqId]->wait(_mutex, timeout);
delete _findRequests[reqId];
_findRequests.erase(reqId);
if (_findResponses.find(reqId) == _findResponses.end()) {
UM_LOG_INFO("Failed to find %s", svcFilter.getServiceName().c_str());
return ServiceDescription();
}
// TODO: Remove other replies as they come in!
Message* foundMsg = _findResponses[reqId];
assert(foundMsg != NULL);
ServiceDescription svcDesc(foundMsg);
svcDesc._svcManager = this;
_findResponses.erase(reqId);
delete foundMsg;
return svcDesc;
}
/**
* Find local service matching filter.
*/
std::set<ServiceDescription> ServiceManager::findLocal(const ServiceFilter& svcFilter) {
std::set<ServiceDescription> foundSvcs;
std::map<intptr_t, ServiceDescription>::iterator svcDescIter = _localSvcDesc.begin();
while(svcDescIter != _localSvcDesc.end()) {
if (svcFilter.matches(svcDescIter->second)) {
foundSvcs.insert(svcDescIter->second);
UM_LOG_INFO("we have a match for %s", svcFilter.getServiceName().c_str());
}
svcDescIter++;
}
return foundSvcs;
}
void RTPSubscriber::removed(const PublisherStub& pub, const NodeStub& node) {
RScopeLock lock(_mutex);
int status;
uint16_t port=pub.getPort();
std::string ip=node.getIP();
// TODO: This fails for publishers added via different nodes
if (_pubs.find(pub.getUUID()) != _pubs.end())
_pubs.erase(pub.getUUID());
if (_domainPubs.count(pub.getDomain()) == 0)
return;
std::multimap<std::string, std::string>::iterator domIter = _domainPubs.find(pub.getDomain());
while(domIter != _domainPubs.end()) {
if (domIter->second == pub.getUUID()) {
_domainPubs.erase(domIter++);
} else {
domIter++;
}
}
if (_domainPubs.count(pub.getDomain()) == 0) {
UM_LOG_INFO("%s unsubscribing from %s (%s:%d)", SHORT_UUID(_uuid).c_str(), pub.getChannelName().c_str(), ip.c_str(), port);
if(_multicast && _pubs.size()==0) {
UM_LOG_INFO("%s: last publisher vanished and we are using multicast, leaving multicast group %s:%d now", SHORT_UUID(_uuid).c_str(), _multicastIP.c_str(), _port);
struct libre::sa maddr;
libre::sa_init(&maddr, AF_INET);
if((status=libre::sa_set_str(&maddr, _multicastIP.c_str(), _port)))
UM_LOG_ERR("%s: error %d in libre::sa_set_str(%s:%u): %s, not leaving multicast group", SHORT_UUID(_uuid).c_str(), status, _multicastIP.c_str(), _port, strerror(status));
else if(libre::udp_multicast_join((libre::udp_sock*)libre::rtp_sock(_rtp_socket), &maddr))
UM_LOG_ERR("%s: system not supporting multicast, not leaving multicast group (%s:%d)", SHORT_UUID(_uuid).c_str(), _multicastIP.c_str(), _port);
}
}
}
void RTPSubscriber::added(const PublisherStub& pub, const NodeStub& node) {
RScopeLock lock(_mutex);
int status;
uint16_t port=pub.getPort();
std::string ip=node.getIP();
if(_domainPubs.count(pub.getDomain()) == 0) {
UM_LOG_INFO("%s: subscribing to %s (%s:%d)", SHORT_UUID(_uuid).c_str(), pub.getChannelName().c_str(), ip.c_str(), port);
if(_multicast && _pubs.size()==0) {
UM_LOG_INFO("%s: first publisher found and we are using multicast, joining multicast group %s:%d now", SHORT_UUID(_uuid).c_str(), _multicastIP.c_str(), _port);
struct libre::sa maddr;
libre::sa_init(&maddr, AF_INET);
if((status=libre::sa_set_str(&maddr, _multicastIP.c_str(), _port)))
UM_LOG_ERR("%s: error %d in libre::sa_set_str(%s:%u): %s, ignoring publisher", SHORT_UUID(_uuid).c_str(), status, _multicastIP.c_str(), _port, strerror(status));
else if(libre::udp_multicast_join((libre::udp_sock*)libre::rtp_sock(_rtp_socket), &maddr))
UM_LOG_ERR("%s: system not supporting multicast, ignoring publisher (%s:%d)", SHORT_UUID(_uuid).c_str(), _multicastIP.c_str(), _port);
}
}
_pubs[pub.getUUID()] = pub;
_domainPubs.insert(std::make_pair(pub.getDomain(), pub.getUUID()));
}
void RTPPublisher::send(Message* msg) {
RScopeLock lock(_mutex);
int status = 0;
uint32_t timestamp = 0;
uint8_t payloadType = _payloadType;
uint16_t sequenceNumber = strTo<uint16_t>(msg->getMeta("um.sequenceNumber")); //only for internal use by umundo-bridge
bool marker = strTo<bool>(msg->getMeta("um.marker"));
if (!msg->getMeta("um.sequenceNumber").size())
sequenceNumber = _sequenceNumber++;
if (!msg->getMeta("um.marker").size())
marker = false;
std::string timestampIncrement = msg->getMeta("um.timestampIncrement");
if (!timestampIncrement.size())
timestampIncrement = _mandatoryMeta["um.timestampIncrement"];
if (msg->getMeta("um.timestamp").size()) { //mainly for internal use by umundo-bridge
timestamp = strTo<uint32_t>(msg->getMeta("um.timestamp"));
} else {
if (timestampIncrement.size()) {
timestamp = (_timestamp += strTo<uint32_t>(timestampIncrement));
} else {
timestamp = (_timestamp += _timestampIncrement);
}
}
if (msg->getMeta("um.payloadType").size()) //mainly for internal use by umundo-bridge
payloadType = strTo<uint8_t>(msg->getMeta("um.payloadType"));
//allocate buffer
libre::mbuf *mb = libre::mbuf_alloc(libre::RTP_HEADER_SIZE + msg->size());
//make room for rtp header
libre::mbuf_set_end(mb, libre::RTP_HEADER_SIZE);
libre::mbuf_skip_to_end(mb);
//write data
libre::mbuf_write_mem(mb, (const uint8_t*)msg->data(), msg->size());
//send data
typedef std::map<std::string, struct libre::sa>::iterator it_type;
for(it_type iterator = _destinations.begin(); iterator != _destinations.end(); iterator++) {
//reset buffer pos to start of data
libre::mbuf_set_pos(mb, libre::RTP_HEADER_SIZE);
if (sequenceNumber)
_rtp_socket->enc.seq = sequenceNumber;
if ((status = libre::rtp_send(_rtp_socket, &iterator->second, marker, payloadType, timestamp, mb)))
UM_LOG_INFO("%s: error in libre::rtp_send() for destination '%s': %s", SHORT_UUID(_uuid).c_str(), iterator->first.c_str(), strerror(status));
}
//cleanup
libre::mem_deref(mb);
}
void RTPPublisher::removed(const SubscriberStub& sub, const NodeStub& node) {
RScopeLock lock(_mutex);
int status;
std::string ip = sub.getIP();
uint16_t port = sub.getPort();
if (!ip.length()) //only use separate subscriber ip (for multicast support etc.), if specified
ip = node.getIP();
// do we now about this sub via this node?
bool subscriptionFound = false;
std::pair<_domainSubs_t::iterator, _domainSubs_t::iterator> subIter = _domainSubs.equal_range(sub.getUUID());
while(subIter.first != subIter.second) {
if (subIter.first->second.first.getUUID() == node.getUUID()) {
subscriptionFound = true;
break;
}
subIter.first++;
}
if (!subscriptionFound)
return;
UM_LOG_INFO("%s: lost a %s subscriber (%s:%u) for channel %s", SHORT_UUID(_uuid).c_str(), sub.isMulticast() ? "multicast" : "unicast", ip.c_str(), port, _channelName.c_str());
struct libre::sa addr;
libre::sa_init(&addr, AF_INET);
if ((status = libre::sa_set_str(&addr, ip.c_str(), port)))
UM_LOG_WARN("%s: error %d in libre::sa_set_str(%s:%u): %s", SHORT_UUID(_uuid).c_str(), status, ip.c_str(), port, strerror(status));
else
_destinations.erase(ip+":"+toStr(port));
if (_domainSubs.count(sub.getUUID()) == 1) { // about to vanish
if (_greeter != NULL) {
Publisher pub(Publisher(StaticPtrCast<PublisherImpl>(shared_from_this())));
_greeter->farewell(pub, sub);
}
_subs.erase(sub.getUUID());
}
_domainSubs.erase(subIter.first);
UMUNDO_SIGNAL(_pubLock);
}
void RTPPublisher::added(const SubscriberStub& sub, const NodeStub& node) {
RScopeLock lock(_mutex);
int status;
std::string ip = sub.getIP();
uint16_t port = sub.getPort();
if (!ip.length()) //only use separate subscriber ip (for multicast support etc.), if specified
ip = node.getIP();
// do we already now about this sub via this node?
std::pair<_domainSubs_t::iterator, _domainSubs_t::iterator> subIter = _domainSubs.equal_range(sub.getUUID());
while(subIter.first != subIter.second) {
if (subIter.first->second.first.getUUID() == node.getUUID())
return; // we already know about this sub from this node
subIter.first++;
}
UM_LOG_INFO("%s: received a new %s subscriber (%s:%u) for channel %s", SHORT_UUID(_uuid).c_str(), sub.isMulticast() ? "multicast" : "unicast", ip.c_str(), port, _channelName.c_str());
struct libre::sa addr;
libre::sa_init(&addr, AF_INET);
status = libre::sa_set_str(&addr, ip.c_str(), port);
if (status) {
UM_LOG_WARN("%s: error %d in libre::sa_set_str(%s:%u): %s", SHORT_UUID(_uuid).c_str(), status, ip.c_str(), port, strerror(status));
} else {
_destinations[ip + ":" + toStr(port)] = addr;
}
_subs[sub.getUUID()] = sub;
_domainSubs.insert(std::make_pair(sub.getUUID(), std::make_pair(node, sub)));
if (_greeter != NULL && _domainSubs.count(sub.getUUID()) == 1) {
// only perform greeting for first occurence of subscriber
Publisher pub(StaticPtrCast<PublisherImpl>(shared_from_this()));
_greeter->welcome(pub, sub);
}
UMUNDO_SIGNAL(_pubLock);
}
/**
* Another ServiceManager was added.
*
* Send all local continuous queries.
*/
void ServiceManager::welcome(Publisher& pub, const SubscriberStub& subStub) {
RScopeLock lock(_mutex);
UM_LOG_INFO("found remote ServiceManager - sending %d queries", _localQueries.size());
std::map<ServiceFilter, ResultSet<ServiceDescription>*>::iterator queryIter = _localQueries.begin();
while (queryIter != _localQueries.end()) {
Message* queryMsg = queryIter->first.toMessage();
queryMsg->putMeta("um.rpc.type", "startDiscovery");
queryMsg->putMeta("um.sub", subStub.getUUID());
queryMsg->putMeta("um.rpc.mgrId", _svcSub->getUUID());
_svcPub->send(queryMsg);
delete queryMsg;
queryIter++;
}
if (_pendingMessages.find(subStub.getUUID()) != _pendingMessages.end()) {
std::list<std::pair<uint64_t, Message*> >::iterator msgIter = _pendingMessages[subStub.getUUID()].begin();
while(msgIter != _pendingMessages[subStub.getUUID()].end()) {
_svcPub->send(msgIter->second);
delete msgIter->second;
}
_pendingMessages.erase(subStub.getUUID());
}
}
/**
* A ServiceManager was removed.
*/
void ServiceManager::farewell(Publisher& pub, const SubscriberStub& subStub) {
RScopeLock lock(_mutex);
UM_LOG_INFO("removed remote ServiceManager - notifying", _localQueries.size());
// did this publisher responded to our queries before?
if (_remoteSvcDesc.find(subStub.getUUID()) != _remoteSvcDesc.end()) {
// check all local queries if the remote services matched and notify about removal
std::map<ServiceFilter, ResultSet<ServiceDescription>*>::iterator queryIter = _localQueries.begin();
while(queryIter != _localQueries.end()) {
std::map<std::string, ServiceDescription>::iterator remoteSvcIter = _remoteSvcDesc[subStub.getUUID()].begin();
while(remoteSvcIter != _remoteSvcDesc[subStub.getUUID()].end()) {
if (queryIter->first.matches(remoteSvcIter->second)) {
queryIter->second->removed(remoteSvcIter->second);
}
remoteSvcIter++;
}
queryIter++;
}
}
_remoteSvcDesc.erase(subStub.getUUID());
}
void ServiceManager::receive(Message* msg) {
RScopeLock lock(_mutex);
// is this a response for one of our requests?
if (msg->getMeta().find("um.rpc.respId") != msg->getMeta().end()) {
std::string respId = msg->getMeta("um.rpc.respId");
if (_findRequests.find(respId) != _findRequests.end()) {
// put message into responses and signal waiting thread
_findResponses[respId] = new Message(*msg);
_findRequests[respId]->signal();
}
}
// is someone simply asking for a service via find?
if (msg->getMeta().find("um.rpc.type") != msg->getMeta().end() &&
msg->getMeta("um.rpc.type").compare("discover") == 0) {
ServiceFilter filter(msg);
std::set<ServiceDescription> foundSvcs = findLocal(filter);
if (foundSvcs.size() > 0) {
ServiceDescription svcDesc = (*(foundSvcs.begin()));
Message* foundMsg = svcDesc.toMessage();
foundMsg->setReceiver(msg->getMeta("um.rpc.mgrId"));
foundMsg->putMeta("um.rpc.respId", msg->getMeta("um.rpc.reqId"));
foundMsg->putMeta("um.rpc.mgrId", _svcSub->getUUID());
// if (_svcPub->isPublishingTo(msg->getMeta("um.rpc.mgrId"))) {
_svcPub->send(foundMsg);
// } else {
// // queue message and send in welcome
// _pendingMessages[msg->getMeta("um.rpc.mgrId")].push_back(std::make_pair(Thread::getTimeStampMs(), foundMsg));
// }
delete foundMsg;
}
}
// is this the start of a continuous query?
if (msg->getMeta().find("um.rpc.type") != msg->getMeta().end() &&
msg->getMeta("um.rpc.type").compare("startDiscovery") == 0) {
ServiceFilter filter(msg);
_remoteQueries[filter.getUUID()] = filter;
UM_LOG_INFO("Received query for '%s'", filter._svcName.c_str());
// do we have such a service?
std::set<ServiceDescription> foundSvcs = findLocal(filter);
std::set<ServiceDescription>::iterator svcDescIter = foundSvcs.begin();
while(svcDescIter != foundSvcs.end()) {
Message* foundMsg = svcDescIter->toMessage();
foundMsg->setReceiver(msg->getMeta("um.rpc.mgrId"));
foundMsg->putMeta("um.rpc.filterId", filter.getUUID());
foundMsg->putMeta("um.rpc.type", "discovered");
foundMsg->putMeta("um.rpc.mgrId", _svcSub->getUUID());
_svcPub->send(foundMsg);
delete foundMsg;
svcDescIter++;
}
}
// is this the end of a continuous query?
if (msg->getMeta().find("um.rpc.type") != msg->getMeta().end() &&
msg->getMeta("um.rpc.type").compare("stopDiscovery") == 0) {
ServiceFilter filter(msg);
if (_remoteQueries.find(filter.getUUID()) != _remoteQueries.end()) {
_remoteQueries.erase(filter.getUUID());
}
}
// is this a reply to a continuous service query?
if (msg->getMeta().find("um.rpc.type") != msg->getMeta().end() &&
(msg->getMeta("um.rpc.type").compare("discovered") == 0 ||
msg->getMeta("um.rpc.type").compare("vanished") == 0)) {
// _svcQueries comparator uses filter uuid
ServiceFilter keyFilter("");
keyFilter._uuid = msg->getMeta("um.rpc.filterId");
if (_localQueries.find(keyFilter) != _localQueries.end()) {
ResultSet<ServiceDescription>* listener = _localQueries[keyFilter];
assert(msg->getMeta("um.rpc.desc.channel").size() > 0);
assert(msg->getMeta("um.rpc.mgrId").size() > 0);
std::string svcChannel = msg->getMeta("um.rpc.desc.channel");
std::string managerId = msg->getMeta("um.rpc.mgrId");
if (_remoteSvcDesc.find(managerId) == _remoteSvcDesc.end() || _remoteSvcDesc[managerId].find(svcChannel) == _remoteSvcDesc[managerId].end()) {
_remoteSvcDesc[managerId][svcChannel] = ServiceDescription(msg);
_remoteSvcDesc[managerId][svcChannel]._svcManager = this;
}
assert(_remoteSvcDesc.find(managerId) != _remoteSvcDesc.end());
assert(_remoteSvcDesc[managerId].find(svcChannel) != _remoteSvcDesc[managerId].end());
if (msg->getMeta("um.rpc.type").compare("discovered") == 0) {
listener->added(_remoteSvcDesc[managerId][svcChannel]);
} else {
listener->removed(_remoteSvcDesc[managerId][svcChannel]);
_remoteSvcDesc[managerId].erase(svcChannel);
}
}
}
}