bug: topic negotiation

Author: sasilva1998

src/uros/core.py

@@ -67,22 +67,17 @@ def __init__(self, serial_id=2, baudrate=115200, **kwargs):
 
     # method to manage and advertise topic
     # before publishing or subscribing
-    def _advertise_topic(self, topic_name, msg, endpoint, buffer_size):
+    def _advertise_topic(self, topic_name, _id, msg, endpoint, buffer_size):
         """
         topic_name: eg. (Greet)
         msg: message object
         endpoint: corresponds to TopicInfo.msg typical topic id values
         """
         register = TopicInfo()
-        register.topic_id = self._id
+        register.topic_id = _id
         register.topic_name = topic_name
         register.message_type = msg._type
         register.md5sum = msg._md5sum
-
-        self.advertised_topics[topic_name] = self._id
-
-        # id are summed by one
-        self._id += 1
         register.buffer_size = buffer_size
 
         # serialization
@@ -101,8 +96,12 @@ def _advertise_topic(self, topic_name, msg, endpoint, buffer_size):
         fpacket = header + _le(length) + crclen + _le(endpoint) + packet + crcpack
         self.uart.write(bytearray(fpacket))
 
+    def _advertise_all_topics(self):
+        for key, value in self.advertised_topics:
+            self._advertise_topic(key, value[0], value[1], value[2], value[3])
+
     def publish(self, topic_name, msg, buffer_size=1024):
-        """[summary]
+        """publishes messages to topics
 
         Args:
             topic_name (string): name of destination topic in ROS network.
@@ -111,7 +110,10 @@ def publish(self, topic_name, msg, buffer_size=1024):
         """
 
         if topic_name not in self.advertised_topics:
-            self._advertise_topic(topic_name, msg, 0, buffer_size)
+            self.advertised_topics[topic_name] = [self._id, msg, 0, buffer_size]
+            # id are summed by one
+            self._advertise_topic(topic_name, self._id, msg, 0, buffer_size)
+            self._id += 1
 
         # same as advertise
         packet = uio.StringIO()
@@ -120,31 +122,33 @@ def publish(self, topic_name, msg, buffer_size=1024):
         packet = list(packet.getvalue().encode("utf-8"))
         length = len(packet)
 
-        topic_id = _le(self.advertised_topics.get(topic_name))
+        topic_id = _le(self.advertised_topics.get(topic_name)[0])
         crclen = [checksum(_le(length))]
         crcpack = [checksum(topic_id + packet)]
 
         fpacket = header + _le(length) + crclen + topic_id + packet + crcpack
         self.uart.write(bytearray(fpacket))
 
-    def subscribe(self, topic_name, msgobj, _cb, buffer_size=1024):
+    def subscribe(self, topic_name, msg, _cb, buffer_size=1024):
         """subscribes to a topic receiving messages and processing them by a callback function
 
         Args:
             topic_name (string): name of destiny topic to send messages.
-            msgobj (ROS message): custom message object generated by ugenpy.
+            msg (ROS message): custom message object generated by ugenpy.
             cb (function): callback function to process incoming messages.
             buffer_size (int, optional): maximum size of buffer for message. Defaults to 1024.
         """
         assert _cb is not None, "Subscribe callback is not set"
 
         # subscribing topic attributes are added
-        self.subscribing_topics[self._id] = [msgobj, _cb]
+        self.subscribing_topics[self._id] = [msg, _cb]
 
         # advertised if not already subscribed
         if topic_name not in self.advertised_topics:
-            msg = msgobj()
-            self._advertise_topic(topic_name, msg, 1, buffer_size)
+            self.advertised_topics[topic_name] = [self._id, msg, 1, buffer_size]
+            # id are summed by one
+            self._advertise_topic(topic_name, self._id, msg, 1, buffer_size)
+            self._id += 1
 
     def _listen(self):
         while True:
@@ -186,6 +190,8 @@ def _listen(self):
                             callback(fdata)
                         else:
                             raise ValueError("Message plus Topic ID Checksum is wrong!")
+                    else:
+                        self._advertise_all_topics()
 
             except (OSError, TypeError, ValueError):
                 logging.info("No incoming data could be read for subscribes.")