publishing added

Author: sasilva1998

src/rosserial_msgs/_TopicInfo.py

@@ -0,0 +1,91 @@
+import ustruct as struct
+
+class TopicInfo(object):
+
+	md5sum = "0ad51f88fc44892f8c10684077646005"
+	_type = "rosserial_msgs/TopicInfo"
+	_has_header = False #flag to mark the presence of a Header object
+	_full_text = """# special topic_ids
+uint16 ID_PUBLISHER=0
+uint16 ID_SUBSCRIBER=1
+uint16 ID_SERVICE_SERVER=2
+uint16 ID_SERVICE_CLIENT=4
+uint16 ID_PARAMETER_REQUEST=6
+uint16 ID_LOG=7
+uint16 ID_TIME=10
+uint16 ID_TX_STOP=11
+
+# The endpoint ID for this topic
+uint16 topic_id
+
+string topic_name
+string message_type
+
+# MD5 checksum for this message type
+string md5sum
+
+# size of the buffer message must fit in
+int32 buffer_size
+"""
+
+	ID_PUBLISHER = 0
+	ID_SUBSCRIBER = 1
+	ID_SERVICE_SERVER = 2
+	ID_SERVICE_CLIENT = 4
+	ID_PARAMETER_REQUEST = 6
+	ID_LOG = 7
+	ID_TIME = 10
+	ID_TX_STOP = 11
+
+	def __init__(self):
+
+		self.topic_id=0
+		self.topic_name=''
+		self.message_type=''
+		self.md5sum=''
+		self.buffer_size=0
+
+	def serialize(self, buff):
+		try:
+			buff.write(struct.pack('<H',self.topic_id))
+			buff.write(struct.pack('<I%ss'%len(self.topic_name),len(self.topic_name),self.topic_name))
+			buff.write(struct.pack('<I%ss'%len(self.message_type),len(self.message_type),self.message_type))
+			buff.write(struct.pack('<I%ss'%len(self.md5sum),len(self.md5sum),self.md5sum))
+			buff.write(struct.pack('<i',self.buffer_size))
+		except Exception as e:
+			print(e)
+
+	def deserialize(self, str)
+		try:
+			end=0
+			start=end
+			end+=2
+			(self.topic_id,) = struct.unpack('<H', str[start:end])
+			start = end
+			end += 4
+			(length,) = struct.unpack('<I', str[start:end])
+			start = end
+			end += length
+			self.topic_name = str[start:end].decode('utf-8')
+			start = end
+			end += 4
+			(length,) = struct.unpack('<I',str[start:end])
+			start = end
+			end += length
+			self.message_type=str[start:end].decode('utf-8')
+			start = end
+			end += 4
+			(length,) = struct.unpack('<I', str[start:end])
+			start = end
+			end += length
+			self.md5sum = str[start:end].decode('utf-8')
+			start = end
+			end += 4
+			(self.buffer_size,) = _get_struct_i().unpack(str[start:end])
+			return self
+		except Exception as e:
+			print(e)
+
+			
+
+

src/std_msgs/_Bool.py

@@ -0,0 +1,29 @@
+import ustruct as struct
+
+class Bool(object):
+	_md5sum = "8b94c1b53db61fb6aed406028ad6332a"
+	_type = "std_msgs/Bool"
+	_has_header = False #flag to mark the presence of a Header object
+	_full_text = """bool data"""
+	__slots__ = ['data']
+	_slot_types = ['bool']
+
+	def __init__(self):
+		self.data = None
+
+	def serialize(self, buff):
+		try:
+			buff.write(struct.pack('<B', self.data))
+		except Exception as e:
+			print(e)
+
+	def deserialize(self, str):
+		try:
+			end = 0
+			start = end
+			end += 1
+			(self.data,) = struct.unpack('<B', str[start:end])
+			self.data = bool(self.data)
+			return self
+		except Exception as e:
+			print(e)

src/std_msgs/_Byte.py

