4
4
uio: packet buffer
5
5
struct: serlization
6
6
_TopicInfo: for topic negotiation
7
- already provided in rosserial_msgs
7
+ already provided in rosserial_msgs
8
8
"""
9
9
10
+ import sys
11
+ import logging
10
12
import machine as m
11
13
import uio
12
- import ustruct as struct
13
- from time import sleep , sleep_ms , sleep_us
14
14
from rosserial_msgs import TopicInfo
15
- import sys
16
- import os
17
- import logging
18
15
19
16
# for now threads are used, will be changed with asyncio in the future
20
17
if sys .platform == "esp32" :
29
26
30
27
# class to manage publish and subscribe
31
28
# COULD BE CHANGED AFTERWARDS
32
- class NodeHandle ( object ) :
33
- def __init__ ( self , serial_id = 2 , baudrate = 115200 , ** kwargs ):
29
+ class NodeHandle :
30
+ """Initiates connection through rosserial using UART ports."""
34
31
32
+ def __init__ (self , serial_id = 2 , baudrate = 115200 , ** kwargs ):
35
33
"""
36
- id: used for topics id (negotiation)
37
- advertised_topics: manage already negotiated topics
38
- subscribing_topics: topics to which will be subscribed are here
39
- serial_id: uart id
40
- baudrate: baudrate used for serial comm
41
- """
42
- self .id = 101
34
+ id: used for topics id (negotiation)
35
+ advertised_topics: manage already negotiated topics
36
+ subscribing_topics: topics to which will be subscribed are here
37
+ serial_id: uart id
38
+ baudrate: baudrate used for serial comm
39
+ """
40
+ self ._id = 101
43
41
self .advertised_topics = dict ()
44
42
self .subscribing_topics = dict ()
45
43
self .serial_id = serial_id
@@ -70,27 +68,22 @@ def __init__(self, serial_id=2, baudrate=115200, **kwargs):
70
68
# method to manage and advertise topic
71
69
# before publishing or subscribing
72
70
def _advertise_topic (self , topic_name , msg , endpoint , buffer_size ):
73
-
74
71
"""
75
- topic_name: eg. (Greet)
76
- msg: message object
77
- endpoint: corresponds to TopicInfo.msg typical topic id values
78
- """
72
+ topic_name: eg. (Greet)
73
+ msg: message object
74
+ endpoint: corresponds to TopicInfo.msg typical topic id values
75
+ """
79
76
register = TopicInfo ()
80
- register .topic_id = self .id
77
+ register .topic_id = self ._id
81
78
register .topic_name = topic_name
82
79
register .message_type = msg ._type
83
80
register .md5sum = msg ._md5sum
84
81
85
- self .advertised_topics [topic_name ] = self .id
82
+ self .advertised_topics [topic_name ] = self ._id
86
83
87
84
# id are summed by one
88
- self .id += 1
89
-
90
- try :
91
- register .buffer_size = buffer_size
92
- except Exception as e :
93
- logging .info ("No buffer size could be defined for topic negotiation." )
85
+ self ._id += 1
86
+ register .buffer_size = buffer_size
94
87
95
88
# serialization
96
89
packet = uio .StringIO ()
@@ -101,14 +94,21 @@ def _advertise_topic(self, topic_name, msg, endpoint, buffer_size):
101
94
length = len (packet )
102
95
103
96
# both checksums
104
- crclen = [checksum (le (length ))]
105
- crcpack = [checksum (le (endpoint ) + packet )]
97
+ crclen = [checksum (_le (length ))]
98
+ crcpack = [checksum (_le (endpoint ) + packet )]
106
99
107
100
# final packet to be sent
108
- fpacket = header + le (length ) + crclen + le (endpoint ) + packet + crcpack
101
+ fpacket = header + _le (length ) + crclen + _le (endpoint ) + packet + crcpack
109
102
self .uart .write (bytearray (fpacket ))
110
103
111
104
def publish (self , topic_name , msg , buffer_size = 1024 ):
105
+ """[summary]
106
+
107
+ Args:
108
+ topic_name (string): name of destination topic in ROS network.
109
+ msg (ROS message): custom message object generated by ugenpy.
110
+ buffer_size (int, optional): maximum size of buffer for message. Defaults to 1024.
111
+ """
112
112
113
113
if topic_name not in self .advertised_topics :
114
114
self ._advertise_topic (topic_name , msg , 0 , buffer_size )
@@ -120,18 +120,26 @@ def publish(self, topic_name, msg, buffer_size=1024):
120
120
packet = list (packet .getvalue ().encode ("utf-8" ))
121
121
length = len (packet )
122
122
123
- topic_id = le (self .advertised_topics .get (topic_name ))
124
- crclen = [checksum (le (length ))]
123
+ topic_id = _le (self .advertised_topics .get (topic_name ))
124
+ crclen = [checksum (_le (length ))]
125
125
crcpack = [checksum (topic_id + packet )]
126
126
127
- fpacket = header + le (length ) + crclen + topic_id + packet + crcpack
127
+ fpacket = header + _le (length ) + crclen + topic_id + packet + crcpack
128
128
self .uart .write (bytearray (fpacket ))
129
129
130
- def subscribe (self , topic_name , msgobj , cb , buffer_size = 1024 ):
131
- assert cb is not None , "Subscribe callback is not set"
130
+ def subscribe (self , topic_name , msgobj , _cb , buffer_size = 1024 ):
131
+ """subscribes to a topic receiving messages and processing them by a callback function
132
+
133
+ Args:
134
+ topic_name (string): name of destiny topic to send messages.
135
+ msgobj (ROS message): custom message object generated by ugenpy.
136
+ cb (function): callback function to process incoming messages.
137
+ buffer_size (int, optional): maximum size of buffer for message. Defaults to 1024.
138
+ """
139
+ assert _cb is not None , "Subscribe callback is not set"
132
140
133
141
# subscribing topic attributes are added
134
- self .subscribing_topics [self .id ] = [msgobj , cb ]
142
+ self .subscribing_topics [self ._id ] = [msgobj , _cb ]
135
143
136
144
# advertised if not already subscribed
137
145
if topic_name not in self .advertised_topics :
@@ -166,8 +174,11 @@ def _listen(self):
166
174
try :
167
175
# incoming object msg initialized
168
176
msgobj = self .subscribing_topics .get (inid )[0 ]
169
- except Exception :
170
- logging .info ("TX request was made or got message from not available subscribed topic." )
177
+ except (OSError , TypeError , IndexError ):
178
+ logging .info (
179
+ "TX request was made or got message from"
180
+ + "not available subscribed topic."
181
+ )
171
182
# object sent to callback
172
183
callback = self .subscribing_topics .get (inid )[1 ]
173
184
fdata = msgobj ()
@@ -176,36 +187,32 @@ def _listen(self):
176
187
else :
177
188
raise ValueError ("Message plus Topic ID Checksum is wrong!" )
178
189
179
- except Exception as e :
190
+ except ( OSError , TypeError , ValueError ) :
180
191
logging .info ("No incoming data could be read for subscribes." )
181
192
182
193
183
194
# functions to be used in class
184
- def word (l , h ):
195
+ def word (_l , _h ):
185
196
"""
186
- Given a low and high bit, converts the number back into a word.
187
- """
188
- return (h << 8 ) + l
197
+ Given a low and high bit, converts the number back into a word.
198
+ """
199
+ return (_h << 8 ) + _l
189
200
190
201
191
202
# checksum method, receives array
192
203
def checksum (arr ):
193
- return 255 - (( sum ( arr )) % 256 )
204
+ """Generates checksum value of message.
194
205
206
+ Args:
207
+ arr (list): list of hexadecimal values.
195
208
196
- # little-endian method
197
- def le (h ):
198
- h &= 0xFFFF
199
- return [h & 0xFF , h >> 8 ]
200
-
209
+ Returns:
210
+ [int]: returns an value between 0 and 256
211
+ """
212
+ return 255 - ((sum (arr )) % 256 )
201
213
202
- # example code
203
- if __name__ == "__main__" :
204
- from std_msgs import String
205
- from uros import NodeHandle
206
214
207
- msg = String ()
208
- msg .data = "HiItsMeMario"
209
- node = NodeHandle (2 , 115200 )
210
- while True :
211
- node .publish ("greet" , msg )
215
+ # little-endian method
216
+ def _le (_h ):
217
+ _h &= 0xFFFF
218
+ return [_h & 0xFF , _h >> 8 ]
0 commit comments