Merge branch 'release/0.9.0'

CHANGELOG.md

@@ -4,6 +4,21 @@ All notable changes to this project will be documented in this file.
 The format is based on [Keep a Changelog](http://keepachangelog.com/).
 
 ## Unreleased
+## 0.9.0 - 2023-09-05
+### Added
+- `unicore`:
+  - New compatibility layer to cover node spinning and initialization.
+  - Object `Core`:
+    - Functions: `spin()`, `spin_once()`, `spin_until_future_complete()`, `init()`, `shutdown()`.
+  - Decorators: `@ros1_only` and `@ros2_only` with an optional argument to use different function instead.
+- `uninode`:
+  - Parameter `tcp_nodelay` is now supported for both Publisher and Subscriber. It translates to `BEST_EFFORT` and vice versa.
+  - QoS ReliabilityPolicy is in ROS1.
+
+### Changed
+- `uninode`:
+  - `ROS_VERSION` is determined inside `unicore`.
+
 ## 0.8.1 - 2023-08-08
 ### Fixed
 - `uninode`:

README.md

@@ -4,6 +4,7 @@ _Python utilities for the F1tenth project._
 Currently, this package contains following modules:
 - [reconfigure](#reconfigure-module)
 - [uninode](#uninode-module)
+- [unicore](#unicore-module)
 
 
 
@@ -393,3 +394,37 @@ class MyNode(Node):
     def topic_callback(self, msg):
         print ("Received:", msg.data)
 ```
+
+
+
+## Unicore module
+
+Universal core layer for both ROS 1 and ROS 2.
+
+Universal core (or unicore) serves as a compatibility layer for running a ROS node that is executable from both ROS versions.
+
+- [Example](#full-example-2)
+
+
+### Full example
+
+Example:
+```python
+from autopsy.core import *
+from autopsy.node import Node
+from std_msgs.msg import Int32
+
+@ros2_only(other_callback)
+def callback(data):
+    print("Called from ROS2!")
+
+def other_callback(data):
+    print("Called from ROS1!")
+
+Core.init()
+
+n = Node("testing_node")
+n.Subscriber("/topic", Int32, callback)
+
+Core.spin(n)
+```

autopsy/core.py

@@ -0,0 +1,170 @@
+#!/usr/bin/env python
+# core.py
+"""Universal core class with ROS 1 / ROS 2 support.
+
+Universal core (or unicore) serves as a compatibility layer for running
+a ROS node that is executable from both ROS versions.
+"""
+######################
+# Imports & Globals
+######################
+
+# Figure out ROS version
+try:
+    import rospy
+
+    ROS_VERSION = 1
+except:
+    try:
+        import rclpy
+
+        ROS_VERSION = 2
+    except:
+        print ("No ROS package detected.")
+        ROS_VERSION = 0
+
+
+######################
+# Decorators
+######################
+
+def _ros_version_only(ros_version, f):
+    """Decorator for disabling functions that are ROS version dependent.
+
+    Using an optional argument a substitute function may be set.
+    This function will be called instead of the original one,
+    when ROS version is not available.
+    """
+    global ROS_VERSION
+
+    func = None
+
+    def substitute_or_block(*args, **kwargs):
+        # Substitute
+        if ( len(args) == 1 ) and ( callable(args[0]) ):
+
+            def substitute(*args, **kwargs):
+                return f ( *args, **kwargs )
+
+            return substitute
+        # Block
+
+    def let_pass(*args, **kwargs):
+        # Let pass function that is decorated without params
+        if ( len(args) == 0 ) or ( not ( callable(args[0]) ) ):
+            return f(*args, **kwargs)
+        # Ignore parameters of a decorator and run original function instead
+        else:
+            #nonlocal func
+            func = args[0]
+
+            def pass_it(*args, **kwargs):
+                return func(*args, **kwargs)
+
+            return pass_it
+
+    return let_pass if ROS_VERSION == ros_version else substitute_or_block
+
+
+def ros1_only(f):
+    """Decorator for enabling functions only with ROS1."""
+    return _ros_version_only(1, f)
+
+
+def ros2_only(f):
+    """Decorator for enabling functions only with ROS2."""
+    return _ros_version_only(2, f)
+
+
+######################
+# Universal Core functions
+######################
+
+class Core(object):
+    """Universal core class that supports both ROS 1 and ROS 2."""
+
+    @ros2_only
+    def init(self, args = None, context = None):
+        """Initialize ROS communications for a given context.
+
+        Arguments:
+        args -- command line arguments, list
+        context -- context to initialize, if None default is used
+
+        Reference:
+        https://docs.ros2.org/foxy/api/rclpy/api/init_shutdown.html#rclpy.init
+        """
+        rclpy.init(args = args, context = context)
+
+
+    def spin(self, node = None, executor = None):
+        """Execute work and block until the node is shutdown.
+
+        Arguments:
+        node -- instance of Node to be checked for work
+        executor -- executor to use, if None global is used
+
+        Reference:
+        https://docs.ros2.org/foxy/api/rclpy/api/init_shutdown.html#rclpy.spin
+        """
+        if ROS_VERSION == 1:
+            rospy.spin()
+        elif ROS_VERSION == 2:
+            rclpy.spin(node = node, executor = executor)
+
+
+    @ros2_only(spin)
+    def spin_once(self, node, executor = None, timeout_sec = None):
+        """Execute one item of work or wait until timeout expires.
+
+        Arguments:
+        node -- instance of Node to be checked for work
+        executor -- executor to use, if None global is used
+        timeout_sec -- seconds to wait, if None or <0 block forever
+
+        Reference:
+        https://docs.ros2.org/foxy/api/rclpy/api/init_shutdown.html#rclpy.spin_once
+
+        Note:
+        ROS1 (rospy) does not support this. Something similar can be
+        achieved using `rospy.wait_for_message()`, but it is not the
+        same. So we translate this into ordinary `rospy.spin()`.
+        """
+        rclpy.spin_once(node = node, executor = executor, timeout_sec = timeout_sec)
+
+
+    @ros2_only(spin)
+    def spin_until_future_complete(self, node, future, executor = None, timeout_sec = None):
+        """Execute work until the future is complete.
+
+        Arguments:
+        node -- instance of Node to be checked for work
+        future -- future object to wait on
+        executor -- executor to use, if None global is used
+        timeout_sec -- seconds to wait, if None or <0 block forever
+
+        Reference:
+        https://docs.ros2.org/foxy/api/rclpy/api/init_shutdown.html#rclpy.spin_until_future_complete
+
+        Note:
+        ROS1 (rospy) does not support this, so we translate
+        it into ordinary `rospy.spin()`.
+        """
+        rclpy.spin_until_future_complete(node = node, future = future, executor = executor, timeout_sec = timeout_sec)
+
+
+    @ros2_only
+    def shutdown(self, context = None):
+        """Shutdown a previously initialized context and global executor.
+
+        Arguments:
+        context -- context to invalidate, if None default is used
+
+        Reference:
+        https://docs.ros2.org/latest/api/rclpy/api/init_shutdown.html#rclpy.shutdown
+        """
+        rclpy.shutdown(context = context)
+
+
+# Workaround for 'unbound method' error in Python 2.
+Core = Core()

autopsy/node.py

@@ -86,24 +86,16 @@ def topic_callback(self, msg):
 # Imports & Globals
 ######################
 
-# Figure out ROS version
-try:
-    import rospy
+from autopsy.core import ROS_VERSION
 
+if ROS_VERSION == 1:
     from .ros1_node import Node as NodeI
     from .ros1_time import Time as TimeI
     from .ros1_qos import *
 
-    ROS_VERSION = 1
-except:
-    try:
-        from rclpy.node import Node as NodeI
-        from rclpy.qos import *
-
-        ROS_VERSION = 2
-    except:
-        print ("No ROS package detected.")
-        ROS_VERSION = 0
+elif ROS_VERSION == 2:
+    from rclpy.node import Node as NodeI
+    from rclpy.qos import *
 
 
 ######################
@@ -131,12 +123,14 @@ def Publisher(self, name, data_class, subscriber_listener=None, tcp_nodelay=Fals
         Arguments (only those that are used):
         name -- name of the topic to publish to, str
         data_class -- class of the ROS message
+        tcp_nodelay -- disable Nagle algorithm on TCPROS to lower latency, bool
+        latch -- enable latching on the connection, bool
         queue_size -- number of messages to be kept in queue, int
 
         Reference:
         http://docs.ros.org/en/kinetic/api/rospy/html/rospy.topics.Publisher-class.html
         """
-        return super(Node, self).create_publisher(msg_type = data_class, topic = name, qos_profile = QoSProfile(depth = queue_size, durability = DurabilityPolicy.TRANSIENT_LOCAL if latch else DurabilityPolicy.VOLATILE))
+        return super(Node, self).create_publisher(msg_type = data_class, topic = name, qos_profile = QoSProfile(depth = queue_size, durability = DurabilityPolicy.TRANSIENT_LOCAL if latch else DurabilityPolicy.VOLATILE, reliability = ReliabilityPolicy.BEST_EFFORT if tcp_nodelay else ReliabilityPolicy.RELIABLE))
 
 
     def Subscriber(self, name, data_class, callback=None, callback_args=None, queue_size=10, buff_size=65536, tcp_nodelay=False):
@@ -147,11 +141,12 @@ def Subscriber(self, name, data_class, callback=None, callback_args=None, queue_
         data_class -- class of the ROS message
         callback -- function to be called upon receiving a message, Callable[msg_type]
         queue_size -- number of messages to be kept in queue, int
+        tcp_nodelay -- disable Nagle algorithm on TCPROS to lower latency, bool
 
         Reference:
         http://docs.ros.org/en/kinetic/api/rospy/html/rospy.topics.Subscriber-class.html
         """
-        return super(Node, self).create_subscription(msg_type = data_class, topic = name, callback = callback, qos_profile = QoSProfile(depth = queue_size))
+        return super(Node, self).create_subscription(msg_type = data_class, topic = name, callback = callback, qos_profile = QoSProfile(depth = queue_size, reliability = ReliabilityPolicy.BEST_EFFORT if tcp_nodelay else ReliabilityPolicy.RELIABLE))
 
 
     def Rate(self, hz, reset=False):

autopsy/ros1_node.py

@@ -11,7 +11,7 @@
 except:
     pass
 
-from .ros1_qos import DurabilityPolicy
+from .ros1_qos import DurabilityPolicy, ReliabilityPolicy
 from .ros1_time import Time
 from .ros1_clock import Clock
 from .ros1_logger import Logger
@@ -66,7 +66,7 @@ def create_publisher(self, msg_type, topic, qos_profile, **kwargs):
         Reference:
         https://docs.ros2.org/latest/api/rclpy/api/node.html#rclpy.node.Node.create_publisher
         """
-        return rospy.Publisher(name = topic, data_class = msg_type, latch = qos_profile.durability == DurabilityPolicy.TRANSIENT_LOCAL, queue_size = qos_profile.depth)
+        return rospy.Publisher(name = topic, data_class = msg_type, tcp_nodelay = qos_profile.reliability == ReliabilityPolicy.BEST_EFFORT, latch = qos_profile.durability == DurabilityPolicy.TRANSIENT_LOCAL, queue_size = qos_profile.depth)
 
 
     def create_subscription(self, msg_type, topic, callback, qos_profile, **kwargs):
@@ -82,7 +82,7 @@ def create_subscription(self, msg_type, topic, callback, qos_profile, **kwargs):
         Reference:
         https://docs.ros2.org/latest/api/rclpy/api/node.html#rclpy.node.Node.create_subscription
         """
-        return rospy.Subscriber(name = topic, data_class = msg_type, callback = callback, queue_size = qos_profile.depth)
+        return rospy.Subscriber(name = topic, data_class = msg_type, callback = callback, queue_size = qos_profile.depth, tcp_nodelay = qos_profile.reliability == ReliabilityPolicy.BEST_EFFORT)
 
 
     def create_rate(self, frequency, **kwargs):

autopsy/ros1_qos.py

@@ -25,6 +25,22 @@ class DurabilityPolicy(Enum):
     VOLATILE = 2
 
 
+######################
+# QoSReliablityPolicy
+######################
+
+class ReliabilityPolicy(Enum):
+    """Enum to mimic ROS2 `ReliabilityPolicy`.
+
+    Reference:
+    https://docs.ros2.org/foxy/api/rclpy/api/qos.html#rclpy.qos.ReliabilityPolicy
+    """
+
+    SYSTEM_DEFAULT = 0
+    RELIABLE = 1
+    BEST_EFFORT = 2
+
+
 ######################
 # QoSProfile class
 ######################
@@ -37,7 +53,10 @@ class QoSProfile(object):
     https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html
     """
 
-    def __init__(self, depth, durability = DurabilityPolicy.SYSTEM_DEFAULT, **kwargs):
+    def __init__(self, depth,
+            durability = DurabilityPolicy.SYSTEM_DEFAULT,
+            reliability = ReliabilityPolicy.SYSTEM_DEFAULT,
+        **kwargs):
         """Initialize the class.
 
         Arguments:
@@ -50,3 +69,4 @@ def __init__(self, depth, durability = DurabilityPolicy.SYSTEM_DEFAULT, **kwargs
         """
         self.depth = depth
         self.durability = durability
+        self.reliability = reliability

package.xml

@@ -4,7 +4,7 @@
   schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>autopsy</name>
-  <version>0.8.1</version>
+  <version>0.9.0</version>
   <description>A set of Python utils for F1Tenth project.</description>
 
   <maintainer email="[email protected]">Jaroslav Klapálek</maintainer>