@@ -0,0 +1,27 @@
+import struct as ustruct
+
+class Byte(object):
+	_md5sum = "ad736a2e8818154c487bb80fe42ce43b"
+	_type = "std_msgs/Byte"
+	_has_header = False #flag to mark the presence of a Header object
+	_full_text = """byte data
+"""
+
+	def __init__(self):
+		self.data=0
+
+	def serialize(self, buff):
+		try:
+			buff.write(struct.pack('<b', self.data))
+		except Exception as e:
+			print(e)
+
+	def deserialize(self, str):
+		try:
+			end = 0
+			start = end
+			end += 1
+			(self.data,) = struct.unpack('<b', str[start:end])
+			return self	
+		except Exception as e:
+			print(e)

src/std_msgs/_Int64.py

@@ -0,0 +1,26 @@
+import ustruct as struct
+
+class Int64(object):
+	_md5sum = "34add168574510e6e17f5d23ecc077ef"
+	_type = "std_msgs/Int64"
+	_has_header = False #flag to mark the presence of a Header object
+	_full_text = """int64 data"""
+
+	def __init__(self):
+		self.data=0
+
+	def serialize(self, buff):
+		try:
+			buff.write(struct.pack('<q', self.data))
+		except Exception as e:
+			print(e)
+
+	def deserialize(self, str):
+		try:
+			end = 0
+			start = end
+			end += 8
+			(self.data,) = struct.unpack('<q', str[start:end])
+			return self
+		except Exception as e:
+			print(e)

src/std_msgs/_String.py

@@ -0,0 +1,32 @@
+import ustruct as struct
+
+class String(object):
+	_md5sum = "992ce8a1687cec8c8bd883ec73ca41d1"
+	_type = "std_msgs/String"
+	_has_header = False #flag to mark the presence of a Header object
+	_full_text = """string data
+"""
+
+	def __init__(self):
+		self.data=''
+
+	def serialize(self, buff):
+		try:
+			buff.write(struct.pack('<I%ss'%len(self.data), len(self.data), self.data))
+		except Exception as e:
+			print(e)
+
+	def deserialize(self,str):
+		try:
+			end = 0
+			start = end
+			end += 4
+			(length,) = struct.unpack('<I', str[start:end])
+			start = end
+			end += length
+			self.data = str[start:end].decode('utf-8')
+			return self
+		except Exception as e:
+			print(e)
+
+

src/uros/__init__.py

@@ -0,0 +1 @@
+from .core import NodeHandle

src/uros/core.py

@@ -0,0 +1,83 @@
+import machine as m
+import uio
+import struct
+from time import sleep, sleep_ms, sleep_us
+from rosserial_msgs._TopicInfo import TopicInfo
+
+header=[0xff,0xfe]
+
+class NodeHandle(object):
+	def __init__(self, serial_id, baudrate):
+		
+		self.id=1
+
+		self.advertised_topics=dict()
+		self.serial_id=serial_id
+		self.baudrate=baudrate
+		self.uart = m.UART(self.serial_id, self.baudrate)
+		self.uart.init(self.baudrate, bits=8, parity=None, stop=1, txbuf=0)
+
+	def init_node(self):
+		pass
+
+	def _advertise_topic(self,topic_name, msg):
+
+		register=TopicInfo()
+		register.topic_id=self.id
+		register.topic_name=topic_name
+		register.message_type=msg._type
+		register.md5sum=msg._md5sum
+
+		self.advertised_topics[topic_name]=self.id
+
+		self.id+=1
+
+		try:
+			register.buffer_size=msg.buffer_size
+		except:
+			pass
+
+		#serialization
+		packet=uio.StringIO()
+		register.serialize(packet)
+
+		packet=list(packet.getvalue().encode('utf-8'))
+		length=len(packet)
+
+		crclen=[checksum(le(length))]
+		crcpack=[checksum([0,0]+packet)]
+
+		#final packet to be sent
+		fpacket=header+le(length)+crclen+[0,0]+packet+crcpack
+
+		self.uart.write(bytearray(fpacket))
+
+
+
+	def publish(self,topic_name,msg):
+		if topic_name not in self.advertised_topics:
+			self._advertise_topic(topic_name, msg)
+
+		packet=uio.StringIO()
+		msg.serialize(packet)
+
+		packet=list(packet.getvalue().encode('utf-8'))
+		length=len(packet)
+
+		topic_id=le(self.advertised_topics.get(topic_name))
+		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):
+		pass
+
+
+def checksum(arr):
+    return 255-((sum(arr))%256)
+
+def le(h):
+    h &= 0xffff
+    return [h & 0xff, h >> 8]

