44 uio: packet buffer
55 struct: serlization
66 _TopicInfo: for topic negotiation
7- already provided in rosserial_msgs
7+ already provided in rosserial_msgs
88"""
99
10+ import sys
11+ import logging
1012import machine as m
1113import uio
12- import ustruct as struct
13- from time import sleep , sleep_ms , sleep_us
1414from rosserial_msgs import TopicInfo
15- import sys
16- import os
17- import logging
1815
1916# for now threads are used, will be changed with asyncio in the future
2017if sys .platform == "esp32" :
2926
3027# class to manage publish and subscribe
3128# 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."""
3431
32+ def __init__ (self , serial_id = 2 , baudrate = 115200 , ** kwargs ):
3533 """
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
4341 self .advertised_topics = dict ()
4442 self .subscribing_topics = dict ()
4543 self .serial_id = serial_id
@@ -70,27 +68,22 @@ def __init__(self, serial_id=2, baudrate=115200, **kwargs):
7068 # method to manage and advertise topic
7169 # before publishing or subscribing
7270 def _advertise_topic (self , topic_name , msg , endpoint , buffer_size ):
73-
7471 """
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+ """
7976 register = TopicInfo ()
80- register .topic_id = self .id
77+ register .topic_id = self ._id
8178 register .topic_name = topic_name
8279 register .message_type = msg ._type
8380 register .md5sum = msg ._md5sum
8481
85- self .advertised_topics [topic_name ] = self .id
82+ self .advertised_topics [topic_name ] = self ._id
8683
8784 # 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
9487
9588 # serialization
9689 packet = uio .StringIO ()
@@ -101,14 +94,21 @@ def _advertise_topic(self, topic_name, msg, endpoint, buffer_size):
10194 length = len (packet )
10295
10396 # both checksums
104- crclen = [checksum (le (length ))]
105- crcpack = [checksum (le (endpoint ) + packet )]
97+ crclen = [checksum (_le (length ))]
98+ crcpack = [checksum (_le (endpoint ) + packet )]
10699
107100 # 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
109102 self .uart .write (bytearray (fpacket ))
110103
111104 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+ """
112112
113113 if topic_name not in self .advertised_topics :
114114 self ._advertise_topic (topic_name , msg , 0 , buffer_size )
@@ -120,18 +120,26 @@ def publish(self, topic_name, msg, buffer_size=1024):
120120 packet = list (packet .getvalue ().encode ("utf-8" ))
121121 length = len (packet )
122122
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 ))]
125125 crcpack = [checksum (topic_id + packet )]
126126
127- fpacket = header + le (length ) + crclen + topic_id + packet + crcpack
127+ fpacket = header + _le (length ) + crclen + topic_id + packet + crcpack
128128 self .uart .write (bytearray (fpacket ))
129129
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"
132140
133141 # subscribing topic attributes are added
134- self .subscribing_topics [self .id ] = [msgobj , cb ]
142+ self .subscribing_topics [self ._id ] = [msgobj , _cb ]
135143
136144 # advertised if not already subscribed
137145 if topic_name not in self .advertised_topics :
@@ -166,8 +174,11 @@ def _listen(self):
166174 try :
167175 # incoming object msg initialized
168176 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+ )
171182 # object sent to callback
172183 callback = self .subscribing_topics .get (inid )[1 ]
173184 fdata = msgobj ()
@@ -176,36 +187,32 @@ def _listen(self):
176187 else :
177188 raise ValueError ("Message plus Topic ID Checksum is wrong!" )
178189
179- except Exception as e :
190+ except ( OSError , TypeError , ValueError ) :
180191 logging .info ("No incoming data could be read for subscribes." )
181192
182193
183194# functions to be used in class
184- def word (l , h ):
195+ def word (_l , _h ):
185196 """
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
189200
190201
191202# checksum method, receives array
192203def checksum (arr ):
193- return 255 - (( sum ( arr )) % 256 )
204+ """Generates checksum value of message.
194205
206+ Args:
207+ arr (list): list of hexadecimal values.
195208
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 )
201213
202- # example code
203- if __name__ == "__main__" :
204- from std_msgs import String
205- from uros import NodeHandle
206214
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