def initialize(self): # socket for configuration requests self._zmq_context = zmq.Context() self._zmq_config_socket = self._zmq_context.socket(zmq.REQ) self._zmq_config_socket.connect(self._config_uri) # socket for receiving messages rospy.loginfo('Parent listening for messages on zmq socket %s' % self._pub_uri) self._zmq_pub_socket = self._zmq_context.socket(zmq.SUB) self._zmq_pub_socket.connect(self._pub_uri) self._zmq_pub_socket.setsockopt(zmq.SUBSCRIBE, '') self._pub_socket_lock = threading.Lock() # socket for publishing messages rospy.loginfo('Parent publishing messages on zmq socket %s' % self._sub_uri) self._zmq_sub_socket = self._zmq_context.socket(zmq.PUB) self._zmq_sub_socket.connect(self._sub_uri) self._sub_socket_lock = threading.Lock() # used to inspect the local ROS system self._ros_interface = RosInterface(self._ros_master_uri, 'mros_parent', self.ros_message_callback) self._ros_interface_lock = threading.Lock()
def initialize(self): # socket for configuration requests self._zmq_context = zmq.Context() self._zmq_config_socket = self._zmq_context.socket(zmq.REP) self._zmq_config_socket.bind(self._config_uri) # socket for publishing messages rospy.loginfo('Child publishing messages on zmq socket %s' % self._pub_uri) self._zmq_pub_socket = self._zmq_context.socket(zmq.PUB) self._zmq_pub_socket.bind(self._pub_uri) self._zmq_pub_lock = threading.Lock() # socket for receiving messages rospy.loginfo('Child listening for messages on zmq socket %s' % self._sub_uri) self._zmq_sub_socket = self._zmq_context.socket(zmq.SUB) self._zmq_sub_socket.bind(self._sub_uri) self._zmq_sub_socket.setsockopt(zmq.SUBSCRIBE, '') # used to interact the local ROS system self._ros_interface = RosInterface(self._ros_master_uri, 'mros_child', self.local_message_callback) self._remote_message_thread = threading.Thread(target=self.remote_message_thread_func) self._remote_message_thread.start()
def initialize(self): # socket for configuration requests self._zmq_context = zmq.Context() self._zmq_config_socket = self._zmq_context.socket(zmq.REP) self._zmq_config_socket.bind(self._config_uri) # socket for publishing messages rospy.loginfo("Child publishing messages on zmq socket %s" % self._pub_uri) self._zmq_pub_socket = self._zmq_context.socket(zmq.PUB) self._zmq_pub_socket.bind(self._pub_uri) self._zmq_pub_lock = threading.Lock() # socket for receiving messages rospy.loginfo("Child listening for messages on zmq socket %s" % self._sub_uri) self._zmq_sub_socket = self._zmq_context.socket(zmq.SUB) self._zmq_sub_socket.bind(self._sub_uri) self._zmq_sub_socket.setsockopt(zmq.SUBSCRIBE, "") # used to interact the local ROS system self._ros_interface = RosInterface(self._ros_master_uri, "mros_child", self.local_message_callback) self._remote_message_thread = threading.Thread(target=self.remote_message_thread_func) self._remote_message_thread.start()
class MRosChild: def __init__(self, ros_master_uri, config_uri, pub_uri, sub_uri, poll_rate=1.0): self._ros_master_uri = ros_master_uri self._config_uri = config_uri self._pub_uri = pub_uri self._sub_uri = sub_uri self._poll_rate = poll_rate self._topics = {} self._topics_lock = threading.Lock() def initialize(self): # socket for configuration requests self._zmq_context = zmq.Context() self._zmq_config_socket = self._zmq_context.socket(zmq.REP) self._zmq_config_socket.bind(self._config_uri) # socket for publishing messages rospy.loginfo('Child publishing messages on zmq socket %s' % self._pub_uri) self._zmq_pub_socket = self._zmq_context.socket(zmq.PUB) self._zmq_pub_socket.bind(self._pub_uri) self._zmq_pub_lock = threading.Lock() # socket for receiving messages rospy.loginfo('Child listening for messages on zmq socket %s' % self._sub_uri) self._zmq_sub_socket = self._zmq_context.socket(zmq.SUB) self._zmq_sub_socket.bind(self._sub_uri) self._zmq_sub_socket.setsockopt(zmq.SUBSCRIBE, '') # used to interact the local ROS system self._ros_interface = RosInterface(self._ros_master_uri, 'mros_child', self.local_message_callback) self._remote_message_thread = threading.Thread(target=self.remote_message_thread_func) self._remote_message_thread.start() def run(self): self.initialize() loop_rate = rospy.Rate(self._poll_rate) while not rospy.is_shutdown(): msg_str = self._zmq_config_socket.recv() msg = pickle.loads(msg_str) cmd = msg[0] if cmd == 'GET_TOPIC_LIST': self._ros_interface.update_ros_graph() rospy.loginfo('Child sending topic list') local_topics = self._ros_interface.get_ros_graph() self._zmq_config_socket.send(pickle.dumps(local_topics)) elif cmd == 'ADVERTISE': for topic, message_type, message_md5sum in msg[1:]: rospy.loginfo('Child advertising %s' % topic) self._ros_interface.advertise(topic, message_type, message_md5sum) self._zmq_config_socket.send(pickle.dumps('OK')) elif cmd == 'SUBSCRIBE': for topic, message_type, message_md5sum in msg[1:]: rospy.loginfo('Child subscribing to %s' % topic) self._ros_interface.subscribe(topic, message_type, message_md5sum) self._zmq_config_socket.send(pickle.dumps('OK')) else: rospy.logerr('Unknown command %s' % cmd) self._zmq_config_socket.send(pickle.dumps('ERROR')) loop_rate.sleep() def local_message_callback(self, msg, topic): ''' Received a message on the local ROS system. Forward it to the remote system. ''' if self._zmq_pub_socket is not None: with self._zmq_pub_lock: rospy.loginfo('Child forwarding message from %s' % topic) self._zmq_pub_socket.send(pickle.dumps((topic, msg._buff))) def remote_message_thread_func(self): ''' Receive messages from the remote ROS system and republish them locally. ''' while not rospy.is_shutdown(): msg_str = self._zmq_sub_socket.recv() topic, msg = pickle.loads(msg_str) rospy.loginfo('Child publishing message received from parent to %s' % topic) self._ros_interface.publish(topic, msg)
class MRosChild: def __init__(self, ros_master_uri, config_uri, pub_uri, sub_uri, poll_rate=1.0): self._ros_master_uri = ros_master_uri self._config_uri = config_uri self._pub_uri = pub_uri self._sub_uri = sub_uri self._poll_rate = poll_rate self._topics = {} self._topics_lock = threading.Lock() def initialize(self): # socket for configuration requests self._zmq_context = zmq.Context() self._zmq_config_socket = self._zmq_context.socket(zmq.REP) self._zmq_config_socket.bind(self._config_uri) # socket for publishing messages rospy.loginfo("Child publishing messages on zmq socket %s" % self._pub_uri) self._zmq_pub_socket = self._zmq_context.socket(zmq.PUB) self._zmq_pub_socket.bind(self._pub_uri) self._zmq_pub_lock = threading.Lock() # socket for receiving messages rospy.loginfo("Child listening for messages on zmq socket %s" % self._sub_uri) self._zmq_sub_socket = self._zmq_context.socket(zmq.SUB) self._zmq_sub_socket.bind(self._sub_uri) self._zmq_sub_socket.setsockopt(zmq.SUBSCRIBE, "") # used to interact the local ROS system self._ros_interface = RosInterface(self._ros_master_uri, "mros_child", self.local_message_callback) self._remote_message_thread = threading.Thread(target=self.remote_message_thread_func) self._remote_message_thread.start() def run(self): self.initialize() loop_rate = rospy.Rate(self._poll_rate) while not rospy.is_shutdown(): msg_str = self._zmq_config_socket.recv() msg = pickle.loads(msg_str) cmd = msg[0] if cmd == "GET_TOPIC_LIST": self._ros_interface.update_ros_graph() rospy.loginfo("Child sending topic list") local_topics = self._ros_interface.get_ros_graph() self._zmq_config_socket.send(pickle.dumps(local_topics)) elif cmd == "ADVERTISE": for topic, message_type, message_md5sum in msg[1:]: rospy.loginfo("Child advertising %s" % topic) self._ros_interface.advertise(topic, message_type, message_md5sum) self._zmq_config_socket.send(pickle.dumps("OK")) elif cmd == "SUBSCRIBE": for topic, message_type, message_md5sum in msg[1:]: rospy.loginfo("Child subscribing to %s" % topic) self._ros_interface.subscribe(topic, message_type, message_md5sum) self._zmq_config_socket.send(pickle.dumps("OK")) else: rospy.logerr("Unknown command %s" % cmd) self._zmq_config_socket.send(pickle.dumps("ERROR")) loop_rate.sleep() def local_message_callback(self, msg, topic): """ Received a message on the local ROS system. Forward it to the remote system. """ if self._zmq_pub_socket is not None: with self._zmq_pub_lock: rospy.loginfo("Child forwarding message from %s" % topic) self._zmq_pub_socket.send(pickle.dumps((topic, msg._buff))) def remote_message_thread_func(self): """ Receive messages from the remote ROS system and republish them locally. """ while not rospy.is_shutdown(): msg_str = self._zmq_sub_socket.recv() topic, msg = pickle.loads(msg_str) rospy.loginfo("Child publishing message received from parent to %s" % topic) self._ros_interface.publish(topic, msg)
class MRosParent: def __init__(self, ros_master_uri, config_uri, pub_uri, sub_uri, config_dict, prefix='', poll_rate=1.): self._ros_master_uri = ros_master_uri self._config_uri = config_uri self._pub_uri = pub_uri self._sub_uri = sub_uri self._config_dict = config_dict self._prefix = config_dict['prefix'] self._poll_rate = poll_rate def initialize(self): # socket for configuration requests self._zmq_context = zmq.Context() self._zmq_config_socket = self._zmq_context.socket(zmq.REQ) self._zmq_config_socket.connect(self._config_uri) # socket for receiving messages rospy.loginfo('Parent listening for messages on zmq socket %s' % self._pub_uri) self._zmq_pub_socket = self._zmq_context.socket(zmq.SUB) self._zmq_pub_socket.connect(self._pub_uri) self._zmq_pub_socket.setsockopt(zmq.SUBSCRIBE, '') self._pub_socket_lock = threading.Lock() # socket for publishing messages rospy.loginfo('Parent publishing messages on zmq socket %s' % self._sub_uri) self._zmq_sub_socket = self._zmq_context.socket(zmq.PUB) self._zmq_sub_socket.connect(self._sub_uri) self._sub_socket_lock = threading.Lock() # used to inspect the local ROS system self._ros_interface = RosInterface(self._ros_master_uri, 'mros_parent', self.ros_message_callback) self._ros_interface_lock = threading.Lock() # start thread that publishes ROS messages we get from remote system #publish_thread = threading.Thread(target=self.publish_thread_func) #publish_thread.start() def parent_to_child_topic(self, topic): return topic[len(self._prefix):] def child_to_parent_topic(self, topic): return self._prefix + topic def run(self): self.initialize() child_topics = [] print self._config_dict for child_topic in self._config_dict['topics']: parent_topic = self.child_to_parent_topic(child_topic) topic_dict = self._config_dict['topics'][child_topic] message_type = str(topic_dict['message_type']) message_class = roslib.message.get_message_class(message_type) md5sum = str(message_class._md5sum) child_topics.append((child_topic, message_type, md5sum)) # subscribe to the topic on the local ROS system rospy.loginfo('Parent subscribing to %s' % parent_topic) self._ros_interface.subscribe(parent_topic, message_type, md5sum) # advertise the topic on the local ROS system rospy.loginfo('Parent advertising %s' % parent_topic) self._ros_interface.advertise(parent_topic, message_type, md5sum) # tell the child to advertise each of the forwarded topics command = ['ADVERTISE'] + child_topics self._zmq_config_socket.send(pickle.dumps(command)) rep = pickle.loads(self._zmq_config_socket.recv()) # tell the child to subscribe to each of the forwarded topics command = ['SUBSCRIBE'] + child_topics self._zmq_config_socket.send(pickle.dumps(command)) rep = pickle.loads(self._zmq_config_socket.recv()) while not rospy.is_shutdown(): rospy.loginfo('Parent waiting for messages from child') child_topic, msg = pickle.loads(self._zmq_pub_socket.recv()) parent_topic = self.child_to_parent_topic(child_topic) rospy.loginfo('Parent publishing message from child topic %s on topic %s' % (child_topic, parent_topic)) with self._ros_interface_lock: self._ros_interface.publish(parent_topic, msg) def run_dynamic(self): self.initialize() while not rospy.is_shutdown(): rospy.loginfo('MRos parent updating') # get a list of topics from the remote self._zmq_config_socket.send(pickle.dumps(('GET_TOPIC_LIST',))) remote_ros_graph = pickle.loads(self._zmq_config_socket.recv()) # inspect the local ros graph self._ros_interface.update_ros_graph() local_ros_graph = self._ros_interface.get_ros_graph() for topic in remote_ros_graph: # all topics that exist on the remote system should have local publishers, so that # they are visible on the local ROS master rospy.loginfo('Parent advertising %s' % topic) self._ros_interface.advertise(topic, remote_ros_graph[topic]['message_type'], remote_ros_graph[topic]['md5sum']) # if there is a subscriber on the remote side, there needs to be a subscriber # on the local side, so that we can forward along all messages on this topic # TODO: destroy local subscriber when there are no more remote subscribers if len(remote_ros_graph[topic]['subscribers']) > 0: rospy.loginfo('Parent subscribing to %s' % topic) self._ros_interface.subscribe(topic, remote_ros_graph[topic]['message_type'], remote_ros_graph[topic]['md5sum']) topics_for_subscription = [] topics_for_advertising = [] for topic in local_ros_graph: # all topics which are subscribed to locally need to be subscribed to remotely print topic, local_ros_graph[topic]['subscribers'] if len(local_ros_graph[topic]['subscribers']) > 0: # FIXME just for testing if not topic in ['/test_topic']: continue topics_for_subscription.append((topic, local_ros_graph[topic]['message_type'], local_ros_graph[topic]['md5sum'])) # all topics which are published locally need to be advertised remotely topics_for_advertising.append((topic, local_ros_graph[topic]['message_type'], local_ros_graph[topic]['md5sum'])) # send list of topics that the remote side should subscribe to command = ['SUBSCRIBE'] + topics_for_subscription self._zmq_config_socket.send(pickle.dumps(command)) rep = pickle.loads(self._zmq_config_socket.recv()) # send list of topics that the remote side should advertise command = ['ADVERTISE'] + topics_for_advertising self._zmq_config_socket.send(pickle.dumps(command)) rep = pickle.loads(self._zmq_config_socket.recv()) rospy.spin() def publish_thread_func(self): while not rospy.is_shutdown(): topic, msg = pickle.loads(self._zmq_pub_socket.recv()) rospy.loginfo('Parent publishing message on %s' % topic) with self._ros_interface_lock: self._ros_interface.publish(topic, msg) def ros_message_callback(self, msg, parent_topic): # TODO: throttle based on priority child_topic = self.parent_to_child_topic(parent_topic) with self._sub_socket_lock: rospy.loginfo('Parent sending ROS message from topic %s to child for topic %s' % (parent_topic, child_topic)) self._zmq_sub_socket.send(pickle.dumps((child_topic, msg._buff)))
class MRosParent: def __init__(self, ros_master_uri, config_uri, pub_uri, sub_uri, config_dict, prefix='', poll_rate=1.): self._ros_master_uri = ros_master_uri self._config_uri = config_uri self._pub_uri = pub_uri self._sub_uri = sub_uri self._config_dict = config_dict self._prefix = config_dict['prefix'] self._poll_rate = poll_rate def initialize(self): # socket for configuration requests self._zmq_context = zmq.Context() self._zmq_config_socket = self._zmq_context.socket(zmq.REQ) self._zmq_config_socket.connect(self._config_uri) # socket for receiving messages rospy.loginfo('Parent listening for messages on zmq socket %s' % self._pub_uri) self._zmq_pub_socket = self._zmq_context.socket(zmq.SUB) self._zmq_pub_socket.connect(self._pub_uri) self._zmq_pub_socket.setsockopt(zmq.SUBSCRIBE, '') self._pub_socket_lock = threading.Lock() # socket for publishing messages rospy.loginfo('Parent publishing messages on zmq socket %s' % self._sub_uri) self._zmq_sub_socket = self._zmq_context.socket(zmq.PUB) self._zmq_sub_socket.connect(self._sub_uri) self._sub_socket_lock = threading.Lock() # used to inspect the local ROS system self._ros_interface = RosInterface(self._ros_master_uri, 'mros_parent', self.ros_message_callback) self._ros_interface_lock = threading.Lock() # start thread that publishes ROS messages we get from remote system #publish_thread = threading.Thread(target=self.publish_thread_func) #publish_thread.start() def parent_to_child_topic(self, topic): return topic[len(self._prefix):] def child_to_parent_topic(self, topic): return self._prefix + topic def run(self): self.initialize() child_topics = [] print self._config_dict for child_topic in self._config_dict['topics']: parent_topic = self.child_to_parent_topic(child_topic) topic_dict = self._config_dict['topics'][child_topic] message_type = str(topic_dict['message_type']) message_class = roslib.message.get_message_class(message_type) md5sum = str(message_class._md5sum) child_topics.append((child_topic, message_type, md5sum)) # subscribe to the topic on the local ROS system rospy.loginfo('Parent subscribing to %s' % parent_topic) self._ros_interface.subscribe(parent_topic, message_type, md5sum) # advertise the topic on the local ROS system rospy.loginfo('Parent advertising %s' % parent_topic) self._ros_interface.advertise(parent_topic, message_type, md5sum) # tell the child to advertise each of the forwarded topics command = ['ADVERTISE'] + child_topics self._zmq_config_socket.send(pickle.dumps(command)) rep = pickle.loads(self._zmq_config_socket.recv()) # tell the child to subscribe to each of the forwarded topics command = ['SUBSCRIBE'] + child_topics self._zmq_config_socket.send(pickle.dumps(command)) rep = pickle.loads(self._zmq_config_socket.recv()) while not rospy.is_shutdown(): rospy.loginfo('Parent waiting for messages from child') child_topic, msg = pickle.loads(self._zmq_pub_socket.recv()) parent_topic = self.child_to_parent_topic(child_topic) rospy.loginfo( 'Parent publishing message from child topic %s on topic %s' % (child_topic, parent_topic)) with self._ros_interface_lock: self._ros_interface.publish(parent_topic, msg) def run_dynamic(self): self.initialize() while not rospy.is_shutdown(): rospy.loginfo('MRos parent updating') # get a list of topics from the remote self._zmq_config_socket.send(pickle.dumps(('GET_TOPIC_LIST', ))) remote_ros_graph = pickle.loads(self._zmq_config_socket.recv()) # inspect the local ros graph self._ros_interface.update_ros_graph() local_ros_graph = self._ros_interface.get_ros_graph() for topic in remote_ros_graph: # all topics that exist on the remote system should have local publishers, so that # they are visible on the local ROS master rospy.loginfo('Parent advertising %s' % topic) self._ros_interface.advertise( topic, remote_ros_graph[topic]['message_type'], remote_ros_graph[topic]['md5sum']) # if there is a subscriber on the remote side, there needs to be a subscriber # on the local side, so that we can forward along all messages on this topic # TODO: destroy local subscriber when there are no more remote subscribers if len(remote_ros_graph[topic]['subscribers']) > 0: rospy.loginfo('Parent subscribing to %s' % topic) self._ros_interface.subscribe( topic, remote_ros_graph[topic]['message_type'], remote_ros_graph[topic]['md5sum']) topics_for_subscription = [] topics_for_advertising = [] for topic in local_ros_graph: # all topics which are subscribed to locally need to be subscribed to remotely print topic, local_ros_graph[topic]['subscribers'] if len(local_ros_graph[topic]['subscribers']) > 0: # FIXME just for testing if not topic in ['/test_topic']: continue topics_for_subscription.append( (topic, local_ros_graph[topic]['message_type'], local_ros_graph[topic]['md5sum'])) # all topics which are published locally need to be advertised remotely topics_for_advertising.append( (topic, local_ros_graph[topic]['message_type'], local_ros_graph[topic]['md5sum'])) # send list of topics that the remote side should subscribe to command = ['SUBSCRIBE'] + topics_for_subscription self._zmq_config_socket.send(pickle.dumps(command)) rep = pickle.loads(self._zmq_config_socket.recv()) # send list of topics that the remote side should advertise command = ['ADVERTISE'] + topics_for_advertising self._zmq_config_socket.send(pickle.dumps(command)) rep = pickle.loads(self._zmq_config_socket.recv()) rospy.spin() def publish_thread_func(self): while not rospy.is_shutdown(): topic, msg = pickle.loads(self._zmq_pub_socket.recv()) rospy.loginfo('Parent publishing message on %s' % topic) with self._ros_interface_lock: self._ros_interface.publish(topic, msg) def ros_message_callback(self, msg, parent_topic): # TODO: throttle based on priority child_topic = self.parent_to_child_topic(parent_topic) with self._sub_socket_lock: rospy.loginfo( 'Parent sending ROS message from topic %s to child for topic %s' % (parent_topic, child_topic)) self._zmq_sub_socket.send(pickle.dumps((child_topic, msg._buff)))