def test_subscribe_param_simple(self):
from rosmaster.registrations import RegistrationManager
from rosmaster.paramserver import ParamDictionary
# setup node and subscriber data
reg_manager = RegistrationManager(ThreadPoolMock())
param_server = ParamDictionary(reg_manager)
# subscribe to parameter that has not been set yet
self.last_update = None
self.assertEquals({}, param_server.subscribe_param('/foo', ('node1', 'http://node1:1')))
param_server.set_param('/foo', 1, notify_task=self.notify_task)
self.assertEquals([([('node1', 'http://node1:1')], '/foo/', 1), ], self.last_update)
# resubscribe
self.assertEquals(1, param_server.subscribe_param('/foo', ('node1', 'http://node1:1')))
param_server.set_param('/foo', 2, notify_task=self.notify_task)
self.assertEquals([([('node1', 'http://node1:1')], '/foo/', 2), ], self.last_update)
# resubscribe (test canonicalization of parameter name)
self.assertEquals(2, param_server.subscribe_param('/foo/', ('node1', 'http://node1:1')))
param_server.set_param('/foo', 'resub2', notify_task=self.notify_task)
self.assertEquals([([('node1', 'http://node1:1')], '/foo/', 'resub2'), ], self.last_update)
# change the URI
self.assertEquals('resub2', param_server.subscribe_param('/foo', ('node1', 'http://node1b:1')))
self.assertEquals('http://node1b:1', reg_manager.get_node('node1').api)
param_server.set_param('/foo', 3, notify_task=self.notify_task)
self.assertEquals([([('node1', 'http://node1b:1')], '/foo/', 3), ], self.last_update)
# multiple subscriptions to same param
self.assertEquals(3, param_server.subscribe_param('/foo', ('node2', 'http://node2:2')))
self.assertEquals('http://node2:2', reg_manager.get_node('node2').api)
param_server.set_param('/foo', 4, notify_task=self.notify_task)
self.assertEquals([([('node1', 'http://node1b:1'), ('node2', 'http://node2:2')], '/foo/', 4), ], self.last_update)
def __init__(self, num_workers=NUM_WORKERS):
"""ctor."""
self.uri = None
self.done = False
self.thread_pool = rosmaster.threadpool.MarkedThreadPool(num_workers)
# pub/sub/providers: dict { topicName : [publishers/subscribers names] }
self.ps_lock = threading.Condition(threading.Lock())
self.reg_manager = RegistrationManager(self.thread_pool)
# maintain refs to reg_manager fields
self.publishers = self.reg_manager.publishers
self.subscribers = self.reg_manager.subscribers
self.services = self.reg_manager.services
self.param_subscribers = self.reg_manager.param_subscribers
self.topics_types = {} #dict { topicName : type }
# parameter server dictionary
self.param_server = rosmaster.paramserver.ParamDictionary(
self.reg_manager)
# authorization
self.auth = ROSMasterAuth()
self.logger = self.auth.logger
def __init__(self, master_uri, num_works, timeout=None):
# Running status
self.shadow_uri = None
self.master_uri = master_uri
self._caller_id = '/Goblin/Shadow/unbind'
self._running = True
# Inner fields
socket.setdefaulttimeout(timeout)
self.thread_pool = MarkedThreadPool(num_works)
self.ps_lock = threading.Condition(threading.Lock())
self.reg_manager = RegistrationManager(self.thread_pool)
self.master_proxy = xmlrpclib.ServerProxy(master_uri)
# TODO: support all local nodes
self.services = self.reg_manager.services
def test_unsubscribe_param(self):
from rosmaster.registrations import RegistrationManager
from rosmaster.paramserver import ParamDictionary
# setup node and subscriber data
reg_manager = RegistrationManager(ThreadPoolMock())
param_server = ParamDictionary(reg_manager)
# basic test
self.last_update = None
self.assertEquals({},
param_server.subscribe_param(
'/foo', ('node1', 'http://node1:1')))
param_server.set_param('/foo', 1, notify_task=self.notify_task)
self.assertEquals([
([('node1', 'http://node1:1')], '/foo/', 1),
], self.last_update)
# - return value is actually generated by Registrations
code, msg, val = param_server.unsubscribe_param(
'/foo', ('node1', 'http://node1:1'))
self.assertEquals(1, code)
self.assertEquals(1, val)
self.last_update = None
param_server.set_param('/foo', 2, notify_task=self.notify_task)
self.assertEquals(None, self.last_update)
# - repeat the unsubscribe
code, msg, val = param_server.unsubscribe_param(
'/foo', ('node1', 'http://node1:1'))
self.assertEquals(1, code)
self.assertEquals(0, val)
self.last_update = None
param_server.set_param('/foo', 2, notify_task=self.notify_task)
self.assertEquals(None, self.last_update)
# verify that stale unsubscribe has no effect on active subscription
self.last_update = None
self.assertEquals({},
param_server.subscribe_param(
'/bar', ('barnode', 'http://barnode:1')))
param_server.set_param('/bar', 3, notify_task=self.notify_task)
self.assertEquals([
([('barnode', 'http://barnode:1')], '/bar/', 3),
], self.last_update)
code, msg, val = param_server.unsubscribe_param(
'/foo', ('barnode', 'http://notbarnode:1'))
self.assertEquals(1, code)
self.assertEquals(0, val)
param_server.set_param('/bar', 4, notify_task=self.notify_task)
self.assertEquals([
([('barnode', 'http://barnode:1')], '/bar/', 4),
], self.last_update)
def __init__(self, num_workers=NUM_WORKERS, rescue=False):
"""ctor."""
self.uri = None
self.done = False
self.thread_pool = rosmaster.threadpool.MarkedThreadPool(num_workers)
# pub/sub/providers: dict { topicName : [publishers/subscribers names] }
self.ps_lock = threading.Condition(threading.Lock())
# Pushyami : change to Registration Manager of master to enable rescue
self.reg_manager = RegistrationManager(self.thread_pool)
# maintain refs to reg_manager fields
self.publishers = self.reg_manager.publishers
self.subscribers = self.reg_manager.subscribers
self.services = self.reg_manager.services
self.param_subscribers = self.reg_manager.param_subscribers
self.topics_types = {} #dict { topicName : type }
# parameter server dictionary
self.param_server = rosmaster.paramserver.ParamDictionary(
self.reg_manager)
# Pushyami : Create RosRescue Object
self.rescue_opt = rescue
if self.rescue_opt:
self.rescue_obj = RosRescue(self.reg_manager, self.param_server)
import time
s = time.time()
self.rescue_obj.getLastSavedState()
e = time.time()
print(
"---------Time to recover previous state + writing updated state--------:"
)
print(e - s)
def __init__(self, master_uri, num_works, timeout=None):
# Running status
self.shadow_uri = None
self.master_uri = master_uri
self._caller_id = '/Goblin/Shadow/unbind'
self._running = True
# Inner fields
socket.setdefaulttimeout(timeout)
self.thread_pool = MarkedThreadPool(num_works)
self.ps_lock = threading.Condition(threading.Lock())
self.reg_manager = RegistrationManager(self.thread_pool)
self.master_proxy = xmlrpclib.ServerProxy(master_uri)
# TODO: support all local nodes
self.services = self.reg_manager.services
def __init__(self, num_workers=NUM_WORKERS):
"""ctor."""
self.uri = None
self.done = False
self.thread_pool = rosmaster.threadpool.MarkedThreadPool(num_workers)
# pub/sub/providers: dict { topicName : [publishers/subscribers names] }
self.ps_lock = threading.Condition(threading.Lock())
self.reg_manager = RegistrationManager(self.thread_pool)
# maintain refs to reg_manager fields
self.publishers = self.reg_manager.publishers
self.subscribers = self.reg_manager.subscribers
self.services = self.reg_manager.services
self.param_subscribers = self.reg_manager.param_subscribers
self.topics_types = {} #dict { topicName : type }
# parameter server dictionary
self.param_server = rosmaster.paramserver.ParamDictionary(self.reg_manager)
def test_subscribe_param_deletion(self):
from rosmaster.registrations import RegistrationManager
from rosmaster.paramserver import ParamDictionary
# setup node and subscriber data
reg_manager = RegistrationManager(ThreadPoolMock())
param_server = ParamDictionary(reg_manager)
# subscription to then delete parameter
self.assertEquals({}, param_server.subscribe_param('/foo', ('node1', 'http://node1:1')))
param_server.set_param('/foo', 1, notify_task=self.notify_task)
param_server.delete_param('/foo', notify_task=self.notify_task)
self.assertEquals([([('node1', 'http://node1:1')], '/foo/', {}), ], self.last_update)
# subscribe to and delete whole tree
gains = {'p': 'P', 'i': 'I', 'd' : 'D'}
self.assertEquals({}, param_server.subscribe_param('/gains', ('deltree', 'http://deltree:1')))
param_server.set_param('/gains', gains.copy(), notify_task=self.notify_task)
param_server.delete_param('/gains', notify_task=self.notify_task)
self.assertEquals([([('deltree', 'http://deltree:1')], '/gains/', {}), ], self.last_update)
# subscribe to and delete params within subtree
self.assertEquals({}, param_server.subscribe_param('/gains2', ('deltree2', 'http://deltree2:2')))
param_server.set_param('/gains2', gains.copy(), notify_task=self.notify_task)
param_server.delete_param('/gains2/p', notify_task=self.notify_task)
self.assertEquals([([('deltree2', 'http://deltree2:2')], '/gains2/p/', {}), ], self.last_update)
param_server.delete_param('/gains2/i', notify_task=self.notify_task)
self.assertEquals([([('deltree2', 'http://deltree2:2')], '/gains2/i/', {}), ], self.last_update)
param_server.delete_param('/gains2', notify_task=self.notify_task)
self.assertEquals([([('deltree2', 'http://deltree2:2')], '/gains2/', {}), ], self.last_update)
# delete parent tree
k = '/ns1/ns2/ns3/key'
self.assertEquals({}, param_server.subscribe_param(k, ('del_parent', 'http://del_parent:1')))
param_server.set_param(k, 1, notify_task=self.notify_task)
param_server.delete_param('/ns1/ns2', notify_task=self.notify_task)
self.assertEquals([([('del_parent', 'http://del_parent:1')], '/ns1/ns2/ns3/key/', {}), ], self.last_update)
def test_subscribe_param_tree(self):
from rosmaster.registrations import RegistrationManager
from rosmaster.paramserver import ParamDictionary
# setup node and subscriber data
reg_manager = RegistrationManager(ThreadPoolMock())
param_server = ParamDictionary(reg_manager)
# Test Parameter Tree Subscriptions
# simple case - subscribe and set whole tree
gains = {'p': 'P', 'i': 'I', 'd' : 'D'}
self.assertEquals({}, param_server.subscribe_param('/gains', ('ptnode', 'http://ptnode:1')))
param_server.set_param('/gains', gains.copy(), notify_task=self.notify_task)
self.assertEquals([([('ptnode', 'http://ptnode:1')], '/gains/', gains), ], self.last_update)
# - test with trailing slash
param_server.set_param('/gains/', gains.copy(), notify_task=self.notify_task)
self.assertEquals([([('ptnode', 'http://ptnode:1')], '/gains/', gains), ], self.last_update)
# change params within tree
param_server.set_param('/gains/p', 'P2', notify_task=self.notify_task)
self.assertEquals([([('ptnode', 'http://ptnode:1')], '/gains/p/', 'P2'), ], self.last_update)
param_server.set_param('/gains/i', 'I2', notify_task=self.notify_task)
self.assertEquals([([('ptnode', 'http://ptnode:1')], '/gains/i/', 'I2'), ], self.last_update)
# test overlapping subscriptions
self.assertEquals('P2', param_server.subscribe_param('/gains/p', ('ptnode2', 'http://ptnode2:2')))
param_server.set_param('/gains', gains.copy(), notify_task=self.notify_task)
self.assertEquals([([('ptnode', 'http://ptnode:1')], '/gains/', gains), \
([('ptnode2', 'http://ptnode2:2')], '/gains/p/', 'P'), \
], self.last_update)
# - retest with trailing slash on subscribe
self.last_update = None
self.assertEquals('P', param_server.subscribe_param('/gains/p/', ('ptnode2', 'http://ptnode2:2')))
param_server.set_param('/gains', gains.copy(), notify_task=self.notify_task)
self.assertEquals([([('ptnode', 'http://ptnode:1')], '/gains/', gains), \
([('ptnode2', 'http://ptnode2:2')], '/gains/p/', 'P'), \
], self.last_update)
# test with overlapping (change to sub param)
param_server.set_param('/gains/p', 'P3', notify_task=self.notify_task)
# - this is a bit overtuned as a more optimal ps could use one update
ptnode2 = ([('ptnode2', 'http://ptnode2:2')], '/gains/p/', 'P3')
ptnode = ([('ptnode', 'http://ptnode:1')], '/gains/p/', 'P3')
self.assertTrue(len(self.last_update) == 2)
self.assertTrue(ptnode2 in self.last_update)
self.assertTrue(ptnode in self.last_update)
# virtual deletion: subscribe to subparam, parameter tree reset
self.last_update = None
param_server.set_param('/gains2', gains.copy(), notify_task=self.notify_task)
self.assertEquals('P', param_server.subscribe_param('/gains2/p/', ('ptnode3', 'http://ptnode3:3')))
# - erase the sub parameters
param_server.set_param('/gains2', {}, notify_task=self.notify_task)
self.assertEquals([([('ptnode3', 'http://ptnode3:3')], '/gains2/p/', {}), ], self.last_update)
#Final test: test subscription to entire tree
self.last_update = None
param_server.delete_param('/gains')
param_server.delete_param('/gains2')
self.assertEquals({}, param_server.get_param('/'))
self.assertEquals({}, param_server.subscribe_param('/', ('allnode', 'http://allnode:1')))
param_server.set_param('/one', 1, notify_task=self.notify_task)
self.assertEquals([([('allnode', 'http://allnode:1')], '/one/', 1), ], self.last_update)
param_server.set_param('/two', 2, notify_task=self.notify_task)
self.assertEquals([([('allnode', 'http://allnode:1')], '/two/', 2), ], self.last_update)
param_server.set_param('/foo/bar', 'bar', notify_task=self.notify_task)
self.assertEquals([([('allnode', 'http://allnode:1')], '/foo/bar/', 'bar'), ], self.last_update)
def test_RegistrationManager_base(self):
import rosmaster.exceptions
from rosmaster.registrations import Registrations, RegistrationManager
threadpool = ThreadPoolMock()
rm = RegistrationManager(threadpool)
self.assert_(isinstance(rm.services, Registrations))
self.assertEquals(Registrations.SERVICE, rm.services.type)
self.assert_(isinstance(rm.param_subscribers, Registrations))
self.assertEquals(Registrations.PARAM_SUBSCRIPTIONS, rm.param_subscribers.type)
self.assert_(isinstance(rm.subscribers, Registrations))
self.assertEquals(Registrations.TOPIC_SUBSCRIPTIONS, rm.subscribers.type)
self.assert_(isinstance(rm.subscribers, Registrations))
self.assertEquals(Registrations.TOPIC_PUBLICATIONS, rm.publishers.type)
self.assert_(isinstance(rm.publishers, Registrations))
#test auto-clearing of registrations if node API changes
rm.register_publisher('pub1', 'caller1', 'http://one:1')
rm.register_publisher('pub1', 'caller2', 'http://two:1')
rm.register_publisher('pub1', 'caller3', 'http://three:1')
rm.register_subscriber('sub1', 'caller1', 'http://one:1')
rm.register_subscriber('sub1', 'caller2', 'http://two:1')
rm.register_subscriber('sub1', 'caller3', 'http://three:1')
rm.register_param_subscriber('p1', 'caller1', 'http://one:1')
rm.register_param_subscriber('p1', 'caller2', 'http://two:1')
rm.register_param_subscriber('p1', 'caller3', 'http://three:1')
rm.register_service('s1', 'caller1', 'http://one:1', 'rosrpc://one:1')
self.assertEquals('http://one:1', rm.get_node('caller1').api)
self.assertEquals('http://two:1', rm.get_node('caller2').api)
self.assertEquals('http://three:1', rm.get_node('caller3').api)
# - first, make sure that changing rosrpc URI does not erase state
rm.register_service('s1', 'caller1', 'http://one:1', 'rosrpc://oneB:1')
n = rm.get_node('caller1')
self.assertEquals(['pub1'], n.topic_publications)
self.assertEquals(['sub1'], n.topic_subscriptions)
self.assertEquals(['p1'], n.param_subscriptions)
self.assertEquals(['s1'], n.services)
self.assert_('http://one:1' in rm.publishers.get_apis('pub1'))
self.assert_('http://one:1' in rm.subscribers.get_apis('sub1'))
self.assert_('http://one:1' in rm.param_subscribers.get_apis('p1'))
self.assert_('http://one:1' in rm.services.get_apis('s1'))
# - also, make sure unregister does not erase state if API changed
rm.unregister_publisher('pub1', 'caller1', 'http://not:1')
self.assert_('http://one:1' in rm.publishers.get_apis('pub1'))
rm.unregister_subscriber('sub1', 'caller1', 'http://not:1')
self.assert_('http://one:1' in rm.subscribers.get_apis('sub1'))
rm.unregister_param_subscriber('p1', 'caller1', 'http://not:1')
self.assert_('http://one:1' in rm.param_subscribers.get_apis('p1'))
rm.unregister_service('sub1', 'caller1', 'rosrpc://not:1')
self.assert_('http://one:1' in rm.services.get_apis('s1'))
# erase caller1 sub/srvs/params via register_publisher
rm.register_publisher('pub1', 'caller1', 'http://newone:1')
self.assertEquals('http://newone:1', rm.get_node('caller1').api)
# - check node ref
n = rm.get_node('caller1')
self.assertEquals(['pub1'], n.topic_publications)
self.assertEquals([], n.services)
self.assertEquals([], n.topic_subscriptions)
self.assertEquals([], n.param_subscriptions)
# - checks publishers
self.assert_('http://newone:1' in rm.publishers.get_apis('pub1'))
# - checks subscribers
self.assert_(rm.subscribers.has_key('sub1'))
self.failIf('http://one:1' in rm.subscribers.get_apis('sub1'))
# - checks param subscribers
self.assert_(rm.param_subscribers.has_key('p1'))
self.failIf('http://one:1' in rm.param_subscribers.get_apis('p1'))
# erase caller2 pub/sub/params via register_service
# - initial state
self.assert_('http://two:1' in rm.publishers.get_apis('pub1'))
self.assert_('http://two:1' in rm.subscribers.get_apis('sub1'))
self.assert_('http://two:1' in rm.param_subscribers.get_apis('p1'))
# - change ownership of s1 to caller2
rm.register_service('s1', 'caller2', 'http://two:1', 'rosrpc://two:1')
self.assert_('http://two:1' in rm.services.get_apis('s1'))
self.assert_('http://two:1' in rm.publishers.get_apis('pub1'))
self.assert_('http://two:1' in rm.subscribers.get_apis('sub1'))
self.assert_('http://two:1' in rm.param_subscribers.get_apis('p1'))
rm.register_service('s1', 'caller2', 'http://newtwo:1', 'rosrpc://newtwo:1')
self.assertEquals('http://newone:1', rm.get_node('caller1').api)
# - check node ref
n = rm.get_node('caller2')
self.assertEquals([], n.topic_publications)
self.assertEquals(['s1'], n.services)
self.assertEquals([], n.topic_subscriptions)
self.assertEquals([], n.param_subscriptions)
# - checks publishers
self.assert_(rm.publishers.has_key('pub1'))
self.failIf('http://two:1' in rm.publishers.get_apis('pub1'))
# - checks subscribers
self.assert_(rm.subscribers.has_key('sub1'))
self.failIf('http://two:1' in rm.subscribers.get_apis('sub1'))
self.assertEquals([['sub1', ['caller3']]], rm.subscribers.get_state())
# - checks param subscribers
self.assert_(rm.param_subscribers.has_key('p1'))
self.failIf('http://two:1' in rm.param_subscribers.get_apis('p1'))
self.assertEquals([['p1', ['caller3']]], rm.param_subscribers.get_state())
def test_RegistrationManager_services(self):
from rosmaster.registrations import Registrations, RegistrationManager
rm = RegistrationManager(ThreadPoolMock())
self.assertEquals(None, rm.get_node('caller1'))
# do an unregister first, before service_api is initialized
code, msg, val = rm.unregister_service('s1', 'caller1', 'rosrpc://one:1234')
self.assertEquals(1, code)
self.assertEquals(0, val)
rm.register_service('s1', 'caller1', 'http://one:1234', 'rosrpc://one:1234')
self.assert_(rm.services.has_key('s1'))
self.assertEquals('rosrpc://one:1234', rm.services.get_service_api('s1'))
self.assertEquals('http://one:1234', rm.get_node('caller1').api)
self.assertEquals([['s1', ['caller1']]], rm.services.get_state())
# - verify that changed caller_api updates ref
rm.register_service('s1', 'caller1', 'http://oneB:1234', 'rosrpc://one:1234')
self.assert_(rm.services.has_key('s1'))
self.assertEquals('rosrpc://one:1234', rm.services.get_service_api('s1'))
self.assertEquals('http://oneB:1234', rm.get_node('caller1').api)
self.assertEquals([['s1', ['caller1']]], rm.services.get_state())
# - verify that changed service_api updates ref
rm.register_service('s1', 'caller1', 'http://oneB:1234', 'rosrpc://oneB:1234')
self.assert_(rm.services.has_key('s1'))
self.assertEquals('rosrpc://oneB:1234', rm.services.get_service_api('s1'))
self.assertEquals('http://oneB:1234', rm.get_node('caller1').api)
self.assertEquals([['s1', ['caller1']]], rm.services.get_state())
rm.register_service('s2', 'caller2', 'http://two:1234', 'rosrpc://two:1234')
self.assertEquals('http://two:1234', rm.get_node('caller2').api)
# - unregister should be noop if service api does not match
code, msg, val = rm.unregister_service('s2', 'caller2', 'rosrpc://b:1234')
self.assertEquals(1, code)
self.assertEquals(0, val)
self.assert_(rm.services.has_key('s2'))
self.assertEquals('http://two:1234', rm.get_node('caller2').api)
self.assertEquals('rosrpc://two:1234', rm.services.get_service_api('s2'))
# - unregister should be noop if service is unknown
code, msg, val = rm.unregister_service('unknown', 'caller2', 'rosrpc://two:1234')
self.assertEquals(1, code)
self.assertEquals(0, val)
self.assert_(rm.services.has_key('s2'))
self.assertEquals('http://two:1234', rm.get_node('caller2').api)
self.assertEquals('rosrpc://two:1234', rm.services.get_service_api('s2'))
# - unregister should clear all knowledge of caller2
code,msg, val = rm.unregister_service('s2', 'caller2', 'rosrpc://two:1234')
self.assertEquals(1, code)
self.assertEquals(1, val)
self.assert_(rm.services.has_key('s1'))
self.failIf(rm.services.has_key('s2'))
self.assertEquals(None, rm.get_node('caller2'))
code, msg, val = rm.unregister_service('s1', 'caller1', 'rosrpc://oneB:1234')
self.assertEquals(1, code)
self.assertEquals(1, val)
self.assert_(not rm.services.__nonzero__())
self.failIf(rm.services.has_key('s1'))
self.assertEquals(None, rm.get_node('caller1'))
def test_RegistrationManager_services(self):
from rosmaster.registrations import Registrations, RegistrationManager
rm = RegistrationManager(ThreadPoolMock())
self.assertEquals(None, rm.get_node('caller1'))
# do an unregister first, before service_api is initialized
code, msg, val = rm.unregister_service('s1', 'caller1',
'rosrpc://one:1234')
self.assertEquals(1, code)
self.assertEquals(0, val)
rm.register_service('s1', 'caller1', 'http://one:1234',
'rosrpc://one:1234')
self.assert_(rm.services.has_key('s1'))
self.assertEquals('rosrpc://one:1234',
rm.services.get_service_api('s1'))
self.assertEquals('http://one:1234', rm.get_node('caller1').api)
self.assertEquals([['s1', ['caller1']]], rm.services.get_state())
# - verify that changed caller_api updates ref
rm.register_service('s1', 'caller1', 'http://oneB:1234',
'rosrpc://one:1234')
self.assert_(rm.services.has_key('s1'))
self.assertEquals('rosrpc://one:1234',
rm.services.get_service_api('s1'))
self.assertEquals('http://oneB:1234', rm.get_node('caller1').api)
self.assertEquals([['s1', ['caller1']]], rm.services.get_state())
# - verify that changed service_api updates ref
rm.register_service('s1', 'caller1', 'http://oneB:1234',
'rosrpc://oneB:1234')
self.assert_(rm.services.has_key('s1'))
self.assertEquals('rosrpc://oneB:1234',
rm.services.get_service_api('s1'))
self.assertEquals('http://oneB:1234', rm.get_node('caller1').api)
self.assertEquals([['s1', ['caller1']]], rm.services.get_state())
rm.register_service('s2', 'caller2', 'http://two:1234',
'rosrpc://two:1234')
self.assertEquals('http://two:1234', rm.get_node('caller2').api)
# - unregister should be noop if service api does not match
code, msg, val = rm.unregister_service('s2', 'caller2',
'rosrpc://b:1234')
self.assertEquals(1, code)
self.assertEquals(0, val)
self.assert_(rm.services.has_key('s2'))
self.assertEquals('http://two:1234', rm.get_node('caller2').api)
self.assertEquals('rosrpc://two:1234',
rm.services.get_service_api('s2'))
# - unregister should be noop if service is unknown
code, msg, val = rm.unregister_service('unknown', 'caller2',
'rosrpc://two:1234')
self.assertEquals(1, code)
self.assertEquals(0, val)
self.assert_(rm.services.has_key('s2'))
self.assertEquals('http://two:1234', rm.get_node('caller2').api)
self.assertEquals('rosrpc://two:1234',
rm.services.get_service_api('s2'))
# - unregister should clear all knowledge of caller2
code, msg, val = rm.unregister_service('s2', 'caller2',
'rosrpc://two:1234')
self.assertEquals(1, code)
self.assertEquals(1, val)
self.assert_(rm.services.has_key('s1'))
self.failIf(rm.services.has_key('s2'))
self.assertEquals(None, rm.get_node('caller2'))
code, msg, val = rm.unregister_service('s1', 'caller1',
'rosrpc://oneB:1234')
self.assertEquals(1, code)
self.assertEquals(1, val)
self.assert_(not rm.services.__nonzero__())
self.failIf(rm.services.has_key('s1'))
self.assertEquals(None, rm.get_node('caller1'))
class ROSMasterHandler(object):
"""
XML-RPC handler for ROS master APIs.
API routines for the ROS Master Node. The Master Node is a
superset of the Slave Node and contains additional API methods for
creating and monitoring a graph of slave nodes.
By convention, ROS nodes take in caller_id as the first parameter
of any API call. The setting of this parameter is rarely done by
client code as ros::msproxy::MasterProxy automatically inserts
this parameter (see ros::client::getMaster()).
"""
def __init__(self, num_workers=NUM_WORKERS):
"""ctor."""
self.uri = None
self.done = False
self.thread_pool = rosmaster.threadpool.MarkedThreadPool(num_workers)
# pub/sub/providers: dict { topicName : [publishers/subscribers names] }
self.ps_lock = threading.Condition(threading.Lock())
self.reg_manager = RegistrationManager(self.thread_pool)
# maintain refs to reg_manager fields
self.publishers = self.reg_manager.publishers
self.subscribers = self.reg_manager.subscribers
self.services = self.reg_manager.services
self.param_subscribers = self.reg_manager.param_subscribers
self.topics_types = {} #dict { topicName : type }
# parameter server dictionary
self.param_server = rosmaster.paramserver.ParamDictionary(self.reg_manager)
def _shutdown(self, reason=''):
if self.thread_pool is not None:
self.thread_pool.join_all(wait_for_tasks=False, wait_for_threads=False)
self.thread_pool = None
self.done = True
def _ready(self, uri):
"""
Initialize the handler with the XMLRPC URI. This is a standard callback from the XmlRpcNode API.
@param uri: XML-RPC URI
@type uri: str
"""
self.uri = uri
def _ok(self):
return not self.done
###############################################################################
# EXTERNAL API
@apivalidate(0, (None, ))
def shutdown(self, caller_id, msg=''):
"""
Stop this server
@param caller_id: ROS caller id
@type caller_id: str
@param msg: a message describing why the node is being shutdown.
@type msg: str
@return: [code, msg, 0]
@rtype: [int, str, int]
"""
if msg:
print("shutdown request: %s" % msg, file=sys.stdout)
else:
print("shutdown requst", file=sys.stdout)
self._shutdown('external shutdown request from [%s]: %s'%(caller_id, msg))
return 1, "shutdown", 0
@apivalidate('')
def getUri(self, caller_id):
"""
Get the XML-RPC URI of this server.
@param caller_id str: ROS caller id
@return [int, str, str]: [1, "", xmlRpcUri]
"""
return 1, "", self.uri
@apivalidate(-1)
def getPid(self, caller_id):
"""
Get the PID of this server
@param caller_id: ROS caller id
@type caller_id: str
@return: [1, "", serverProcessPID]
@rtype: [int, str, int]
"""
return 1, "", os.getpid()
################################################################
# PARAMETER SERVER ROUTINES
@apivalidate(0, (non_empty_str('key'),))
def deleteParam(self, caller_id, key):
"""
Parameter Server: delete parameter
@param caller_id: ROS caller id
@type caller_id: str
@param key: parameter name
@type key: str
@return: [code, msg, 0]
@rtype: [int, str, int]
"""
try:
key = resolve_name(key, caller_id)
self.param_server.delete_param(key, self._notify_param_subscribers)
mloginfo("-PARAM [%s] by %s",key, caller_id)
return 1, "parameter %s deleted"%key, 0
except KeyError as e:
return -1, "parameter [%s] is not set"%key, 0
@apivalidate(0, (non_empty_str('key'), not_none('value')))
def setParam(self, caller_id, key, value):
"""
Parameter Server: set parameter. NOTE: if value is a
dictionary it will be treated as a parameter tree, where key
is the parameter namespace. For example:::
{'x':1,'y':2,'sub':{'z':3}}
will set key/x=1, key/y=2, and key/sub/z=3. Furthermore, it
will replace all existing parameters in the key parameter
namespace with the parameters in value. You must set
parameters individually if you wish to perform a union update.
@param caller_id: ROS caller id
@type caller_id: str
@param key: parameter name
@type key: str
@param value: parameter value.
@type value: XMLRPCLegalValue
@return: [code, msg, 0]
@rtype: [int, str, int]
"""
key = resolve_name(key, caller_id)
self.param_server.set_param(key, value, self._notify_param_subscribers)
mloginfo("+PARAM [%s] by %s",key, caller_id)
return 1, "parameter %s set"%key, 0
@apivalidate(0, (non_empty_str('key'),))
def getParam(self, caller_id, key):
"""
Retrieve parameter value from server.
@param caller_id: ROS caller id
@type caller_id: str
@param key: parameter to lookup. If key is a namespace,
getParam() will return a parameter tree.
@type key: str
getParam() will return a parameter tree.
@return: [code, statusMessage, parameterValue]. If code is not
1, parameterValue should be ignored. If key is a namespace,
the return value will be a dictionary, where each key is a
parameter in that namespace. Sub-namespaces are also
represented as dictionaries.
@rtype: [int, str, XMLRPCLegalValue]
"""
try:
key = resolve_name(key, caller_id)
return 1, "Parameter [%s]"%key, self.param_server.get_param(key)
except KeyError as e:
return -1, "Parameter [%s] is not set"%key, 0
@apivalidate(0, (non_empty_str('key'),))
def searchParam(self, caller_id, key):
"""
Search for parameter key on parameter server. Search starts in caller's namespace and proceeds
upwards through parent namespaces until Parameter Server finds a matching key.
searchParam's behavior is to search for the first partial match.
For example, imagine that there are two 'robot_description' parameters::
/robot_description
/robot_description/arm
/robot_description/base
/pr2/robot_description
/pr2/robot_description/base
If I start in the namespace /pr2/foo and search for
'robot_description', searchParam will match
/pr2/robot_description. If I search for 'robot_description/arm'
it will return /pr2/robot_description/arm, even though that
parameter does not exist (yet).
@param caller_id str: ROS caller id
@type caller_id: str
@param key: parameter key to search for.
@type key: str
@return: [code, statusMessage, foundKey]. If code is not 1, foundKey should be
ignored.
@rtype: [int, str, str]
"""
search_key = self.param_server.search_param(caller_id, key)
if search_key:
return 1, "Found [%s]"%search_key, search_key
else:
return -1, "Cannot find parameter [%s] in an upwards search"%key, ''
@apivalidate(0, (is_api('caller_api'), non_empty_str('key'),))
def subscribeParam(self, caller_id, caller_api, key):
"""
Retrieve parameter value from server and subscribe to updates to that param. See
paramUpdate() in the Node API.
@param caller_id str: ROS caller id
@type caller_id: str
@param key: parameter to lookup.
@type key: str
@param caller_api: API URI for paramUpdate callbacks.
@type caller_api: str
@return: [code, statusMessage, parameterValue]. If code is not
1, parameterValue should be ignored. parameterValue is an empty dictionary if the parameter
has not been set yet.
@rtype: [int, str, XMLRPCLegalValue]
"""
key = resolve_name(key, caller_id)
try:
# ps_lock has precedence and is required due to
# potential self.reg_manager modification
self.ps_lock.acquire()
val = self.param_server.subscribe_param(key, (caller_id, caller_api))
finally:
self.ps_lock.release()
return 1, "Subscribed to parameter [%s]"%key, val
@apivalidate(0, (is_api('caller_api'), non_empty_str('key'),))
def unsubscribeParam(self, caller_id, caller_api, key):
"""
Retrieve parameter value from server and subscribe to updates to that param. See
paramUpdate() in the Node API.
@param caller_id str: ROS caller id
@type caller_id: str
@param key: parameter to lookup.
@type key: str
@param caller_api: API URI for paramUpdate callbacks.
@type caller_api: str
@return: [code, statusMessage, numUnsubscribed].
If numUnsubscribed is zero it means that the caller was not subscribed to the parameter.
@rtype: [int, str, int]
"""
key = resolve_name(key, caller_id)
try:
# ps_lock is required due to potential self.reg_manager modification
self.ps_lock.acquire()
retval = self.param_server.unsubscribe_param(key, (caller_id, caller_api))
finally:
self.ps_lock.release()
return 1, "Unsubscribe to parameter [%s]"%key, 1
@apivalidate(False, (non_empty_str('key'),))
def hasParam(self, caller_id, key):
"""
Check if parameter is stored on server.
@param caller_id str: ROS caller id
@type caller_id: str
@param key: parameter to check
@type key: str
@return: [code, statusMessage, hasParam]
@rtype: [int, str, bool]
"""
key = resolve_name(key, caller_id)
if self.param_server.has_param(key):
return 1, key, True
else:
return 1, key, False
@apivalidate([])
def getParamNames(self, caller_id):
"""
Get list of all parameter names stored on this server.
This does not adjust parameter names for caller's scope.
@param caller_id: ROS caller id
@type caller_id: str
@return: [code, statusMessage, parameterNameList]
@rtype: [int, str, [str]]
"""
return 1, "Parameter names", self.param_server.get_param_names()
##################################################################################
# NOTIFICATION ROUTINES
def _notify(self, registrations, task, key, value, node_apis):
"""
Generic implementation of callback notification
@param registrations: Registrations
@type registrations: L{Registrations}
@param task: task to queue
@type task: fn
@param key: registration key
@type key: str
@param value: value to pass to task
@type value: Any
"""
# cache thread_pool for thread safety
thread_pool = self.thread_pool
if not thread_pool:
return
try:
for node_api in node_apis:
# use the api as a marker so that we limit one thread per subscriber
thread_pool.queue_task(node_api, task, (node_api, key, value))
except KeyError:
_logger.warn('subscriber data stale (key [%s], listener [%s]): node API unknown'%(key, s))
def _notify_param_subscribers(self, updates):
"""
Notify parameter subscribers of new parameter value
@param updates [([str], str, any)*]: [(subscribers, param_key, param_value)*]
@param param_value str: parameter value
"""
# cache thread_pool for thread safety
thread_pool = self.thread_pool
if not thread_pool:
return
for subscribers, key, value in updates:
# use the api as a marker so that we limit one thread per subscriber
for caller_id, caller_api in subscribers:
self.thread_pool.queue_task(caller_api, self.param_update_task, (caller_id, caller_api, key, value))
def param_update_task(self, caller_id, caller_api, param_key, param_value):
"""
Contact api.paramUpdate with specified parameters
@param caller_id: caller ID
@type caller_id: str
@param caller_api: XML-RPC URI of node to contact
@type caller_api: str
@param param_key: parameter key to pass to node
@type param_key: str
@param param_value: parameter value to pass to node
@type param_value: str
"""
mloginfo("paramUpdate[%s]", param_key)
code, _, _ = xmlrpcapi(caller_api).paramUpdate('/master', param_key, param_value)
if code == -1:
try:
# ps_lock is required due to potential self.reg_manager modification
self.ps_lock.acquire()
# reverse lookup to figure out who we just called
matches = self.reg_manager.reverse_lookup(caller_api)
for m in matches:
retval = self.param_server.unsubscribe_param(param_key, (m.id, caller_api))
finally:
self.ps_lock.release()
def _notify_topic_subscribers(self, topic, pub_uris, sub_uris):
"""
Notify subscribers with new publisher list
@param topic: name of topic
@type topic: str
@param pub_uris: list of URIs of publishers.
@type pub_uris: [str]
"""
self._notify(self.subscribers, publisher_update_task, topic, pub_uris, sub_uris)
##################################################################################
# SERVICE PROVIDER
@apivalidate(0, ( is_service('service'), is_api('service_api'), is_api('caller_api')))
def registerService(self, caller_id, service, service_api, caller_api):
"""
Register the caller as a provider of the specified service.
@param caller_id str: ROS caller id
@type caller_id: str
@param service: Fully-qualified name of service
@type service: str
@param service_api: Service URI
@type service_api: str
@param caller_api: XML-RPC URI of caller node
@type caller_api: str
@return: (code, message, ignore)
@rtype: (int, str, int)
"""
try:
self.ps_lock.acquire()
self.reg_manager.register_service(service, caller_id, caller_api, service_api)
mloginfo("+SERVICE [%s] %s %s", service, caller_id, caller_api)
finally:
self.ps_lock.release()
return 1, "Registered [%s] as provider of [%s]"%(caller_id, service), 1
@apivalidate(0, (is_service('service'),))
def lookupService(self, caller_id, service):
"""
Lookup all provider of a particular service.
@param caller_id str: ROS caller id
@type caller_id: str
@param service: fully-qualified name of service to lookup.
@type: service: str
@return: (code, message, serviceUrl). service URL is provider's
ROSRPC URI with address and port. Fails if there is no provider.
@rtype: (int, str, str)
"""
try:
self.ps_lock.acquire()
service_url = self.services.get_service_api(service)
finally:
self.ps_lock.release()
if service_url:
return 1, "rosrpc URI: [%s]"%service_url, service_url
else:
return -1, "no provider", ''
@apivalidate(0, ( is_service('service'), is_api('service_api')))
def unregisterService(self, caller_id, service, service_api):
"""
Unregister the caller as a provider of the specified service.
@param caller_id str: ROS caller id
@type caller_id: str
@param service: Fully-qualified name of service
@type service: str
@param service_api: API URI of service to unregister. Unregistration will only occur if current
registration matches.
@type service_api: str
@return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1).
If this is zero it means that the caller was not registered as a service provider.
The call still succeeds as the intended final state is reached.
@rtype: (int, str, int)
"""
try:
self.ps_lock.acquire()
retval = self.reg_manager.unregister_service(service, caller_id, service_api)
mloginfo("-SERVICE [%s] %s %s", service, caller_id, service_api)
return retval
finally:
self.ps_lock.release()
##################################################################################
# PUBLISH/SUBSCRIBE
@apivalidate(0, ( is_topic('topic'), valid_type_name('topic_type'), is_api('caller_api')))
def registerSubscriber(self, caller_id, topic, topic_type, caller_api):
"""
Subscribe the caller to the specified topic. In addition to receiving
a list of current publishers, the subscriber will also receive notifications
of new publishers via the publisherUpdate API.
@param caller_id: ROS caller id
@type caller_id: str
@param topic str: Fully-qualified name of topic to subscribe to.
@param topic_type: Datatype for topic. Must be a package-resource name, i.e. the .msg name.
@type topic_type: str
@param caller_api: XML-RPC URI of caller node for new publisher notifications
@type caller_api: str
@return: (code, message, publishers). Publishers is a list of XMLRPC API URIs
for nodes currently publishing the specified topic.
@rtype: (int, str, [str])
"""
#NOTE: subscribers do not get to set topic type
try:
self.ps_lock.acquire()
self.reg_manager.register_subscriber(topic, caller_id, caller_api)
# ROS 1.1: subscriber can now set type if it is not already set
# - don't let '*' type squash valid typing
if not topic in self.topics_types and topic_type != rosgraph.names.ANYTYPE:
self.topics_types[topic] = topic_type
mloginfo("+SUB [%s] %s %s",topic, caller_id, caller_api)
pub_uris = self.publishers.get_apis(topic)
finally:
self.ps_lock.release()
return 1, "Subscribed to [%s]"%topic, pub_uris
@apivalidate(0, (is_topic('topic'), is_api('caller_api')))
def unregisterSubscriber(self, caller_id, topic, caller_api):
"""
Unregister the caller as a publisher of the topic.
@param caller_id: ROS caller id
@type caller_id: str
@param topic: Fully-qualified name of topic to unregister.
@type topic: str
@param caller_api: API URI of service to unregister. Unregistration will only occur if current
registration matches.
@type caller_api: str
@return: (code, statusMessage, numUnsubscribed).
If numUnsubscribed is zero it means that the caller was not registered as a subscriber.
The call still succeeds as the intended final state is reached.
@rtype: (int, str, int)
"""
try:
self.ps_lock.acquire()
retval = self.reg_manager.unregister_subscriber(topic, caller_id, caller_api)
mloginfo("-SUB [%s] %s %s",topic, caller_id, caller_api)
return retval
finally:
self.ps_lock.release()
@apivalidate(0, ( is_topic('topic'), valid_type_name('topic_type'), is_api('caller_api')))
def registerPublisher(self, caller_id, topic, topic_type, caller_api):
"""
Register the caller as a publisher the topic.
@param caller_id: ROS caller id
@type caller_id: str
@param topic: Fully-qualified name of topic to register.
@type topic: str
@param topic_type: Datatype for topic. Must be a
package-resource name, i.e. the .msg name.
@type topic_type: str
@param caller_api str: ROS caller XML-RPC API URI
@type caller_api: str
@return: (code, statusMessage, subscriberApis).
List of current subscribers of topic in the form of XMLRPC URIs.
@rtype: (int, str, [str])
"""
#NOTE: we need topic_type for getPublishedTopics.
try:
self.ps_lock.acquire()
self.reg_manager.register_publisher(topic, caller_id, caller_api)
# don't let '*' type squash valid typing
if topic_type != rosgraph.names.ANYTYPE or not topic in self.topics_types:
self.topics_types[topic] = topic_type
pub_uris = self.publishers.get_apis(topic)
sub_uris = self.subscribers.get_apis(topic)
self._notify_topic_subscribers(topic, pub_uris, sub_uris)
mloginfo("+PUB [%s] %s %s",topic, caller_id, caller_api)
sub_uris = self.subscribers.get_apis(topic)
finally:
self.ps_lock.release()
return 1, "Registered [%s] as publisher of [%s]"%(caller_id, topic), sub_uris
@apivalidate(0, (is_topic('topic'), is_api('caller_api')))
def unregisterPublisher(self, caller_id, topic, caller_api):
"""
Unregister the caller as a publisher of the topic.
@param caller_id: ROS caller id
@type caller_id: str
@param topic: Fully-qualified name of topic to unregister.
@type topic: str
@param caller_api str: API URI of service to
unregister. Unregistration will only occur if current
registration matches.
@type caller_api: str
@return: (code, statusMessage, numUnregistered).
If numUnregistered is zero it means that the caller was not registered as a publisher.
The call still succeeds as the intended final state is reached.
@rtype: (int, str, int)
"""
try:
self.ps_lock.acquire()
retval = self.reg_manager.unregister_publisher(topic, caller_id, caller_api)
if retval[VAL]:
self._notify_topic_subscribers(topic, self.publishers.get_apis(topic), self.subscribers.get_apis(topic))
mloginfo("-PUB [%s] %s %s",topic, caller_id, caller_api)
finally:
self.ps_lock.release()
return retval
##################################################################################
# GRAPH STATE APIS
@apivalidate('', (valid_name('node'),))
def lookupNode(self, caller_id, node_name):
"""
Get the XML-RPC URI of the node with the associated
name/caller_id. This API is for looking information about
publishers and subscribers. Use lookupService instead to lookup
ROS-RPC URIs.
@param caller_id: ROS caller id
@type caller_id: str
@param node: name of node to lookup
@type node: str
@return: (code, msg, URI)
@rtype: (int, str, str)
"""
try:
self.ps_lock.acquire()
node = self.reg_manager.get_node(node_name)
if node is not None:
retval = 1, "node api", node.api
else:
retval = -1, "unknown node [%s]"%node_name, ''
finally:
self.ps_lock.release()
return retval
@apivalidate(0, (empty_or_valid_name('subgraph'),))
def getPublishedTopics(self, caller_id, subgraph):
"""
Get list of topics that can be subscribed to. This does not return topics that have no publishers.
See L{getSystemState()} to get more comprehensive list.
@param caller_id: ROS caller id
@type caller_id: str
@param subgraph: Restrict topic names to match within the specified subgraph. Subgraph namespace
is resolved relative to the caller's namespace. Use '' to specify all names.
@type subgraph: str
@return: (code, msg, [[topic1, type1]...[topicN, typeN]])
@rtype: (int, str, [[str, str],])
"""
try:
self.ps_lock.acquire()
# force subgraph to be a namespace with trailing slash
if subgraph and subgraph[-1] != rosgraph.names.SEP:
subgraph = subgraph + rosgraph.names.SEP
#we don't bother with subscribers as subscribers don't report topic types. also, the intended
#use case is for subscribe-by-topic-type
retval = [[t, self.topics_types[t]] for t in self.publishers.iterkeys() if t.startswith(subgraph)]
finally:
self.ps_lock.release()
return 1, "current topics", retval
@apivalidate([])
def getTopicTypes(self, caller_id):
"""
Retrieve list topic names and their types.
@param caller_id: ROS caller id
@type caller_id: str
@rtype: (int, str, [[str,str]] )
@return: (code, statusMessage, topicTypes). topicTypes is a list of [topicName, topicType] pairs.
"""
try:
self.ps_lock.acquire()
retval = list(self.topics_types.items())
finally:
self.ps_lock.release()
return 1, "current system state", retval
@apivalidate([[],[], []])
def getSystemState(self, caller_id):
"""
Retrieve list representation of system state (i.e. publishers, subscribers, and services).
@param caller_id: ROS caller id
@type caller_id: str
@rtype: (int, str, [[str,[str]], [str,[str]], [str,[str]]])
@return: (code, statusMessage, systemState).
System state is in list representation::
[publishers, subscribers, services].
publishers is of the form::
[ [topic1, [topic1Publisher1...topic1PublisherN]] ... ]
subscribers is of the form::
[ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ]
services is of the form::
[ [service1, [service1Provider1...service1ProviderN]] ... ]
"""
edges = []
try:
self.ps_lock.acquire()
retval = [r.get_state() for r in (self.publishers, self.subscribers, self.services)]
finally:
self.ps_lock.release()
return 1, "current system state", retval
def test_RegistrationManager_topic_pub(self):
from rosmaster.registrations import Registrations, RegistrationManager
rm = RegistrationManager(ThreadPoolMock())
self.subtest_RegistrationManager(rm, rm.publishers,
rm.register_publisher,
rm.unregister_publisher)
def test_RegistrationManager_param_sub(self):
from rosmaster.registrations import Registrations, RegistrationManager
rm = RegistrationManager(ThreadPoolMock())
self.subtest_RegistrationManager(rm, rm.param_subscribers,
rm.register_param_subscriber,
rm.unregister_param_subscriber)
class GoblinShadowHandler(object):
"""
Goblin Shadow handler is a client-side local proxy of the original ROS Master.
This additional intermediary provides some key features with slight overhead.
"""
def __init__(self, master_uri, num_works, timeout=None):
# Running status
self.shadow_uri = None
self.master_uri = master_uri
self._caller_id = '/Goblin/Shadow/unbind'
self._running = True
# Inner fields
socket.setdefaulttimeout(timeout)
self.thread_pool = MarkedThreadPool(num_works)
self.ps_lock = threading.Condition(threading.Lock())
self.reg_manager = RegistrationManager(self.thread_pool)
self.master_proxy = xmlrpclib.ServerProxy(master_uri)
# TODO: support all local nodes
self.services = self.reg_manager.services
# TODO: support local param-server caches
# APIs for running XML-RPC
def _shutdown(self, reason=''):
"""
Forked from ROSMasterHandler
@param reason:
@return:
"""
if self.thread_pool is not None:
self.thread_pool.join_all(wait_for_tasks=False, wait_for_threads=False)
self.thread_pool = None
self._running = False
def _ready(self, uri):
"""
Impl standard XML-RPC API to update URI
@param uri:
@return:
"""
self.shadow_uri = uri
self._caller_id = '/Goblin/Shadow/unbind_{}'.format(uri)
def _dispatch(self, method, params):
"""
Dispatch not-covered method to original ROS Master
"""
logger.info('Required: {}{}'.format(method, params))
if method in METHODS:
logger.debug('-- LOCAL')
status, msg, value = METHODS[method](self, *params)
logger.debug('>> LOCAL {}'.format((status, msg, value)))
else:
logger.debug('-- REMOTE: {!r}'.format(getattr(self.master_proxy, method)))
status, msg, value = getattr(self.master_proxy, method)(*params)
logger.debug('>> REMOTE: {}'.format((status, msg, value)))
return status, msg, value
def is_running(self):
return self._running
# private methods for running Shadow
def _lookup_local_service(self, service):
uri = None
with self.ps_lock:
uri = self.services.get_service_api(service)
return uri
def _lookup_remote_service(self, service):
return self.master_proxy.lookupService(self._caller_id, service)[2]
def _reg_local_service(self, caller_id, service, service_api, caller_api):
logger.info('-- Reg Local {} {}'.format(service, service_api))
with self.ps_lock:
self.reg_manager.register_service(service, caller_id, caller_api, service_api)
logger.info("+SERVICE [%s] %s %s", service, caller_id, caller_api)
def _reg_remote_service(self, *args):
r = self.master_proxy.registerService(*args)
logger.info('-- Reg Remote {}'.format(r))
def _unreg_local_service(self, caller_id, service, service_api):
with self.ps_lock:
self.reg_manager.unregister_service(service, caller_id, service_api)
def _unreg_remote_service(self, *args):
r = self.master_proxy.unregisterService(*args)
logger.info('-- Unreg Remote {}'.format(r))
# Overwritten APIs
@apivalidate(0, (is_service('service'),))
@overwrite
def lookupService(self, caller_id, service):
"""
Lookup all provider of a particular service in following rules:
1. Local/Auto/Unlabeled
1. Use local services if possible.
2. Otherwise, use remote services.
2. Remote
1. If average time cost belows the balance-curve, use remote services.
2. Otherwise, use remote services if possible.
@param caller_id str: ROS caller id
@type caller_id: str
@param service: fully-qualified name of service to lookup.
@type: service: str
@return: (code, message, serviceUrl). service URL is provider's
ROSRPC URI with address and port. Fails if there is no provider.
@rtype: (int, str, str)
"""
uri = None
cfg = swcfg.services.get_config(service)
def skeleton(x, y):
fallback_p = const(bool(cfg.fallback))
# TODO: support fallback list
return on_fallback(x, y, fallback_p, success_uri)
# local?
if swcfg.localtype == cfg.priority:
fn = skeleton(self._lookup_local_service, self._lookup_remote_service)
else:
fn = skeleton(self._lookup_remote_service, self._lookup_local_service)
uri = fn(service)
if success_uri(uri):
return ResponseFactory.uri_found(service, uri).pack()
else:
return ResponseFactory.unknown_service(service).pack()
@apivalidate(0, (is_service('service'), is_api('service_api'), is_api('caller_api')))
@overwrite
def registerService(self, caller_id, service, service_api, caller_api):
"""
Forked from ROSMasterHandler.
Register the caller as a provider of the specified service.
0. If service is `remote-only`, register with ROS Master
1. If Service is `local-only`, register with current shadow instance.
2. Otherwise, register with both sides.
@param caller_id str: ROS caller id
@type caller_id: str
@param service: Fully-qualified name of service
@type service: str
@param service_api: Service URI
@type service_api: str
@param caller_api: XML-RPC URI of caller node
@type caller_api: str
@return: (code, message, ignore)
@rtype: (int, str, int)
"""
cfg = swcfg.services.get_config(service)
if not cfg.is_local_only():
self._reg_remote_service(caller_id, service, service_api, caller_api)
if not cfg.is_remote_only():
self._reg_local_service(caller_id, service, service_api, caller_api)
return ResponseFactory.service_reg(caller_id, service).pack()
@apivalidate(0, (is_service('service'), is_api('service_api')))
@overwrite
def unregisterService(self, caller_id, service, service_api):
"""
Forked from ROSMasterHandler.
Unregister the caller as a provider of the specified service.
@param caller_id str: ROS caller id
@type caller_id: str
@param service: Fully-qualified name of service
@type service: str
@param service_api: API URI of service to unregister. Unregistration will only occur if current
registration matches.
@type service_api: str
@return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1).
If this is zero it means that the caller was not registered as a service provider.
The call still succeeds as the intended final state is reached.
@rtype: (int, str, int)
"""
cfg = swcfg.services.get_config(service)
if not cfg.is_local_only():
self._unreg_remote_service(caller_id, service, service_api)
if not cfg.is_remote_only():
self._unreg_local_service(caller_id, service, service_api)
return ResponseFactory.service_unreg(caller_id, service).pack()
def test_RegistrationManager_base(self):
import rosmaster.exceptions
from rosmaster.registrations import Registrations, RegistrationManager
threadpool = ThreadPoolMock()
rm = RegistrationManager(threadpool)
self.assert_(isinstance(rm.services, Registrations))
self.assertEquals(Registrations.SERVICE, rm.services.type)
self.assert_(isinstance(rm.param_subscribers, Registrations))
self.assertEquals(Registrations.PARAM_SUBSCRIPTIONS,
rm.param_subscribers.type)
self.assert_(isinstance(rm.subscribers, Registrations))
self.assertEquals(Registrations.TOPIC_SUBSCRIPTIONS,
rm.subscribers.type)
self.assert_(isinstance(rm.subscribers, Registrations))
self.assertEquals(Registrations.TOPIC_PUBLICATIONS, rm.publishers.type)
self.assert_(isinstance(rm.publishers, Registrations))
#test auto-clearing of registrations if node API changes
rm.register_publisher('pub1', 'caller1', 'http://one:1')
rm.register_publisher('pub1', 'caller2', 'http://two:1')
rm.register_publisher('pub1', 'caller3', 'http://three:1')
rm.register_subscriber('sub1', 'caller1', 'http://one:1')
rm.register_subscriber('sub1', 'caller2', 'http://two:1')
rm.register_subscriber('sub1', 'caller3', 'http://three:1')
rm.register_param_subscriber('p1', 'caller1', 'http://one:1')
rm.register_param_subscriber('p1', 'caller2', 'http://two:1')
rm.register_param_subscriber('p1', 'caller3', 'http://three:1')
rm.register_service('s1', 'caller1', 'http://one:1', 'rosrpc://one:1')
self.assertEquals('http://one:1', rm.get_node('caller1').api)
self.assertEquals('http://two:1', rm.get_node('caller2').api)
self.assertEquals('http://three:1', rm.get_node('caller3').api)
# - first, make sure that changing rosrpc URI does not erase state
rm.register_service('s1', 'caller1', 'http://one:1', 'rosrpc://oneB:1')
n = rm.get_node('caller1')
self.assertEquals(['pub1'], n.topic_publications)
self.assertEquals(['sub1'], n.topic_subscriptions)
self.assertEquals(['p1'], n.param_subscriptions)
self.assertEquals(['s1'], n.services)
self.assert_('http://one:1' in rm.publishers.get_apis('pub1'))
self.assert_('http://one:1' in rm.subscribers.get_apis('sub1'))
self.assert_('http://one:1' in rm.param_subscribers.get_apis('p1'))
self.assert_('http://one:1' in rm.services.get_apis('s1'))
# - also, make sure unregister does not erase state if API changed
rm.unregister_publisher('pub1', 'caller1', 'http://not:1')
self.assert_('http://one:1' in rm.publishers.get_apis('pub1'))
rm.unregister_subscriber('sub1', 'caller1', 'http://not:1')
self.assert_('http://one:1' in rm.subscribers.get_apis('sub1'))
rm.unregister_param_subscriber('p1', 'caller1', 'http://not:1')
self.assert_('http://one:1' in rm.param_subscribers.get_apis('p1'))
rm.unregister_service('sub1', 'caller1', 'rosrpc://not:1')
self.assert_('http://one:1' in rm.services.get_apis('s1'))
# erase caller1 sub/srvs/params via register_publisher
rm.register_publisher('pub1', 'caller1', 'http://newone:1')
self.assertEquals('http://newone:1', rm.get_node('caller1').api)
# - check node ref
n = rm.get_node('caller1')
self.assertEquals(['pub1'], n.topic_publications)
self.assertEquals([], n.services)
self.assertEquals([], n.topic_subscriptions)
self.assertEquals([], n.param_subscriptions)
# - checks publishers
self.assert_('http://newone:1' in rm.publishers.get_apis('pub1'))
# - checks subscribers
self.assert_(rm.subscribers.has_key('sub1'))
self.failIf('http://one:1' in rm.subscribers.get_apis('sub1'))
# - checks param subscribers
self.assert_(rm.param_subscribers.has_key('p1'))
self.failIf('http://one:1' in rm.param_subscribers.get_apis('p1'))
# erase caller2 pub/sub/params via register_service
# - initial state
self.assert_('http://two:1' in rm.publishers.get_apis('pub1'))
self.assert_('http://two:1' in rm.subscribers.get_apis('sub1'))
self.assert_('http://two:1' in rm.param_subscribers.get_apis('p1'))
# - change ownership of s1 to caller2
rm.register_service('s1', 'caller2', 'http://two:1', 'rosrpc://two:1')
self.assert_('http://two:1' in rm.services.get_apis('s1'))
self.assert_('http://two:1' in rm.publishers.get_apis('pub1'))
self.assert_('http://two:1' in rm.subscribers.get_apis('sub1'))
self.assert_('http://two:1' in rm.param_subscribers.get_apis('p1'))
rm.register_service('s1', 'caller2', 'http://newtwo:1',
'rosrpc://newtwo:1')
self.assertEquals('http://newone:1', rm.get_node('caller1').api)
# - check node ref
n = rm.get_node('caller2')
self.assertEquals([], n.topic_publications)
self.assertEquals(['s1'], n.services)
self.assertEquals([], n.topic_subscriptions)
self.assertEquals([], n.param_subscriptions)
# - checks publishers
self.assert_(rm.publishers.has_key('pub1'))
self.failIf('http://two:1' in rm.publishers.get_apis('pub1'))
# - checks subscribers
self.assert_(rm.subscribers.has_key('sub1'))
self.failIf('http://two:1' in rm.subscribers.get_apis('sub1'))
self.assertEquals([['sub1', ['caller3']]], rm.subscribers.get_state())
# - checks param subscribers
self.assert_(rm.param_subscribers.has_key('p1'))
self.failIf('http://two:1' in rm.param_subscribers.get_apis('p1'))
self.assertEquals([['p1', ['caller3']]],
rm.param_subscribers.get_state())
class ROSMasterHandler(object):
"""
XML-RPC handler for ROS master APIs.
API routines for the ROS Master Node. The Master Node is a
superset of the Slave Node and contains additional API methods for
creating and monitoring a graph of slave nodes.
By convention, ROS nodes take in caller_id as the first parameter
of any API call. The setting of this parameter is rarely done by
client code as ros::msproxy::MasterProxy automatically inserts
this parameter (see ros::client::getMaster()).
"""
def __init__(self, num_workers=NUM_WORKERS):
"""ctor."""
self.uri = None
self.done = False
self.thread_pool = rosmaster.threadpool.MarkedThreadPool(num_workers)
# pub/sub/providers: dict { topicName : [publishers/subscribers names] }
self.ps_lock = threading.Condition(threading.Lock())
self.reg_manager = RegistrationManager(self.thread_pool)
# maintain refs to reg_manager fields
self.publishers = self.reg_manager.publishers
self.subscribers = self.reg_manager.subscribers
self.services = self.reg_manager.services
self.param_subscribers = self.reg_manager.param_subscribers
self.topics_types = {} #dict { topicName : type }
# parameter server dictionary
self.param_server = rosmaster.paramserver.ParamDictionary(
self.reg_manager)
def _shutdown(self, reason=''):
if self.thread_pool is not None:
self.thread_pool.join_all(wait_for_tasks=False,
wait_for_threads=False)
self.thread_pool = None
self.done = True
def _ready(self, uri):
"""
Initialize the handler with the XMLRPC URI. This is a standard callback from the XmlRpcNode API.
@param uri: XML-RPC URI
@type uri: str
"""
self.uri = uri
def _ok(self):
return not self.done
###############################################################################
# EXTERNAL API
@apivalidate(0, (None, ))
def shutdown(self, caller_id, msg=''):
"""
Stop this server
@param caller_id: ROS caller id
@type caller_id: str
@param msg: a message describing why the node is being shutdown.
@type msg: str
@return: [code, msg, 0]
@rtype: [int, str, int]
"""
if msg:
print("shutdown request: %s" % msg, file=sys.stdout)
else:
print("shutdown requst", file=sys.stdout)
self._shutdown('external shutdown request from [%s]: %s' %
(caller_id, msg))
return 1, "shutdown", 0
@apivalidate('')
def getUri(self, caller_id):
"""
Get the XML-RPC URI of this server.
@param caller_id str: ROS caller id
@return [int, str, str]: [1, "", xmlRpcUri]
"""
return 1, "", self.uri
@apivalidate(-1)
def getPid(self, caller_id):
"""
Get the PID of this server
@param caller_id: ROS caller id
@type caller_id: str
@return: [1, "", serverProcessPID]
@rtype: [int, str, int]
"""
return 1, "", os.getpid()
################################################################
# PARAMETER SERVER ROUTINES
@apivalidate(0, (non_empty_str('key'), ))
def deleteParam(self, caller_id, key):
"""
Parameter Server: delete parameter
@param caller_id: ROS caller id
@type caller_id: str
@param key: parameter name
@type key: str
@return: [code, msg, 0]
@rtype: [int, str, int]
"""
try:
key = resolve_name(key, caller_id)
self.param_server.delete_param(key, self._notify_param_subscribers)
mloginfo("-PARAM [%s] by %s", key, caller_id)
return 1, "parameter %s deleted" % key, 0
except KeyError as e:
return -1, "parameter [%s] is not set" % key, 0
@apivalidate(0, (non_empty_str('key'), not_none('value')))
def setParam(self, caller_id, key, value):
"""
Parameter Server: set parameter. NOTE: if value is a
dictionary it will be treated as a parameter tree, where key
is the parameter namespace. For example:::
{'x':1,'y':2,'sub':{'z':3}}
will set key/x=1, key/y=2, and key/sub/z=3. Furthermore, it
will replace all existing parameters in the key parameter
namespace with the parameters in value. You must set
parameters individually if you wish to perform a union update.
@param caller_id: ROS caller id
@type caller_id: str
@param key: parameter name
@type key: str
@param value: parameter value.
@type value: XMLRPCLegalValue
@return: [code, msg, 0]
@rtype: [int, str, int]
"""
key = resolve_name(key, caller_id)
self.param_server.set_param(key, value, self._notify_param_subscribers)
mloginfo("+PARAM [%s] by %s", key, caller_id)
return 1, "parameter %s set" % key, 0
@apivalidate(0, (non_empty_str('key'), ))
def getParam(self, caller_id, key):
"""
Retrieve parameter value from server.
@param caller_id: ROS caller id
@type caller_id: str
@param key: parameter to lookup. If key is a namespace,
getParam() will return a parameter tree.
@type key: str
getParam() will return a parameter tree.
@return: [code, statusMessage, parameterValue]. If code is not
1, parameterValue should be ignored. If key is a namespace,
the return value will be a dictionary, where each key is a
parameter in that namespace. Sub-namespaces are also
represented as dictionaries.
@rtype: [int, str, XMLRPCLegalValue]
"""
try:
key = resolve_name(key, caller_id)
return 1, "Parameter [%s]" % key, self.param_server.get_param(key)
except KeyError as e:
return -1, "Parameter [%s] is not set" % key, 0
@apivalidate(0, (non_empty_str('key'), ))
def searchParam(self, caller_id, key):
"""
Search for parameter key on parameter server. Search starts in caller's namespace and proceeds
upwards through parent namespaces until Parameter Server finds a matching key.
searchParam's behavior is to search for the first partial match.
For example, imagine that there are two 'robot_description' parameters::
/robot_description
/robot_description/arm
/robot_description/base
/pr2/robot_description
/pr2/robot_description/base
If I start in the namespace /pr2/foo and search for
'robot_description', searchParam will match
/pr2/robot_description. If I search for 'robot_description/arm'
it will return /pr2/robot_description/arm, even though that
parameter does not exist (yet).
@param caller_id str: ROS caller id
@type caller_id: str
@param key: parameter key to search for.
@type key: str
@return: [code, statusMessage, foundKey]. If code is not 1, foundKey should be
ignored.
@rtype: [int, str, str]
"""
search_key = self.param_server.search_param(caller_id, key)
if search_key:
return 1, "Found [%s]" % search_key, search_key
else:
return -1, "Cannot find parameter [%s] in an upwards search" % key, ''
@apivalidate(0, (
is_api('caller_api'),
non_empty_str('key'),
))
def subscribeParam(self, caller_id, caller_api, key):
"""
Retrieve parameter value from server and subscribe to updates to that param. See
paramUpdate() in the Node API.
@param caller_id str: ROS caller id
@type caller_id: str
@param key: parameter to lookup.
@type key: str
@param caller_api: API URI for paramUpdate callbacks.
@type caller_api: str
@return: [code, statusMessage, parameterValue]. If code is not
1, parameterValue should be ignored. parameterValue is an empty dictionary if the parameter
has not been set yet.
@rtype: [int, str, XMLRPCLegalValue]
"""
key = resolve_name(key, caller_id)
try:
# ps_lock has precedence and is required due to
# potential self.reg_manager modification
self.ps_lock.acquire()
val = self.param_server.subscribe_param(key,
(caller_id, caller_api))
finally:
self.ps_lock.release()
return 1, "Subscribed to parameter [%s]" % key, val
@apivalidate(0, (
is_api('caller_api'),
non_empty_str('key'),
))
def unsubscribeParam(self, caller_id, caller_api, key):
"""
Retrieve parameter value from server and subscribe to updates to that param. See
paramUpdate() in the Node API.
@param caller_id str: ROS caller id
@type caller_id: str
@param key: parameter to lookup.
@type key: str
@param caller_api: API URI for paramUpdate callbacks.
@type caller_api: str
@return: [code, statusMessage, numUnsubscribed].
If numUnsubscribed is zero it means that the caller was not subscribed to the parameter.
@rtype: [int, str, int]
"""
key = resolve_name(key, caller_id)
try:
# ps_lock is required due to potential self.reg_manager modification
self.ps_lock.acquire()
retval = self.param_server.unsubscribe_param(
key, (caller_id, caller_api))
finally:
self.ps_lock.release()
return 1, "Unsubscribe to parameter [%s]" % key, 1
@apivalidate(False, (non_empty_str('key'), ))
def hasParam(self, caller_id, key):
"""
Check if parameter is stored on server.
@param caller_id str: ROS caller id
@type caller_id: str
@param key: parameter to check
@type key: str
@return: [code, statusMessage, hasParam]
@rtype: [int, str, bool]
"""
key = resolve_name(key, caller_id)
if self.param_server.has_param(key):
return 1, key, True
else:
return 1, key, False
@apivalidate([])
def getParamNames(self, caller_id):
"""
Get list of all parameter names stored on this server.
This does not adjust parameter names for caller's scope.
@param caller_id: ROS caller id
@type caller_id: str
@return: [code, statusMessage, parameterNameList]
@rtype: [int, str, [str]]
"""
return 1, "Parameter names", self.param_server.get_param_names()
##################################################################################
# NOTIFICATION ROUTINES
def _notify(self, registrations, task, key, value, node_apis):
"""
Generic implementation of callback notification
@param registrations: Registrations
@type registrations: L{Registrations}
@param task: task to queue
@type task: fn
@param key: registration key
@type key: str
@param value: value to pass to task
@type value: Any
"""
# cache thread_pool for thread safety
thread_pool = self.thread_pool
if not thread_pool:
return
try:
for node_api in node_apis:
# use the api as a marker so that we limit one thread per subscriber
thread_pool.queue_task(node_api, task, (node_api, key, value))
except KeyError:
_logger.warn(
'subscriber data stale (key [%s], listener [%s]): node API unknown'
% (key, s))
def _notify_param_subscribers(self, updates):
"""
Notify parameter subscribers of new parameter value
@param updates [([str], str, any)*]: [(subscribers, param_key, param_value)*]
@param param_value str: parameter value
"""
# cache thread_pool for thread safety
thread_pool = self.thread_pool
if not thread_pool:
return
for subscribers, key, value in updates:
# use the api as a marker so that we limit one thread per subscriber
for caller_id, caller_api in subscribers:
self.thread_pool.queue_task(
caller_api, self.param_update_task,
(caller_id, caller_api, key, value))
def param_update_task(self, caller_id, caller_api, param_key, param_value):
"""
Contact api.paramUpdate with specified parameters
@param caller_id: caller ID
@type caller_id: str
@param caller_api: XML-RPC URI of node to contact
@type caller_api: str
@param param_key: parameter key to pass to node
@type param_key: str
@param param_value: parameter value to pass to node
@type param_value: str
"""
mloginfo("paramUpdate[%s]", param_key)
code, _, _ = xmlrpcapi(caller_api).paramUpdate('/master', param_key,
param_value)
if code == -1:
try:
# ps_lock is required due to potential self.reg_manager modification
self.ps_lock.acquire()
# reverse lookup to figure out who we just called
matches = self.reg_manager.reverse_lookup(caller_api)
for m in matches:
retval = self.param_server.unsubscribe_param(
param_key, (m.id, caller_api))
finally:
self.ps_lock.release()
def _notify_topic_subscribers(self, topic, pub_uris, sub_uris):
"""
Notify subscribers with new publisher list
@param topic: name of topic
@type topic: str
@param pub_uris: list of URIs of publishers.
@type pub_uris: [str]
"""
self._notify(self.subscribers, publisher_update_task, topic, pub_uris,
sub_uris)
##################################################################################
# SERVICE PROVIDER
@apivalidate(
0,
(is_service('service'), is_api('service_api'), is_api('caller_api')))
def registerService(self, caller_id, service, service_api, caller_api):
"""
Register the caller as a provider of the specified service.
@param caller_id str: ROS caller id
@type caller_id: str
@param service: Fully-qualified name of service
@type service: str
@param service_api: Service URI
@type service_api: str
@param caller_api: XML-RPC URI of caller node
@type caller_api: str
@return: (code, message, ignore)
@rtype: (int, str, int)
"""
try:
self.ps_lock.acquire()
self.reg_manager.register_service(service, caller_id, caller_api,
service_api)
mloginfo("+SERVICE [%s] %s %s", service, caller_id, caller_api)
finally:
self.ps_lock.release()
return 1, "Registered [%s] as provider of [%s]" % (caller_id,
service), 1
@apivalidate('', (is_service('service'), ))
def lookupService(self, caller_id, service):
"""
Lookup all provider of a particular service.
@param caller_id str: ROS caller id
@type caller_id: str
@param service: fully-qualified name of service to lookup.
@type: service: str
@return: (code, message, serviceUrl). service URL is provider's
ROSRPC URI with address and port. Fails if there is no provider.
@rtype: (int, str, str)
"""
try:
self.ps_lock.acquire()
service_url = self.services.get_service_api(service)
finally:
self.ps_lock.release()
if service_url:
return 1, "rosrpc URI: [%s]" % service_url, service_url
else:
return -1, "no provider", ''
@apivalidate(0, (is_service('service'), is_api('service_api')))
def unregisterService(self, caller_id, service, service_api):
"""
Unregister the caller as a provider of the specified service.
@param caller_id str: ROS caller id
@type caller_id: str
@param service: Fully-qualified name of service
@type service: str
@param service_api: API URI of service to unregister. Unregistration will only occur if current
registration matches.
@type service_api: str
@return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1).
If this is zero it means that the caller was not registered as a service provider.
The call still succeeds as the intended final state is reached.
@rtype: (int, str, int)
"""
try:
self.ps_lock.acquire()
retval = self.reg_manager.unregister_service(
service, caller_id, service_api)
mloginfo("-SERVICE [%s] %s %s", service, caller_id, service_api)
return retval
finally:
self.ps_lock.release()
##################################################################################
# PUBLISH/SUBSCRIBE
@apivalidate([], (is_topic('topic'), valid_type_name('topic_type'),
is_api('caller_api')))
def registerSubscriber(self, caller_id, topic, topic_type, caller_api):
"""
Subscribe the caller to the specified topic. In addition to receiving
a list of current publishers, the subscriber will also receive notifications
of new publishers via the publisherUpdate API.
@param caller_id: ROS caller id
@type caller_id: str
@param topic str: Fully-qualified name of topic to subscribe to.
@param topic_type: Datatype for topic. Must be a package-resource name, i.e. the .msg name.
@type topic_type: str
@param caller_api: XML-RPC URI of caller node for new publisher notifications
@type caller_api: str
@return: (code, message, publishers). Publishers is a list of XMLRPC API URIs
for nodes currently publishing the specified topic.
@rtype: (int, str, [str])
"""
#NOTE: subscribers do not get to set topic type
try:
self.ps_lock.acquire()
self.reg_manager.register_subscriber(topic, caller_id, caller_api)
# ROS 1.1: subscriber can now set type if it is not already set
# - don't let '*' type squash valid typing
if not topic in self.topics_types and topic_type != rosgraph.names.ANYTYPE:
self.topics_types[topic] = topic_type
mloginfo("+SUB [%s] %s %s", topic, caller_id, caller_api)
pub_uris = self.publishers.get_apis(topic)
finally:
self.ps_lock.release()
return 1, "Subscribed to [%s]" % topic, pub_uris
@apivalidate(0, (is_topic('topic'), is_api('caller_api')))
def unregisterSubscriber(self, caller_id, topic, caller_api):
"""
Unregister the caller as a subscriber of the topic.
@param caller_id: ROS caller id
@type caller_id: str
@param topic: Fully-qualified name of topic to unregister.
@type topic: str
@param caller_api: API URI of service to unregister. Unregistration will only occur if current
registration matches.
@type caller_api: str
@return: (code, statusMessage, numUnsubscribed).
If numUnsubscribed is zero it means that the caller was not registered as a subscriber.
The call still succeeds as the intended final state is reached.
@rtype: (int, str, int)
"""
try:
self.ps_lock.acquire()
retval = self.reg_manager.unregister_subscriber(
topic, caller_id, caller_api)
mloginfo("-SUB [%s] %s %s", topic, caller_id, caller_api)
return retval
finally:
self.ps_lock.release()
@apivalidate([], (is_topic('topic'), valid_type_name('topic_type'),
is_api('caller_api')))
def registerPublisher(self, caller_id, topic, topic_type, caller_api):
"""
Register the caller as a publisher the topic.
@param caller_id: ROS caller id
@type caller_id: str
@param topic: Fully-qualified name of topic to register.
@type topic: str
@param topic_type: Datatype for topic. Must be a
package-resource name, i.e. the .msg name.
@type topic_type: str
@param caller_api str: ROS caller XML-RPC API URI
@type caller_api: str
@return: (code, statusMessage, subscriberApis).
List of current subscribers of topic in the form of XMLRPC URIs.
@rtype: (int, str, [str])
"""
#NOTE: we need topic_type for getPublishedTopics.
try:
self.ps_lock.acquire()
self.reg_manager.register_publisher(topic, caller_id, caller_api)
# don't let '*' type squash valid typing
if topic_type != rosgraph.names.ANYTYPE or not topic in self.topics_types:
self.topics_types[topic] = topic_type
pub_uris = self.publishers.get_apis(topic)
sub_uris = self.subscribers.get_apis(topic)
self._notify_topic_subscribers(topic, pub_uris, sub_uris)
mloginfo("+PUB [%s] %s %s", topic, caller_id, caller_api)
sub_uris = self.subscribers.get_apis(topic)
finally:
self.ps_lock.release()
return 1, "Registered [%s] as publisher of [%s]" % (caller_id,
topic), sub_uris
@apivalidate(0, (is_topic('topic'), is_api('caller_api')))
def unregisterPublisher(self, caller_id, topic, caller_api):
"""
Unregister the caller as a publisher of the topic.
@param caller_id: ROS caller id
@type caller_id: str
@param topic: Fully-qualified name of topic to unregister.
@type topic: str
@param caller_api str: API URI of service to
unregister. Unregistration will only occur if current
registration matches.
@type caller_api: str
@return: (code, statusMessage, numUnregistered).
If numUnregistered is zero it means that the caller was not registered as a publisher.
The call still succeeds as the intended final state is reached.
@rtype: (int, str, int)
"""
try:
self.ps_lock.acquire()
retval = self.reg_manager.unregister_publisher(
topic, caller_id, caller_api)
if retval[VAL]:
self._notify_topic_subscribers(
topic, self.publishers.get_apis(topic),
self.subscribers.get_apis(topic))
mloginfo("-PUB [%s] %s %s", topic, caller_id, caller_api)
finally:
self.ps_lock.release()
return retval
##################################################################################
# GRAPH STATE APIS
@apivalidate('', (valid_name('node'), ))
def lookupNode(self, caller_id, node_name):
"""
Get the XML-RPC URI of the node with the associated
name/caller_id. This API is for looking information about
publishers and subscribers. Use lookupService instead to lookup
ROS-RPC URIs.
@param caller_id: ROS caller id
@type caller_id: str
@param node: name of node to lookup
@type node: str
@return: (code, msg, URI)
@rtype: (int, str, str)
"""
try:
self.ps_lock.acquire()
node = self.reg_manager.get_node(node_name)
if node is not None:
retval = 1, "node api", node.api
else:
retval = -1, "unknown node [%s]" % node_name, ''
finally:
self.ps_lock.release()
return retval
@apivalidate(0, (empty_or_valid_name('subgraph'), ))
def getPublishedTopics(self, caller_id, subgraph):
"""
Get list of topics that can be subscribed to. This does not return topics that have no publishers.
See L{getSystemState()} to get more comprehensive list.
@param caller_id: ROS caller id
@type caller_id: str
@param subgraph: Restrict topic names to match within the specified subgraph. Subgraph namespace
is resolved relative to the caller's namespace. Use '' to specify all names.
@type subgraph: str
@return: (code, msg, [[topic1, type1]...[topicN, typeN]])
@rtype: (int, str, [[str, str],])
"""
try:
self.ps_lock.acquire()
# force subgraph to be a namespace with trailing slash
if subgraph and subgraph[-1] != rosgraph.names.SEP:
subgraph = subgraph + rosgraph.names.SEP
#we don't bother with subscribers as subscribers don't report topic types. also, the intended
#use case is for subscribe-by-topic-type
retval = [[t, self.topics_types[t]]
for t in self.publishers.iterkeys()
if t.startswith(subgraph)]
finally:
self.ps_lock.release()
return 1, "current topics", retval
@apivalidate([])
def getTopicTypes(self, caller_id):
"""
Retrieve list topic names and their types.
@param caller_id: ROS caller id
@type caller_id: str
@rtype: (int, str, [[str,str]] )
@return: (code, statusMessage, topicTypes). topicTypes is a list of [topicName, topicType] pairs.
"""
try:
self.ps_lock.acquire()
retval = list(self.topics_types.items())
finally:
self.ps_lock.release()
return 1, "current system state", retval
@apivalidate([[], [], []])
def getSystemState(self, caller_id):
"""
Retrieve list representation of system state (i.e. publishers, subscribers, and services).
@param caller_id: ROS caller id
@type caller_id: str
@rtype: (int, str, [[str,[str]], [str,[str]], [str,[str]]])
@return: (code, statusMessage, systemState).
System state is in list representation::
[publishers, subscribers, services].
publishers is of the form::
[ [topic1, [topic1Publisher1...topic1PublisherN]] ... ]
subscribers is of the form::
[ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ]
services is of the form::
[ [service1, [service1Provider1...service1ProviderN]] ... ]
"""
edges = []
try:
self.ps_lock.acquire()
retval = [
r.get_state()
for r in (self.publishers, self.subscribers, self.services)
]
finally:
self.ps_lock.release()
return 1, "current system state", retval
class GoblinShadowHandler(object):
"""
Goblin Shadow handler is a client-side local proxy of the original ROS Master.
This additional intermediary provides some key features with slight overhead.
"""
def __init__(self, master_uri, num_works, timeout=None):
# Running status
self.shadow_uri = None
self.master_uri = master_uri
self._caller_id = '/Goblin/Shadow/unbind'
self._running = True
# Inner fields
socket.setdefaulttimeout(timeout)
self.thread_pool = MarkedThreadPool(num_works)
self.ps_lock = threading.Condition(threading.Lock())
self.reg_manager = RegistrationManager(self.thread_pool)
self.master_proxy = xmlrpclib.ServerProxy(master_uri)
# TODO: support all local nodes
self.services = self.reg_manager.services
# TODO: support local param-server caches
# APIs for running XML-RPC
def _shutdown(self, reason=''):
"""
Forked from ROSMasterHandler
@param reason:
@return:
"""
if self.thread_pool is not None:
self.thread_pool.join_all(wait_for_tasks=False,
wait_for_threads=False)
self.thread_pool = None
self._running = False
def _ready(self, uri):
"""
Impl standard XML-RPC API to update URI
@param uri:
@return:
"""
self.shadow_uri = uri
self._caller_id = '/Goblin/Shadow/unbind_{}'.format(uri)
def _dispatch(self, method, params):
"""
Dispatch not-covered method to original ROS Master
"""
logger.info('Required: {}{}'.format(method, params))
if method in METHODS:
logger.debug('-- LOCAL')
status, msg, value = METHODS[method](self, *params)
logger.debug('>> LOCAL {}'.format((status, msg, value)))
else:
logger.debug('-- REMOTE: {!r}'.format(
getattr(self.master_proxy, method)))
status, msg, value = getattr(self.master_proxy, method)(*params)
logger.debug('>> REMOTE: {}'.format((status, msg, value)))
return status, msg, value
def is_running(self):
return self._running
# private methods for running Shadow
def _lookup_local_service(self, service):
uri = None
with self.ps_lock:
uri = self.services.get_service_api(service)
return uri
def _lookup_remote_service(self, service):
return self.master_proxy.lookupService(self._caller_id, service)[2]
def _reg_local_service(self, caller_id, service, service_api, caller_api):
logger.info('-- Reg Local {} {}'.format(service, service_api))
with self.ps_lock:
self.reg_manager.register_service(service, caller_id, caller_api,
service_api)
logger.info("+SERVICE [%s] %s %s", service, caller_id, caller_api)
def _reg_remote_service(self, *args):
r = self.master_proxy.registerService(*args)
logger.info('-- Reg Remote {}'.format(r))
def _unreg_local_service(self, caller_id, service, service_api):
with self.ps_lock:
self.reg_manager.unregister_service(service, caller_id,
service_api)
def _unreg_remote_service(self, *args):
r = self.master_proxy.unregisterService(*args)
logger.info('-- Unreg Remote {}'.format(r))
# Overwritten APIs
@apivalidate(0, (is_service('service'), ))
@overwrite
def lookupService(self, caller_id, service):
"""
Lookup all provider of a particular service in following rules:
1. Local/Auto/Unlabeled
1. Use local services if possible.
2. Otherwise, use remote services.
2. Remote
1. If average time cost belows the balance-curve, use remote services.
2. Otherwise, use remote services if possible.
@param caller_id str: ROS caller id
@type caller_id: str
@param service: fully-qualified name of service to lookup.
@type: service: str
@return: (code, message, serviceUrl). service URL is provider's
ROSRPC URI with address and port. Fails if there is no provider.
@rtype: (int, str, str)
"""
uri = None
cfg = swcfg.services.get_config(service)
def skeleton(x, y):
fallback_p = const(bool(cfg.fallback))
# TODO: support fallback list
return on_fallback(x, y, fallback_p, success_uri)
# local?
if swcfg.localtype == cfg.priority:
fn = skeleton(self._lookup_local_service,
self._lookup_remote_service)
else:
fn = skeleton(self._lookup_remote_service,
self._lookup_local_service)
uri = fn(service)
if success_uri(uri):
return ResponseFactory.uri_found(service, uri).pack()
else:
return ResponseFactory.unknown_service(service).pack()
@apivalidate(
0,
(is_service('service'), is_api('service_api'), is_api('caller_api')))
@overwrite
def registerService(self, caller_id, service, service_api, caller_api):
"""
Forked from ROSMasterHandler.
Register the caller as a provider of the specified service.
0. If service is `remote-only`, register with ROS Master
1. If Service is `local-only`, register with current shadow instance.
2. Otherwise, register with both sides.
@param caller_id str: ROS caller id
@type caller_id: str
@param service: Fully-qualified name of service
@type service: str
@param service_api: Service URI
@type service_api: str
@param caller_api: XML-RPC URI of caller node
@type caller_api: str
@return: (code, message, ignore)
@rtype: (int, str, int)
"""
cfg = swcfg.services.get_config(service)
if not cfg.is_local_only():
self._reg_remote_service(caller_id, service, service_api,
caller_api)
if not cfg.is_remote_only():
self._reg_local_service(caller_id, service, service_api,
caller_api)
return ResponseFactory.service_reg(caller_id, service).pack()
@apivalidate(0, (is_service('service'), is_api('service_api')))
@overwrite
def unregisterService(self, caller_id, service, service_api):
"""
Forked from ROSMasterHandler.
Unregister the caller as a provider of the specified service.
@param caller_id str: ROS caller id
@type caller_id: str
@param service: Fully-qualified name of service
@type service: str
@param service_api: API URI of service to unregister. Unregistration will only occur if current
registration matches.
@type service_api: str
@return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1).
If this is zero it means that the caller was not registered as a service provider.
The call still succeeds as the intended final state is reached.
@rtype: (int, str, int)
"""
cfg = swcfg.services.get_config(service)
if not cfg.is_local_only():
self._unreg_remote_service(caller_id, service, service_api)
if not cfg.is_remote_only():
self._unreg_local_service(caller_id, service, service_api)
return ResponseFactory.service_unreg(caller_id, service).pack()
