Skip to content

Commit c191bf4

Browse files
sasilva1998sasilva1998
committed
formatting
1 parent d4c636f commit c191bf4

File tree

8 files changed

+297
-259
lines changed

8 files changed

+297
-259
lines changed

‎src/uros/core.py

Lines changed: 154 additions & 147 deletions
Original file line numberDiff line numberDiff line change
@@ -14,175 +14,182 @@
1414
from rosserial_msgs import TopicInfo
1515
import sys
1616
import os
17-
#for now threads are used, will be changed with asyncio in the future
17+
18+
# for now threads are used, will be changed with asyncio in the future
1819
if sys.platform == "esp32":
1920
import _thread as threading
2021
else:
2122
import threading
2223

23-
#rosserial protocol header
24-
header=[0xff,0xfe]
24+
# rosserial protocol header
25+
header = [0xFF, 0xFE]
2526

26-
#class to manage publish and subscribe
27-
#COULD BE CHANGED AFTERWARDS
27+
# class to manage publish and subscribe
28+
# COULD BE CHANGED AFTERWARDS
2829
class NodeHandle(object):
29-
def __init__(self, serial_id, baudrate):
30-
31-
"""
30+
def __init__(self, serial_id, baudrate):
31+
32+
"""
3233
id: used for topics id (negotiation)
3334
advertised_topics: manage already negotiated topics
3435
subscribing_topics: topics to which will be subscribed are here
3536
serial_id: uart id
3637
baudrate: baudrate used for serial comm
3738
"""
38-
self.id=101
39-
self.advertised_topics=dict()
40-
self.subscribing_topics=dict()
41-
self.serial_id=serial_id
42-
self.baudrate=baudrate
43-
self.uart = m.UART(self.serial_id, self.baudrate)
44-
self.uart.init(self.baudrate, bits=8, parity=None, stop=1, txbuf=0)
45-
46-
if sys.platform == "esp32":
47-
threading.start_new_thread(self._listen, ())
48-
else:
49-
threading.Thread(target = self._listen).start()
50-
51-
52-
#method to manage and advertise topic
53-
#before publishing or subscribing
54-
def _advertise_topic(self, topic_name, msg, endpoint, buffer_size):
55-
56-
"""
39+
self.id = 101
40+
self.advertised_topics = dict()
41+
self.subscribing_topics = dict()
42+
self.serial_id = serial_id
43+
self.baudrate = baudrate
44+
self.uart = m.UART(self.serial_id, self.baudrate)
45+
self.uart.init(self.baudrate, bits=8, parity=None, stop=1, txbuf=0)
46+
47+
if sys.platform == "esp32":
48+
threading.start_new_thread(self._listen, ())
49+
else:
50+
threading.Thread(target=self._listen).start()
51+
52+
# method to manage and advertise topic
53+
# before publishing or subscribing
54+
def _advertise_topic(self, topic_name, msg, endpoint, buffer_size):
55+
56+
"""
5757
topic_name: eg. (Greet)
5858
msg: message object
5959
endpoint: corresponds to TopicInfo.msg typical topic id values
6060
"""
61-
register=TopicInfo()
62-
register.topic_id=self.id
63-
register.topic_name=topic_name
64-
register.message_type=msg._type
65-
register.md5sum=msg._md5sum
66-
67-
self.advertised_topics[topic_name]=self.id
68-
69-
#id are summed by one
70-
self.id+=1
71-
72-
try:
73-
register.buffer_size=buffer_size
74-
except Exception as e:
75-
print('No buffer size could be defined for topic negotiation.')
76-
77-
#serialization
78-
packet=uio.StringIO()
79-
register.serialize(packet)
80-
81-
#already serialized (packet)
82-
packet=list(packet.getvalue().encode('utf-8'))
83-
length=len(packet)
84-
85-
#both checksums
86-
crclen=[checksum(le(length))]
87-
crcpack=[checksum(le(endpoint)+packet)]
88-
89-
#final packet to be sent
90-
fpacket=header+le(length)+crclen+le(endpoint)+packet+crcpack
91-
self.uart.write(bytearray(fpacket))
92-
93-
94-
def publish(self, topic_name, msg, buffer_size=1024):
95-
96-
if topic_name not in self.advertised_topics:
97-
self._advertise_topic(topic_name, msg, 0, buffer_size)
98-
99-
#same as advertise
100-
packet=uio.StringIO()
101-
msg.serialize(packet)
102-
103-
packet=list(packet.getvalue().encode('utf-8'))
104-
length=len(packet)
105-
106-
topic_id=le(self.advertised_topics.get(topic_name))
107-
crclen=[checksum(le(length))]
108-
crcpack=[checksum(topic_id+packet)]
109-
110-
fpacket=header+le(length)+crclen+topic_id+packet+crcpack
111-
self.uart.write(bytearray(fpacket))
112-
113-
def subscribe(self, topic_name, msgobj, cb, buffer_size=1024):
114-
assert cb is not None, "Subscribe callback is not set"
115-
116-
#subscribing topic attributes are added
117-
self.subscribing_topics[self.id]=[msgobj,cb]
118-
119-
#advertised if not already subscribed
120-
if topic_name not in self.advertised_topics:
121-
msg = msgobj()
122-
self._advertise_topic(topic_name, msg, 1, buffer_size)
123-
124-
def _listen(self):
125-
while True:
126-
try:
127-
flag=self.uart.read(2)
128-
#check header
129-
if flag == b'\xff\xfe':
130-
#get bytes length
131-
lengthbyte = self.uart.read(2)
132-
length = word(list(lengthbyte)[0], list(lengthbyte)[1])
133-
lenchk = self.uart.read(1)
134-
135-
#validate length checksum
136-
lenchecksum = sum(list(lengthbyte)) + ord(lenchk)
137-
if lenchecksum % 256 != 255:
138-
raise ValueError('Length checksum is not right!')
139-
140-
topic_id=list(self.uart.read(2))
141-
inid = word(topic_id[0],topic_id[1])
142-
if inid != 0:
143-
msgdata = self.uart.read(length)
144-
chk = self.uart.read(1)
145-
146-
#validate topic plus msg checksum
147-
datachecksum = sum((topic_id)) + sum(list(msgdata)) + ord(chk)
148-
if datachecksum % 256 == 255:
149-
try:
150-
#incoming object msg initialized
151-
msgobj = self.subscribing_topics.get(inid)[0]
152-
except Exception :
153-
print('TX request was made or got message from not available subscribed topic.')
154-
#object sent to callback
155-
callback = self.subscribing_topics.get(inid)[1]
156-
fdata = msgobj()
157-
fdata = fdata.deserialize(msgdata)
158-
callback(fdata)
159-
else:
160-
raise ValueError('Message plus Topic ID Checksum is wrong!')
161-
162-
except Exception as e:
163-
print('No incoming data could be read for subscribes.')
164-
165-
#functions to be used in class
61+
register = TopicInfo()
62+
register.topic_id = self.id
63+
register.topic_name = topic_name
64+
register.message_type = msg._type
65+
register.md5sum = msg._md5sum
66+
67+
self.advertised_topics[topic_name] = self.id
68+
69+
# id are summed by one
70+
self.id += 1
71+
72+
try:
73+
register.buffer_size = buffer_size
74+
except Exception as e:
75+
print("No buffer size could be defined for topic negotiation.")
76+
77+
# serialization
78+
packet = uio.StringIO()
79+
register.serialize(packet)
80+
81+
# already serialized (packet)
82+
packet = list(packet.getvalue().encode("utf-8"))
83+
length = len(packet)
84+
85+
# both checksums
86+
crclen = [checksum(le(length))]
87+
crcpack = [checksum(le(endpoint) + packet)]
88+
89+
# final packet to be sent
90+
fpacket = header + le(length) + crclen + le(endpoint) + packet + crcpack
91+
self.uart.write(bytearray(fpacket))
92+
93+
def publish(self, topic_name, msg, buffer_size=1024):
94+
95+
if topic_name not in self.advertised_topics:
96+
self._advertise_topic(topic_name, msg, 0, buffer_size)
97+
98+
# same as advertise
99+
packet = uio.StringIO()
100+
msg.serialize(packet)
101+
102+
packet = list(packet.getvalue().encode("utf-8"))
103+
length = len(packet)
104+
105+
topic_id = le(self.advertised_topics.get(topic_name))
106+
crclen = [checksum(le(length))]
107+
crcpack = [checksum(topic_id + packet)]
108+
109+
fpacket = header + le(length) + crclen + topic_id + packet + crcpack
110+
self.uart.write(bytearray(fpacket))
111+
112+
def subscribe(self, topic_name, msgobj, cb, buffer_size=1024):
113+
assert cb is not None, "Subscribe callback is not set"
114+
115+
# subscribing topic attributes are added
116+
self.subscribing_topics[self.id] = [msgobj, cb]
117+
118+
# advertised if not already subscribed
119+
if topic_name not in self.advertised_topics:
120+
msg = msgobj()
121+
self._advertise_topic(topic_name, msg, 1, buffer_size)
122+
123+
def _listen(self):
124+
while True:
125+
try:
126+
flag = self.uart.read(2)
127+
# check header
128+
if flag == b"\xff\xfe":
129+
# get bytes length
130+
lengthbyte = self.uart.read(2)
131+
length = word(list(lengthbyte)[0], list(lengthbyte)[1])
132+
lenchk = self.uart.read(1)
133+
134+
# validate length checksum
135+
lenchecksum = sum(list(lengthbyte)) + ord(lenchk)
136+
if lenchecksum % 256 != 255:
137+
raise ValueError("Length checksum is not right!")
138+
139+
topic_id = list(self.uart.read(2))
140+
inid = word(topic_id[0], topic_id[1])
141+
if inid != 0:
142+
msgdata = self.uart.read(length)
143+
chk = self.uart.read(1)
144+
145+
# validate topic plus msg checksum
146+
datachecksum = sum((topic_id)) + sum(list(msgdata)) + ord(chk)
147+
if datachecksum % 256 == 255:
148+
try:
149+
# incoming object msg initialized
150+
msgobj = self.subscribing_topics.get(inid)[0]
151+
except Exception:
152+
print(
153+
"TX request was made or got message from not available subscribed topic."
154+
)
155+
# object sent to callback
156+
callback = self.subscribing_topics.get(inid)[1]
157+
fdata = msgobj()
158+
fdata = fdata.deserialize(msgdata)
159+
callback(fdata)
160+
else:
161+
raise ValueError("Message plus Topic ID Checksum is wrong!")
162+
163+
except Exception as e:
164+
print("No incoming data could be read for subscribes.")
165+
166+
167+
# functions to be used in class
166168
def word(l, h):
167-
"""
169+
"""
168170
Given a low and high bit, converts the number back into a word.
169171
"""
170-
return (h << 8) + l
172+
return (h << 8) + l
173+
171174

172-
#checksum method, receives array
175+
# checksum method, receives array
173176
def checksum(arr):
174-
return 255-((sum(arr))%256)
175-
#little-endian method
177+
return 255 - ((sum(arr)) % 256)
178+
179+
180+
# little-endian method
176181
def le(h):
177-
h &= 0xffff
178-
return [h & 0xff, h >> 8]
182+
h &= 0xFFFF
183+
return [h & 0xFF, h >> 8]
184+
179185

180-
#example code
186+
# example code
181187
if __name__ == "__main__":
182-
from std_msgs import String
183-
from uros import NodeHandle
184-
msg=String()
185-
msg.data= 'HiItsMeMario'
186-
node=NodeHandle(2,115200)
187-
while True:
188-
node.publish('greet',msg)
188+
from std_msgs import String
189+
from uros import NodeHandle
190+
191+
msg = String()
192+
msg.data = "HiItsMeMario"
193+
node = NodeHandle(2, 115200)
194+
while True:
195+
node.publish("greet", msg)

‎tests/colorrgba.py

Lines changed: 10 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,13 @@
11
import uros
2-
from std_msgs import ColorRGBA #message object ColorRGBA
2+
from std_msgs import ColorRGBA # message object ColorRGBA
33
from time import sleep
4-
node=uros.NodeHandle(2,115200) #node initialized, for tx2/rx2 and 115200 baudrate
5-
msg=ColorRGBA() #msg object init
6-
msg.r=1 #values to variables assigned
7-
msg.g=3
8-
msg.b=4
9-
msg.a=1
4+
5+
node = uros.NodeHandle(2, 115200) # node initialized, for tx2/rx2 and 115200 baudrate
6+
msg = ColorRGBA() # msg object init
7+
msg.r = 1 # values to variables assigned
8+
msg.g = 3
9+
msg.b = 4
10+
msg.a = 1
1011
while True:
11-
node.publish('Colorsh',msg) #publish data to node Colorsh
12-
sleep(1)
12+
node.publish("Colorsh", msg) # publish data to node Colorsh
13+
sleep(1)

‎tests/doublesubscribe.py

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2,9 +2,11 @@
22
from uros import NodeHandle
33
import time
44

5+
56
def cb(msg):
67
print(msg.data)
78

8-
node=NodeHandle(2,57600)
9-
node.subscribe('chatter', String, cb)
10-
node.subscribe('greet', String, cb)
9+
10+
node = NodeHandle(2, 57600)
11+
node.subscribe("chatter", String, cb)
12+
node.subscribe("greet", String, cb)

0 commit comments

Comments
 (0)