Repository navigation
Expand file tree
/
Copy pathcore.py
More file actions
224 lines (186 loc) · 7.75 KB
/
Copy pathcore.py
File metadata and controls
224 lines (186 loc) · 7.75 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
"""
libraries needed
machine: for uart usage
uio: packet buffer
struct: serlization
_TopicInfo: for topic negotiation
already provided in rosserial_msgs
"""
import sys
import logging
import machine as m
import uio
from rosserial_msgs import TopicInfo
# for now threads are used, will be changed with asyncio in the future
if sys.platform == "esp32":
import _thread as threading
else:
import threading
# rosserial protocol header
header = [0xFF, 0xFE]
logging.basicConfig(level=logging.INFO)
# class to manage publish and subscribe
# COULD BE CHANGED AFTERWARDS
class NodeHandle:
"""Initiates connection through rosserial using UART ports."""
def __init__(self, serial_id=2, baudrate=115200, **kwargs):
"""
id: used for topics id (negotiation)
advertised_topics: manage already negotiated topics
subscribing_topics: topics to which will be subscribed are here
serial_id: uart id
baudrate: baudrate used for serial comm
"""
self._id = 101
self.advertised_topics = dict()
self.subscribing_topics = dict()
self.serial_id = serial_id
self.baudrate = baudrate
if "serial" in kwargs:
self.uart = kwargs.get("serial")
elif "tx" in kwargs and "rx" in kwargs:
self.uart = m.UART(self.serial_id, self.baudrate)
self.uart.init(
self.baudrate,
tx=kwargs.get("tx"),
rx=kwargs.get("rx"),
bits=8,
parity=None,
stop=1,
txbuf=0,
)
else:
self.uart = m.UART(self.serial_id, self.baudrate)
self.uart.init(self.baudrate, bits=8, parity=None, stop=1, txbuf=0)
if sys.platform == "esp32":
threading.start_new_thread(self._listen, ())
else:
threading.Thread(target=self._listen).start()
# method to manage and advertise topic
# before publishing or subscribing
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 = _id
register.topic_name = topic_name
register.message_type = msg._type
register.md5sum = msg._md5sum
register.buffer_size = buffer_size
# serialization
packet = uio.StringIO()
register.serialize(packet)
# already serialized (packet)
packet = list(packet.getvalue().encode("utf-8"))
length = len(packet)
# both checksums
crclen = [checksum(_le(length))]
crcpack = [checksum(_le(endpoint) + packet)]
# final packet to be sent
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.items():
self._advertise_topic(key, value[0], value[1], value[2], value[3])
def publish(self, topic_name, msg, buffer_size=1024):
"""publishes messages to topics
Args:
topic_name (string): name of destination topic in ROS network.
msg (ROS message): custom message object generated by ugenpy.
buffer_size (int, optional): maximum size of buffer for message. Defaults to 1024.
"""
if topic_name not in self.advertised_topics:
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()
msg.serialize(packet)
packet = list(packet.getvalue().encode("utf-8"))
length = len(packet)
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, 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.
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] = [msg, _cb]
# advertised if not already subscribed
if topic_name not in self.advertised_topics:
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:
try:
flag = self.uart.read(2)
# check header
if flag == b"\xff\xfe":
# get bytes length
lengthbyte = self.uart.read(2)
length = word(list(lengthbyte)[0], list(lengthbyte)[1])
lenchk = self.uart.read(1)
# validate length checksum
lenchecksum = sum(list(lengthbyte)) + ord(lenchk)
if lenchecksum % 256 != 255:
raise ValueError("Length checksum is not right!")
topic_id = list(self.uart.read(2))
inid = word(topic_id[0], topic_id[1])
if inid != 0:
msgdata = self.uart.read(length)
chk = self.uart.read(1)
# validate topic plus msg checksum
datachecksum = sum((topic_id)) + sum(list(msgdata)) + ord(chk)
if datachecksum % 256 == 255:
try:
# incoming object msg initialized
msgobj = self.subscribing_topics.get(inid)[0]
except (OSError, TypeError, IndexError):
logging.info(
"TX request was made or got message from"
+ "not available subscribed topic."
)
# object sent to callback
callback = self.subscribing_topics.get(inid)[1]
fdata = msgobj()
fdata = fdata.deserialize(msgdata)
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.")
# functions to be used in class
def word(_l, _h):
"""
Given a low and high bit, converts the number back into a word.
"""
return (_h << 8) + _l
# checksum method, receives array
def checksum(arr):
"""Generates checksum value of message.
Args:
arr (list): list of hexadecimal values.
Returns:
[int]: returns an value between 0 and 256
"""
return 255 - ((sum(arr)) % 256)
# little-endian method
def _le(_h):
_h &= 0xFFFF
return [_h & 0xFF, _h >> 8]