diff --git a/.gitignore b/.gitignore
index b6e4761..2ab793b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -127,3 +127,4 @@ dmypy.json
# Pyre type checker
.pyre/
+.vscode
diff --git a/src/uros/core.py b/src/uros/core.py
index 1f441a9..beb8233 100644
--- a/src/uros/core.py
+++ b/src/uros/core.py
@@ -4,17 +4,14 @@
uio: packet buffer
struct: serlization
_TopicInfo: for topic negotiation
- already provided in rosserial_msgs
+ already provided in rosserial_msgs
"""
+import sys
+import logging
import machine as m
import uio
-import ustruct as struct
-from time import sleep, sleep_ms, sleep_us
from rosserial_msgs import TopicInfo
-import sys
-import os
-import logging
# for now threads are used, will be changed with asyncio in the future
if sys.platform == "esp32":
@@ -29,17 +26,18 @@
# class to manage publish and subscribe
# COULD BE CHANGED AFTERWARDS
-class NodeHandle(object):
- def __init__(self, serial_id=2, baudrate=115200, **kwargs):
+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
+ 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
@@ -69,28 +67,18 @@ def __init__(self, serial_id=2, baudrate=115200, **kwargs):
# method to manage and advertise topic
# before publishing or subscribing
- def _advertise_topic(self, topic_name, msg, endpoint, buffer_size):
-
+ 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
"""
- topic_name: eg. (Greet)
- msg: message object
- endpoint: corresponds to TopicInfo.msg typical topic id values
- """
register = TopicInfo()
- register.topic_id = self.id
+ register.topic_id = _id
register.topic_name = topic_name
register.message_type = msg._type
register.md5sum = msg._md5sum
-
- self.advertised_topics[topic_name] = self.id
-
- # id are summed by one
- self.id += 1
-
- try:
- register.buffer_size = buffer_size
- except Exception as e:
- logging.info("No buffer size could be defined for topic negotiation.")
+ register.buffer_size = buffer_size
# serialization
packet = uio.StringIO()
@@ -101,17 +89,31 @@ def _advertise_topic(self, topic_name, msg, endpoint, buffer_size):
length = len(packet)
# both checksums
- crclen = [checksum(le(length))]
- crcpack = [checksum(le(endpoint) + packet)]
+ crclen = [checksum(_le(length))]
+ crcpack = [checksum(_le(endpoint) + packet)]
# final packet to be sent
- fpacket = header + le(length) + crclen + le(endpoint) + packet + crcpack
+ 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._advertise_topic(topic_name, msg, 0, buffer_size)
+ 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()
@@ -120,23 +122,33 @@ def publish(self, topic_name, msg, buffer_size=1024):
packet = list(packet.getvalue().encode("utf-8"))
length = len(packet)
- topic_id = le(self.advertised_topics.get(topic_name))
- crclen = [checksum(le(length))]
+ 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
+ fpacket = header + _le(length) + crclen + topic_id + packet + crcpack
self.uart.write(bytearray(fpacket))
- def subscribe(self, topic_name, msgobj, cb, buffer_size=1024):
- assert cb is not None, "Subscribe callback is not set"
+ 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] = [msgobj, cb]
+ self.subscribing_topics[self._id] = [msg, _cb]
# advertised if not already subscribed
if topic_name not in self.advertised_topics:
- msg = msgobj()
- self._advertise_topic(topic_name, msg, 1, buffer_size)
+ 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:
@@ -166,8 +178,11 @@ def _listen(self):
try:
# incoming object msg initialized
msgobj = self.subscribing_topics.get(inid)[0]
- except Exception:
- logging.info("TX request was made or got message from not available subscribed topic.")
+ 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()
@@ -175,37 +190,35 @@ def _listen(self):
callback(fdata)
else:
raise ValueError("Message plus Topic ID Checksum is wrong!")
+ else:
+ self._advertise_all_topics()
- except Exception as e:
+ except (OSError, TypeError, ValueError):
logging.info("No incoming data could be read for subscribes.")
# functions to be used in class
-def word(l, h):
+def word(_l, _h):
+ """
+ Given a low and high bit, converts the number back into a word.
"""
- Given a low and high bit, converts the number back into a word.
- """
- return (h << 8) + l
+ return (_h << 8) + _l
# checksum method, receives array
def checksum(arr):
- return 255 - ((sum(arr)) % 256)
-
+ """Generates checksum value of message.
-# little-endian method
-def le(h):
- h &= 0xFFFF
- return [h & 0xFF, h >> 8]
+ Args:
+ arr (list): list of hexadecimal values.
+ Returns:
+ [int]: returns an value between 0 and 256
+ """
+ return 255 - ((sum(arr)) % 256)
-# example code
-if __name__ == "__main__":
- from std_msgs import String
- from uros import NodeHandle
- msg = String()
- msg.data = "HiItsMeMario"
- node = NodeHandle(2, 115200)
- while True:
- node.publish("greet", msg)
+# little-endian method
+def _le(_h):
+ _h &= 0xFFFF
+ return [_h & 0xFF, _h >> 8]
pFad - Phonifier reborn
Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.
Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.
Alternative Proxies:
Alternative Proxy
pFad Proxy
pFad v3 Proxy
pFad v4 Proxy