tests/protocoltest.py

@@ -0,0 +1,82 @@
+import machine as m
+import uio
+import struct
+from time import sleep, sleep_ms, sleep_us
+
+#rosserial protocol header
+header=bytearray([0xff,0xfe])
+
+#little-endian function
+def le(h):
+    h &= 0xffff
+    return [h & 0xff, h >> 8]
+
+#generic checksum
+def checksum(arr):
+    return 255-((sum(arr))%256)
+
+#defined baudrate to use
+baudrate=57600
+
+#uart definition for tx2 and rx2
+uart = m.UART(2,baudrate)
+uart.init(baudrate, bits=8, parity=None, stop=1, txbuf=0)
+
+#values for topic negotiation
+topic_id=5
+topic_name="Greet"
+message_type="std_msgs/String"
+md5sum='992ce8a1687cec8c8bd883ec73ca41d1'
+buffer_size=30
+
+#packet serialization for negotiation
+packettopic=uio.StringIO()
+def serialize(packet):
+    packettopic.write(struct.pack('<H',topic_id))
+    packettopic.write(struct.pack('<I%ss'%len(topic_name),len(topic_name),topic_name))
+    packettopic.write(struct.pack('<I%ss'%len(message_type),len(message_type),message_type))
+    packettopic.write(struct.pack('<I%ss'%len(md5sum),len(md5sum),md5sum))
+    packettopic.write(struct.pack('<i',buffer_size))
+serialize(packettopic)
+packettopic=packettopic.getvalue().encode('utf-8')
+packetlen=len(list(packettopic))
+checksumlen=checksum([packetlen])
+checksumpack=list([checksum(list([0,0]+list(packettopic)))])
+
+#packet serialization for publishing
+packetdata=uio.StringIO()
+dato="hola funpython"
+def serializeString(packet):
+    packetdata.write(struct.pack('<I%ss'%len(dato),len(dato),dato))
+serializeString(packetdata)
+packetdata=packetdata.getvalue().encode('utf-8')
+packetlendata=len(list(packetdata))
+checksumlendata=checksum([packetlendata])
+checksumpackdata=list([checksum(list([5,0]+list(packetdata)))])
+
+listo=0
+
+while True:
+    data = uart.read()
+    if data==b'\xff\xfe\x00\x00\xff\x00\x00\xff':
+        #negotiation is made
+        uart.write(header)
+        uart.write(bytearray(le(packetlen)))
+        uart.write(bytearray([checksumlen]))
+        uart.write(bytearray([0,0]))
+        uart.write(packettopic)
+        uart.write(bytearray(checksumpack))
+        print('enviado registro de topic')
+        listo=1
+    elif listo==1:
+        #publishing to the node
+        uart.write(header)
+        uart.write(bytearray(le(packetlendata)))
+        uart.write(bytearray([checksumlendata]))
+        uart.write(bytearray([5,0]))
+        uart.write(packetdata)
+        uart.write(bytearray(checksumpackdata))
+        print('enviado hola')
+        sleep(1)      
+
+        

tests/publishhola.py

@@ -0,0 +1,11 @@
+#uros contains NodeHandle which is reponsable to publish and subscribe
+import uros
+from std_msgs._String import String #object string imported
+from time import sleep
+node=uros.NodeHandle(2,115200) #node initialized, for tx2/rx2 and 115200 baudrate
+msg=String() #msg object init
+msg.data='Hola FunPython - Pilas esa cuarentena :v'
+while True:
+    node.publish('Greet',msg) #publish data to node
+    sleep(1)
+