def update_nodelta(self, params_dt): """ Running updates of nothing (workaround until params are handled by cache) :param params_dt: :return: """ # we re NOT done here, we might still need to update params params_if_dt = self.params_if_pool.update_delta(params_dt=params_dt) topic_types_dt = DiffTuple( added=[], removed=[] # shouldnt matter ) services_if_dt = DiffTuple([], []) # TMP : NOT dropping topics early (just be patient and wait for the cache callback to come...) # topics_if_dt = self.topics_pool.update_delta(topics_dt, topic_types_dt) subscribers_if_dt = DiffTuple([], []) publishers_if_dt = DiffTuple([], []) # and here we need to return to not do the normal full update dt = DiffTuple(added=params_if_dt.added + services_if_dt.added + subscribers_if_dt.added + publishers_if_dt.added, removed=params_if_dt.removed + services_if_dt.removed + subscribers_if_dt.removed + publishers_if_dt.removed) self._debug_logger.debug(""" ROS INTERFACE DIFF ADDED : {dt.added} ROS INTERFACE DIFF REMOVED : {dt.removed} """.format(**locals())) return dt
def compute_state(self, subscribers_dt, topic_types_dt): """ This is called only if there is a cache proxy with a callback, and expects DiffTuple filled up with names or types :param topics_dt: :return: """ added_subs = {t[0]: t[1] for t in subscribers_dt.added} removed_subs = {t[0]: t[1] for t in subscribers_dt.removed} for apn, ap in added_subs.iteritems(): for rpn, rp in removed_subs.iteritems(): if rp in ap: # remove nodes that are added and removed -> no change seen ap.remove(rp) removed_subs[rpn].remove(rp) for rpn, rp in removed_subs.iteritems(): for apn, ap in added_subs.iteritems(): if ap in rp: # remove nodes that are removed and added -> no change seen rp.remove(ap) added_subs[apn].remove(ap) subscribers_dt = DiffTuple( added=[[k, v] for k, v in added_subs.iteritems()], removed=[[k, v] for k, v in removed_subs.iteritems()]) computed_subscribers_dt = DiffTuple([], []) _logger.debug("removed_subs_dt : {subscribers_dt}".format(**locals())) for t in subscribers_dt.added: tt = next( ifilter(lambda ltt: t[0] == ltt[0], topic_types_dt.added), []) ttp = TopicTuple(name=t[0], type=tt[1] if len(tt) > 0 else None, endpoints=set(t[1])) if ttp.name in self.available: # if already available, we only update the endpoints list self.available[ttp.name].endpoints |= ttp.endpoints # no change here, no need to add that topic to the computed diff. else: self.available[ttp.name] = ttp computed_subscribers_dt.added.append(t[0]) for t in subscribers_dt.removed: tt = next( ifilter(lambda ltt: t[0] == ltt[0], topic_types_dt.removed), []) ttp = TopicTuple(name=t[0], type=tt[1] if len(tt) > 0 else None, endpoints=set(t[1])) if ttp.name in self.available: self.available[ttp.name].endpoints -= ttp.endpoints if not self.available[ttp.name].endpoints: self.available.pop(ttp.name, None) computed_subscribers_dt.removed.append(t[0]) # We still need to return DiffTuples return computed_subscribers_dt
def appear_disappear(): # create the publisher and then try exposing the topic again, simulating # it coming online before expose call. nonexistent_pub = rospy.Subscriber(topicname, Empty, queue_size=1) with Timeout(5) as t: dt = DiffTuple([], []) while not t.timed_out and nonexistent_pub.resolved_name not in dt.added: subscribers, topic_types = self.get_system_state() dt = self.subscriber_if_pool.update( subscribers, topic_types) self.assertEqual(dt.removed, []) # nothing removed time.sleep(0.1) # to avoid spinning out of control self.assertTrue(not t.timed_out) self.assertTrue(nonexistent_pub.resolved_name in dt.added) # detected # TODO : do we need a test with subscriber ? # every added topic should be in the list of args self.assertTrue( topicname in self.subscriber_if_pool.subscribers_args) # topic backend has been created self.assertTrue( topicname in self.subscriber_if_pool.subscribers.keys()) # up to here possible sequences should have been already tested by previous tests # Now comes our actual disappearance / withholding test nonexistent_pub.unregister() # every added topic should be in the list of args self.assertTrue( topicname in self.subscriber_if_pool.subscribers_args) # the backend should STILL be there self.assertTrue( topicname in self.subscriber_if_pool.subscribers.keys()) # Note the Topic implementation should take care of possible errors in this case with Timeout(5) as t: dt = DiffTuple([], []) while not t.timed_out and topicname not in dt.removed: subscribers, topic_types = self.get_system_state() dt = self.subscriber_if_pool.update( subscribers, topic_types) self.assertEqual(dt.added, []) # nothing added time.sleep(0.1) # to avoid spinning out of control self.assertTrue(not t.timed_out) self.assertTrue(topicname in dt.removed) # detected lost # every exposed topic should remain in the list of args ( in case regex match another topic ) self.assertTrue( topicname in self.subscriber_if_pool.subscribers_args) # make sure the topic backend should NOT be there any longer self.assertTrue( topicname not in self.subscriber_if_pool.subscribers.keys())
def test_service_update_disappear_withhold(self): """ Test service exposing functionality for a service which already exists in the ros environment. Simple Normal usecase Sequence : UPDATE -> DISAPPEAR -> WITHHOLD :return: """ servicename = '/test/absentsrv1' # every added service should be in the list of args self.assertTrue(servicename not in self.service_if_pool.services_args) # the backend should not have been created self.assertTrue(servicename not in self.service_if_pool.services.keys()) self.service_if_pool.expose_services([servicename]) # every added service should be in the list of args self.assertTrue(servicename in self.service_if_pool.services_args) # service backend has not been created self.assertTrue(servicename not in self.service_if_pool.services.keys()) dt = DiffTuple([], []) # create the service and then try updating again, simulating # it coming online after expose call. nonexistent_srv = rospy.Service(servicename, EmptySrv, srv_cb) try: # wait here until service actually appear in cache proxy with Timeout(5) as t: while not t.timed_out and nonexistent_srv.resolved_name not in dt.added: services, service_types = self.get_system_state() dt = self.service_if_pool.update(services, service_types) self.assertEqual(dt.removed, []) # nothing removed time.sleep(0.1) # to avoid spinning out of control self.assertTrue(not t.timed_out) self.assertTrue(nonexistent_srv.resolved_name in dt.added) # nonexistent_srv added self.assertEqual(dt.removed, []) # nothing removed # every added service should be in the list of args self.assertTrue(servicename in self.service_if_pool.services_args) # service backend has been created self.assertTrue(servicename in self.service_if_pool.services.keys()) # up to here possible sequences should have been already tested by previous tests # Now comes our actual disappearance / withholding test finally: nonexistent_srv.shutdown('testing disappearing service') # every added service should be in the list of args self.assertTrue(servicename in self.service_if_pool.services_args) # the backend should STILL be there self.assertTrue(servicename in self.service_if_pool.services.keys()) # Note the service implementation should take care of possible errors in this case self.service_if_pool.expose_services([]) # every withhold service should NOT be in the list of args self.assertTrue(servicename not in self.service_if_pool.services_args) # service backend should NOT be there any longer self.assertTrue(servicename not in self.service_if_pool.services.keys())
def test_service_expose_appear_update(self): """ Test basic service adding functionality for a service which does not yet exist in the ros environment ( + corner cases ) Sequence : (UPDATE? ->) -> EXPOSE -> (UPDATE? ->) APPEAR -> UPDATE :return: """ servicename = '/test/absentsrv1' # every added service should be in the list of args self.assertTrue(servicename not in self.service_if_pool.services_args) # the backend should not have been created self.assertTrue(servicename not in self.service_if_pool.services.keys()) # First update should not change state services, service_types = self.get_system_state() dt = self.service_if_pool.update(services, service_types) self.assertEqual(dt.added, []) # nothing added self.assertEqual(dt.removed, []) # nothing removed # every added service should be in the list of args self.assertTrue(servicename not in self.service_if_pool.services_args) # the backend should not have been created self.assertTrue(servicename not in self.service_if_pool.services.keys()) self.service_if_pool.expose_services([servicename]) # every added service should be in the list of args self.assertTrue(servicename in self.service_if_pool.services_args) # the backend should not have been created self.assertTrue(servicename not in self.service_if_pool.services.keys()) services, service_types = self.get_system_state() dt = self.service_if_pool.update(services, service_types) self.assertEqual(dt.added, []) # nothing added self.assertEqual(dt.removed, []) # nothing removed # make sure the service is STILL in the list of args self.assertTrue(servicename in self.service_if_pool.services_args) # make sure the service backend has STILL not been created self.assertTrue(servicename not in self.service_if_pool.services.keys()) # create the service and then try updating again, simulating # it coming online after expose call. nonexistent_srv = rospy.Service(servicename, EmptySrv, srv_cb) try: with Timeout(5) as t: dt = DiffTuple([], []) while not t.timed_out and nonexistent_srv.resolved_name not in dt.added: services, service_types = self.get_system_state() dt = self.service_if_pool.update(services, service_types) self.assertEqual(dt.removed, []) # nothing removed time.sleep(0.1) # to avoid spinning out of control self.assertTrue(not t.timed_out) self.assertTrue(nonexistent_srv.resolved_name in dt.added) # nonexistent_srv added # every exposed service should remain in the list of args ( in case regex match another service ) self.assertTrue(servicename in self.service_if_pool.services_args) # make sure the service backend has been created self.assertTrue(servicename in self.service_if_pool.services.keys()) finally: nonexistent_srv.shutdown('testing complete')
def test_param_expose_appear_update(self): """ Test basic param adding functionality for a param which does not yet exist in the ros environment ( + corner cases ) Sequence : (UPDATE? ->) -> EXPOSE -> (UPDATE? ->) APPEAR -> UPDATE :return: """ paramname = '/test/absentparam1' # every added param should be in the list of args self.assertTrue(paramname not in self.param_if_pool.params_args) # the backend should not have been created self.assertTrue(paramname not in self.param_if_pool.params.keys()) # First update should not change state params = self.get_system_state() dt = self.param_if_pool.update(params) self.assertEqual(dt.added, []) # nothing added self.assertEqual(dt.removed, []) # nothing removed # every added param should be in the list of args self.assertTrue(paramname not in self.param_if_pool.params_args) # the backend should not have been created self.assertTrue(paramname not in self.param_if_pool.params.keys()) self.param_if_pool.expose_params([paramname]) # every added param should be in the list of args self.assertTrue(paramname in self.param_if_pool.params_args) # the backend should not have been created self.assertTrue(paramname not in self.param_if_pool.params.keys()) params = self.get_system_state() dt = self.param_if_pool.update(params) self.assertEqual(dt.added, []) # nothing added self.assertEqual(dt.removed, []) # nothing removed # make sure the param is STILL in the list of args self.assertTrue(paramname in self.param_if_pool.params_args) # make sure the param backend has STILL not been created self.assertTrue(paramname not in self.param_if_pool.params.keys()) # create the param and then try updating again, simulating # it coming online after expose call. rospy.set_param(paramname, 'param_value') try: with Timeout(5) as t: dt = DiffTuple([], []) while not t.timed_out and paramname not in dt.added: params = self.get_system_state() dt = self.param_if_pool.update(params) self.assertEqual(dt.removed, []) # nothing removed time.sleep(0.1) # to avoid spinning out of control self.assertTrue(not t.timed_out) self.assertTrue(paramname in dt.added) # nonexistent_srv added # every exposed param should remain in the list of args ( in case regex match another service ) self.assertTrue(paramname in self.param_if_pool.params_args) # make sure the param backend has been created self.assertTrue(paramname in self.param_if_pool.params.keys()) finally: rospy.delete_param(paramname)
def update_fullstate(self, publishers, subscribers, services, params, topic_types, service_types): # NORMAL full update self._debug_logger.debug("""SYSTEM STATE : - publishers : {publishers} - subscribers : {subscribers} - services : {services} - params : {params} - topic_types : {topic_types} - service_types : {service_types} """.format(**locals())) # TODO : unify with the reset behavior in case of cache... # Needs to be done first, since topic algorithm depends on it # print("PARAMS : {params}".format(**locals())) params_if_dt = self.params_if_pool.update(params=params) # print("PARAM IF DT : {params_if_dt}".format(**locals())) # print("SERVICES : {services}".format(**locals())) services_if_dt = self.services_if_pool.update(services, service_types) # print("SERVICE IF DT : {services_if_dt}".format(**locals())) # print("SUBSCRIBERS : {subscribers}".format(**locals())) subscribers_if_dt = self.subscribers_if_pool.update( subscribers, topic_types) # print("SUBSCRIBER IF DT : {subscribers_if_dt}".format(**locals())) # print("PUBLISHERS : {publishers}".format(**locals())) publishers_if_dt = self.publishers_if_pool.update( publishers, topic_types) # print("PUBLISHER IF DT : {publishers_if_dt}".format(**locals())) dt = DiffTuple(added=params_if_dt.added + services_if_dt.added + subscribers_if_dt.added + publishers_if_dt.added, removed=params_if_dt.removed + services_if_dt.removed + subscribers_if_dt.removed + publishers_if_dt.removed) self._debug_logger.debug(""" ROS INTERFACE ADDED : {dt.added} ROS INTERFACE REMOVED : {dt.removed} """.format(**locals())) return dt
def test_subscriber_appear_expose_update(self): """ Test topic exposing functionality for a topic which already exists in the ros environment. Simple Normal usecase Sequence : APPEAR -> EXPOSE -> UPDATE :return: """ topicname = '/test/string' self.subscriber_if_pool.expose_subscribers([topicname]) # every added topic should be in the list of args self.assertTrue(topicname in self.subscriber_if_pool.subscribers_args) # topic backend has not been created since the update didn't run yet self.assertTrue( topicname not in self.subscriber_if_pool.subscribers.keys()) dt = DiffTuple([], []) # NOTE : We need to wait to make sure the tests nodes are started... with Timeout(5) as t: while not t.timed_out and topicname not in dt.added: subscribers, topic_types = self.get_system_state() dt = self.subscriber_if_pool.update(subscribers, topic_types) time.sleep(0.1) # to avoid spinning out of control self.assertTrue(not t.timed_out) self.assertTrue(topicname in dt.added) # has been detected # every exposed topic should remain in the list of args ( in case regex match another topic ) self.assertTrue(topicname in self.subscriber_if_pool.subscribers_args) # make sure the topic backend has been created self.assertTrue( topicname in self.subscriber_if_pool.subscribers.keys()) # cleaning up self.subscriber_if_pool.expose_subscribers([]) # every added topic should be in the list of args self.assertTrue( topicname not in self.subscriber_if_pool.subscribers_args) # topic backend has not been created since the update didn't run yet self.assertTrue( topicname not in self.subscriber_if_pool.subscribers.keys())
def compute_state(self, params_dt): """ called to update params from rospy. CAREFUL : this can be called from another thread (subscriber callback) """ computed_params_dt = DiffTuple([], []) for p in params_dt.added: pt = ParamTuple(name=p, type=None) if pt.name in self.available: if self.available[pt.name].type is None or pt.type is not None: self.available[pt.name].type = pt.type else: self.available[pt.name] = pt computed_params_dt.added.append(pt.name) for p in params_dt.removed: pt = ParamTuple(name=p, type=None) if pt.name in self.available: self.available.pop(pt.name, None) computed_params_dt.removed.append(pt.name) return computed_params_dt
def test_publisher_update_disappear_withhold(self): """ Test topic exposing functionality for a topic which already exists in the ros environment. Simple Normal usecase Sequence : UPDATE -> DISAPPEAR -> WITHHOLD :return: """ topicname = '/test/nonexistent5' # every added topic should be in the list of args self.assertTrue( topicname not in self.publisher_if_pool.publishers_args) # the backend should not have been created self.assertTrue( topicname not in self.publisher_if_pool.publishers.keys()) # First update should not change state publishers, topic_types = self.get_system_state() dt = self.publisher_if_pool.update(publishers, topic_types) self.assertEqual(dt.added, []) # nothing added self.assertEqual(dt.removed, []) # nothing removed # every added topic should be in the list of args self.assertTrue( topicname not in self.publisher_if_pool.publishers_args) # the backend should not have been created self.assertTrue( topicname not in self.publisher_if_pool.publishers.keys()) dt = self.publisher_if_pool.expose_publishers([topicname]) self.assertEqual(dt.added, []) # nothing added yet ( not existing ) self.assertEqual(dt.removed, []) # nothing removed # every added topic should be in the list of args self.assertTrue(topicname in self.publisher_if_pool.publishers_args) # topic backend has not been created self.assertTrue( topicname not in self.publisher_if_pool.publishers.keys()) # create the publisher and then try exposing the topic again, simulating # it coming online before expose call. nonexistent_pub = rospy.Publisher(topicname, Empty, queue_size=1) with Timeout(5) as t: dt = DiffTuple([], []) while not t.timed_out and nonexistent_pub.resolved_name not in dt.added: publishers, topic_types = self.get_system_state() dt = self.publisher_if_pool.update(publishers, topic_types) self.assertEqual(dt.removed, []) # nothing removed time.sleep(0.1) # to avoid spinning out of control self.assertTrue(not t.timed_out) self.assertTrue(nonexistent_pub.resolved_name in dt.added) # added now because it just appeared self.assertEqual(dt.removed, []) # nothing removed # TODO : do we need a test with subscriber ? # every added topic should be in the list of args self.assertTrue(topicname in self.publisher_if_pool.publishers_args) # topic backend has been created self.assertTrue(topicname in self.publisher_if_pool.publishers.keys()) # up to here possible sequences should have been already tested by previous tests # Now comes our actual disappearrence / withholding test nonexistent_pub.unregister( ) # https://github.com/ros/ros_comm/issues/111 ( topic is still registered on master... ) # TODO : test disappear ( how ? ) # every added topic should be in the list of args self.assertTrue(topicname in self.publisher_if_pool.publishers_args) # the backend should STILL be there self.assertTrue(topicname in self.publisher_if_pool.publishers.keys()) # Note the Topic implementation should take care of possible errors in this case self.publisher_if_pool.expose_publishers([]) # every withhold topic should NOT be in the list of args self.assertTrue( topicname not in self.publisher_if_pool.publishers_args) # topic backend should NOT be there any longer self.assertTrue( topicname not in self.publisher_if_pool.publishers.keys())
def test_service_withhold_update_disappear(self): """ Test service witholding functionality for a service which doesnt exists anymore in the ros environment. Normal usecase. Sequence : (-> UPDATE ?) -> WITHHOLD -> UPDATE -> DISAPPEAR (-> UPDATE ?) :return: """ servicename = '/test/absentsrv1' # every added service should be in the list of args self.assertTrue(servicename not in self.service_if_pool.services_args) # the backend should not have been created self.assertTrue(servicename not in self.service_if_pool.services.keys()) self.service_if_pool.expose_services([servicename]) # every added service should be in the list of args self.assertTrue(servicename in self.service_if_pool.services_args) # service backend has NOT been created yet self.assertTrue(servicename not in self.service_if_pool.services.keys()) dt = DiffTuple([], []) # create the service and then try updating again, simulating # it coming online after expose call. nonexistent_srv = rospy.Service(servicename, EmptySrv, srv_cb) try: # wait here until service actually appear in cache proxy with Timeout(5) as t: while not t.timed_out and nonexistent_srv.resolved_name not in dt.added: services, service_types = self.get_system_state() dt = self.service_if_pool.update(services, service_types) self.assertEqual(dt.removed, []) # nothing removed time.sleep(0.1) # to avoid spinning out of control self.assertTrue(not t.timed_out) self.assertTrue(nonexistent_srv.resolved_name in dt.added) # nonexistent_srv added self.assertEqual(dt.removed, []) # nothing removed # every withhold service should STILL be in the list of args self.assertTrue(servicename in self.service_if_pool.services_args) # service backend has been created self.assertTrue(servicename in self.service_if_pool.services.keys()) dt = self.service_if_pool.expose_services([]) self.assertEqual(dt.added, []) # nothing added self.assertTrue(nonexistent_srv.resolved_name in dt.removed) # nonexistent_srv removed # every withhold service should NOT be in the list of args self.assertTrue(servicename not in self.service_if_pool.services_args) # service backend should be GONE self.assertTrue(servicename not in self.service_if_pool.services.keys()) services, service_types = self.get_system_state() dt = self.service_if_pool.update(services, service_types) self.assertEqual(dt.added, []) # nothing added self.assertEqual(dt.removed, []) # nothing removed # every withhold service should STILL NOT be in the list of args self.assertTrue(servicename not in self.service_if_pool.services_args) # service backend should be GONE self.assertTrue(servicename not in self.service_if_pool.services.keys()) finally: nonexistent_srv.shutdown('testing disappearing service') services, service_types = self.get_system_state() dt = self.service_if_pool.update(services, service_types) self.assertEqual(dt.added, []) # nothing added self.assertEqual(dt.removed, []) # nonexistent_srv already removed # every withhold service should STILL NOT be in the list of args self.assertTrue(servicename not in self.service_if_pool.services_args) # service backend should be GONE self.assertTrue(servicename not in self.service_if_pool.services.keys())
def update_statedelta(self, added_publishers, added_subscribers, added_services, added_params, added_topic_types, added_service_types, removed_publishers, removed_subscribers, removed_services, removed_params, removed_topic_types, removed_service_types): params_dt = DiffTuple(added=added_params, removed=removed_params) # Needs to be done first, since topic algorithm depends on it params_if_dt = self.params_if_pool.update_delta(params_dt=params_dt) # here we need to get only the nodes' names to match ROs master API format services_dt = DiffTuple( added=[[k, [n[0] for n in nset]] for k, nset in added_services.iteritems()], removed=[[k, [n[0] for n in nset]] for k, nset in removed_services.iteritems()]) service_types_dt = DiffTuple(added=added_service_types, removed=removed_service_types) services_if_dt = self.services_if_pool.update_delta( services_dt, service_types_dt) # here we need to get only the nodes' names to match ROs master API format # CAREFUL about unicity here, a Pub|Sub can have multiple endpoint within the same node... # We do not care about that information. If we have to care, then we should pass also the uri... publishers_dt = DiffTuple( added=[[k, [n[0] for n in set(nset)]] for k, nset in added_publishers.iteritems()], removed=[[k, [n[0] for n in set(nset)]] for k, nset in removed_publishers.iteritems()]) subscribers_dt = DiffTuple( added=[[k, [n[0] for n in set(nset)]] for k, nset in added_subscribers.iteritems()], removed=[[k, [n[0] for n in set(nset)]] for k, nset in removed_subscribers.iteritems()]) # NOW DONE IN update_delta # # CAREFUL, topic interface by itself also makes the topic detected on system # # Check if there are any pyros interface with it and ignore them # topics_to_drop = TopicBack.get_interface_only_topics() # # TODO : simplify : same as early_topics_to_drop ? # # # filtering the topic dict # for td, tnode in added_topics.iteritems(): # tnode = [n for n in tnode if n not in topics_to_drop.get(td, [])] # # added_topics_list = [[td, added_topics[td]] for td in added_topics if added_topics[td]] # filtering out topics with no endpoints # # # we also need to simulate topic removal here (only names), to trigger a cleanup of interface if it s last one # removed_topics_list = [[td, removed_topics[td]] for td in removed_topics] + early_topics_to_drop topic_types_dt = DiffTuple(added=added_topic_types, removed=removed_topic_types) subscribers_if_dt = self.subscribers_if_pool.update_delta( subscribers_dt, topic_types_dt) publishers_if_dt = self.publishers_if_pool.update_delta( publishers_dt, topic_types_dt) if publishers_if_dt.added or publishers_if_dt.removed: self._debug_logger.debug( rospy.get_name() + " Pyros.ros : Publishers Delta {publishers_if_dt}".format( **locals())) if subscribers_if_dt.added or subscribers_if_dt.removed: self._debug_logger.debug( rospy.get_name() + " Pyros.ros : Subscribers Delta {subscribers_if_dt}".format( **locals())) if services_if_dt.added or services_if_dt.removed: self._debug_logger.debug( rospy.get_name() + " Pyros.ros : Services Delta {services_if_dt}".format( **locals())) # TODO : put that in debug log and show based on python logger configuration # print("Pyros ROS interface UPDATE") # print("Params ADDED : {0}".format([p for p in params_dt.added])) # print("Params GONE : {0}".format([p for p in params_dt.removed])) # print("Topics ADDED : {0}".format([t[0] for t in topics_dt.added] + early_topics_dt.added)) # print("Topics GONE : {0}".format([t[0] for t in topics_dt.removed] + early_topics_dt.removed)) # print("Srvs ADDED: {0}".format([s[0] for s in services_dt.added])) # print("Srvs GONE: {0}".format([s[0] for s in services_dt.removed])) # update_on_diff wants only names # dt = super(RosInterface, self).update_on_diff( # DiffTuple([s[0] for s in services_dt.added], [s[0] for s in services_dt.removed]), # DiffTuple([t[0] for t in topics_dt.added] + early_topics_dt.added, [t[0] for t in topics_dt.removed] + early_topics_dt.removed), # # Careful params_dt has a different content than service and topics, due to different ROS API # # TODO : make this as uniform as possible # DiffTuple([p for p in params_dt.added], [p for p in params_dt.removed]) # ) # and here we need to return to not do the normal full update dt = DiffTuple(added=params_if_dt.added + services_if_dt.added + subscribers_if_dt.added + publishers_if_dt.added, removed=params_if_dt.removed + services_if_dt.removed + subscribers_if_dt.removed + publishers_if_dt.removed) self._debug_logger.debug(""" ROS INTERFACE DIFF ADDED : {dt.added} ROS INTERFACE DIFF REMOVED : {dt.removed} """.format(**locals())) return dt
def update(self): backedup_complete_cb_ss = None #update will retrieve system state here publishers = [] subscribers = [] services = [] params = [] topic_types = [] service_types = [] # and populate these to represent the changes done on the interface params_if_dt = DiffTuple([], []) services_if_dt = DiffTuple([], []) subscribers_if_dt = DiffTuple([], []) publishers_if_dt = DiffTuple([], []) # Destroying connection cache proxy if needed if self.connection_cache is not None and not self.enable_cache: # removing existing connection cache proxy to force a reinit of everything # to make sure we dont get a messed up system state with wrong list/diff from # dynamically switching cache on and off. self.connection_cache = None # TODO Instead of one or the other, we should have "two layer behaviors" with different frequencies # Fast loop checking only diff # Slow loop checking full state # It will allow recovering from any mistakes because of wrong diffs (update speed/race conditions/etc.) if self.enable_cache: if self.connection_cache is None: # Building Connection Cache Proxy if needed self.connection_cache = connection_cache_proxy_create( self._proxy_cb) if self.connection_cache: self.enable_cache = True else: self.enable_cache = False # TMP until it s implemented in the connection cache # Because the cache doesnt currently do it params = set(rospy.get_param_names()) # determining params diff despite lack of API params_dt = DiffTuple( added=[p for p in params if p not in self.params_available], removed=[p for p in self.params_available if p not in params]) # If we have the connection_cache and a callback setup we process the diff (and maybe param changes) if self.connection_cache and (params_dt.added or params_dt.removed or backedup_complete_cb_ss is not None or self.cb_ss.qsize() > 0): try: if backedup_complete_cb_ss is not None: # using the message backed up after latest diff process cb_ss = backedup_complete_cb_ss backedup_complete_cb_ss = None #CAREFUL : untested... else: cb_ss = self.cb_ss.get_nowait() # print("CC MSG !") # if we didn't except on empty queue, so we got a message except Queue.Empty: return self.update_nodelta(params_dt) else: # if there was no change, # it means it s the first and we need to initialize with the full list if cb_ss.added is None and cb_ss.removed is None: #print("CC COMPLETE !") publishers = cb_ss.complete.get('publishers', []) subscribers = cb_ss.complete.get('subscribers', []) services = cb_ss.complete.get('services', []) # NOT YET... #params = cb_ss.complete.get('params', params) topic_types = cb_ss.complete.get('topic_types', []) service_types = cb_ss.complete.get('service_types', []) # To convert to ROS master API format : publishers = [[p, [n[0] for n in nset]] for p, nset in publishers.iteritems()] subscribers = [[s, [n[0] for n in nset]] for s, nset in subscribers.iteritems()] services = [[s, [n[0] for n in nset]] for s, nset in services.iteritems()] #print("CC COMPLETEDDDD !") # we go back to normal flow #print("UPDATE FULLSTATE ON CACHE LIST") return self.update_fullstate(publishers, subscribers, services, params, topic_types, service_types) else: # we have a delta, we can use it directly and skip the rest #print("CC DELTA !") next_cb_ss = cb_ss while self.cb_ss.qsize() > 0 and not ( next_cb_ss.added is None and next_cb_ss is None): # merging multiple diff messages as fast as possible, until the next complete status next_cb_ss = self.cb_ss.get_nowait() if not (next_cb_ss.added is None and next_cb_ss is None): cb_ss = CacheTuple( complete=None, # this is ignored here # CAREFUL one of added or remove can still be None here... added=connection_cache_merge_marshalled( next_cb_ss.added or {}, cb_ss.added), removed=connection_cache_merge_marshalled( next_cb_ss.removed or {}, cb_ss.removed)) else: # we need to pass next_cb_ss to handle complete list again... backedup_complete_cb_ss = next_cb_ss added_publishers = cb_ss.added.get('publishers', []) added_subscribers = cb_ss.added.get('subscribers', []) added_services = cb_ss.added.get('services', []) # NOT YET #added_params = cb_ss.added.get('params', params_dt.added) added_params = params_dt.added added_topic_types = cb_ss.added.get('topic_types', []) added_service_types = cb_ss.added.get('service_types', []) removed_publishers = cb_ss.removed.get('publishers', []) removed_subscribers = cb_ss.removed.get('subscribers', []) removed_services = cb_ss.removed.get('services', []) # NOT YET #removed_params = cb_ss.removed.get('params', params_dt.removed) removed_params = params_dt.removed removed_topic_types = cb_ss.removed.get('topic_types', []) removed_service_types = cb_ss.removed.get( 'service_types', []) #print("UPDATE STATEDELTA ON CACHE DIFF") return self.update_statedelta( added_publishers, added_subscribers, added_services, added_params, added_topic_types, added_service_types, removed_publishers, removed_subscribers, removed_services, removed_params, removed_topic_types, removed_service_types) elif not self.connection_cache: # make sure we are not using connection cache (otherwise state representations might be out of sync !!) #print("GETTING STATE FROM MASTER") publishers, subscribers, services, params, topic_types, service_types = self.retrieve_system_state( ) # This will call the master #print("UPDATE FULLSTATE") # NOTE : we want to be certain here that we do not mix full state representation from master with representation from cache (out of sync !!!) return self.update_fullstate(publishers, subscribers, services, params, topic_types, service_types) else: # This happens when the update is triggered, we are supposed to use the cache, but we got no message from the connection cache proxy. # WE DO NOT WANT TO GET FULLSTATE FROM CACHE, since it is out of date (ex: removed publishers are not removed until next message...) # print("GETTING STATE FROM CACHE") # publishers, subscribers, services, params, topic_types, service_types = self.retrieve_system_state() # This will call the cache # # print("UPDATE FULLSTATE") # return self.update_fullstate(publishers, subscribers, services, params, topic_types, service_types) # ==> We need to wait for next message... return self.update_nodelta(params_dt)
def update_delta(self, subscribers_dt, topic_types_dt=None): # FILTERING OUT TOPICS CREATED BY INTERFACE : # First we get all pubs/subs interfaces only nodes subs_if_nodes_on, subs_if_nodes_off = self.get_sub_interfaces_only_nodes( ) # print(" SUB ADDED DETECTED :") # print(subscribers_dt.added) # Second we filter out ON interface topics from received ADDED topics list subscribers_dt_added = [[ t[0], [n for n in t[1] if n not in subs_if_nodes_on.get(t[0], set())] ] for t in subscribers_dt.added] # filtering out topics with no endpoints subscribers_dt_added = [[tl[0], tl[1]] for tl in subscribers_dt_added if tl[1]] # print(" SUB ADDED FILTERED :") # print(subscribers_dt_added) # # print(" SUB REMOVED DETECTED :") # print(subscribers_dt.removed) # Second we filter out OFF interface topics from received REMOVED topics list subscribers_dt_removed = [ [ t[0], [ n for n in t[1] #if n not in topics_if_nodes_off.get(t[0], set()) # NOT DOABLE CURRENTLY : we would also prevent re interfacing a node that came back up... # Probably better to fix flow between direct update and callback first... # BUT compute_state() should take care of this... ] ] for t in subscribers_dt.removed ] # # Second we merge in ON interface topics into received REMOVED topics list # # This is useful to drop topics interfaces that are satisfying themselves... # for t, nodeset in subs_if_nodes_on.iteritems(): # # note manipulating dictionaries will allow us to get rid of this mess # found = False # for td in subscribers_dt_removed: # if td[0] == t: # td[1] += nodeset # found = True # break # if not found: # subscribers_dt_removed.append([t, list(nodeset)]) # filtering out topics with no endpoints subscribers_dt_removed = [[tl[0], tl[1]] for tl in subscribers_dt_removed if tl[1]] # print(" SUB REMOVED FILTERED :") # print(subscribers_dt_removed) # computing state representation subscribers_namelist_dt = self.compute_state( DiffTuple(added=subscribers_dt_added, removed=subscribers_dt_removed), topic_types_dt or []) if subscribers_namelist_dt.added or subscribers_namelist_dt.removed: _logger.debug(rospy.get_name() + " Subscribers Delta {subscribers_namelist_dt}". format(**locals())) # TODO : put that in debug log and show based on python logger configuration # print("SUBSCRIBER APPEARED: {subscribers_namelist_dt.added}".format(**locals())) # print("SUBSCRIBER GONE : {subscribers_namelist_dt.removed}".format(**locals())) # update_services wants only names dt = self.transient_change_diff( transient_appeared=subscribers_namelist_dt.added, transient_gone=subscribers_namelist_dt. removed # we want only hte name here # add_names=regexes_match_sublist(self.transients_args, [s[0] for s in topics_dt.added]), # remove_names=[s[0] for s in topics_dt.removed if s[0] not in self.get_transients_available()] ) if dt.added or dt.removed: _logger.debug(rospy.get_name() + " Update Delta {dt}".format(**locals())) # print(" UPDATE DELTA:") # print(dt) return dt