armw 0.1.0a0__tar.gz

armw-0.1.0a0/.gitignore

@@ -0,0 +1,5 @@
+armw.egg-info
+.idea
+build
+
+armw/_version.py

armw-0.1.0a0/PKG-INFO

@@ -0,0 +1,16 @@
+Metadata-Version: 2.1
+Name: armw
+Version: 0.1.0a0
+Summary: ARMW
+Author-email: Nathan McGuire <nmcguire@whoi.edu>
+Project-URL: homepage, http://acomms.whoi.edu
+Keywords: Library,Python Modules,Scientific/Engineering
+Classifier: Development Status :: 5 - Production/Stable
+Classifier: Intended Audience :: Developers
+Classifier: Intended Audience :: Science/Research
+Classifier: Programming Language :: Python :: 3.6
+Classifier: Topic :: Software Development :: Libraries :: Python Modules
+Classifier: Topic :: Scientific/Engineering
+Classifier: License :: OSI Approved :: GNU Lesser General Public License v3 or later (LGPLv3+)
+Requires-Python: >=3.6
+Description-Content-Type: text/plain

armw-0.1.0a0/README.md

@@ -0,0 +1,3 @@
+# Agnostic Robot Middleware
+
+A shim layer that will let the same code run in ROS1 and ROS2

armw-0.1.0a0/armw/__init__.py

@@ -0,0 +1,178 @@
+import armw.globals
+import armw.base_node
+import armw.base_parameter_server
+import armw.doc_int_enum
+
+# Figure out the middleware version
+
+MIDDLEWARE = ""
+
+try:
+    import rospy
+
+    MIDDLEWARE = "ros1"
+except:
+    pass
+
+try:
+    import rclpy
+
+    MIDDLEWARE = "ros2"
+except:
+    pass
+
+# Do some stuff with it
+if MIDDLEWARE == "ros1":
+    import armw.ros_common
+    from armw.ros1.ros1_node import Ros1Node
+    from armw.ros1.ros1_parameter_server import Ros1ParameterServer
+    import armw.ros1.interfaces
+
+    # function and class names
+    ArmwNode = armw.ros1.ros1_node.Ros1Node
+    ParameterServer = armw.ros1.ros1_parameter_server.Ros1ParameterServer
+    Publisher = rospy.Publisher
+
+    Time = rospy.Time
+    Duration = rospy.Duration
+    Rate = rospy.Rate
+    AnyMsg = rospy.AnyMsg
+
+    InterruptException = rospy.ROSInterruptException
+
+    main = armw.ros1.ros1_node.main
+    is_shutdown = rospy.is_shutdown
+    sleep = rospy.sleep
+    get_time = rospy.get_time
+    import_message = armw.ros1.interfaces.import_message
+    import_service_ = armw.ros1.interfaces.import_service_
+    deserialze_anymsg = armw.ros1.interfaces.deserialze_anymsg
+    canonicalize_name = rospy.names.canonicalize_name
+    package_and_msg_name = armw.ros_common.package_and_msg_name
+
+    # Log levels
+    DEBUG = rospy.DEBUG
+    INFO = rospy.INFO
+    WARN = rospy.WARN
+    ERROR = rospy.ERROR
+elif MIDDLEWARE == "ros2":
+    import rclpy.publisher
+
+    import time
+    import armw.ros_common
+    import armw.ros2.ros2_node
+    import armw.ros2.ros2_parameter_server
+    import armw.ros2.interfaces
+    import armw.ros2.time
+    import armw.ros2.util
+
+    # function and class names
+    ArmwNode = armw.ros2.ros2_node.Ros2Node
+    ParameterServer = armw.ros2.ros2_parameter_server.Ros2ParameterServer
+    Publisher = rclpy.publisher.Publisher
+
+    Time = armw.ros2.time.Time
+    Duration = armw.ros2.time.Duration
+    Rate = armw.ros2.time.Rate
+    AnyMsg = None
+
+    InterruptException = Exception
+
+    main = armw.ros2.ros2_node.main
+    is_shutdown = lambda: not rclpy.ok()
+    sleep = time.sleep
+    get_time = armw.ros2.time.get_time
+    import_message = armw.ros2.interfaces.import_message
+    import_service_ = armw.ros2.interfaces.import_service_
+    deserialze_anymsg = armw.ros2.interfaces.deserialze_anymsg
+    canonicalize_name = armw.ros2.util.canonicalize_name
+    package_and_msg_name = armw.ros_common.package_and_msg_name
+
+    # Log levels
+    DEBUG = 1
+    INFO = 2
+    WARN = 3
+    ERROR = 4
+else:
+    print("No middleware available")
+    exit(1)
+
+ParameterEnum = armw.doc_int_enum.DocIntEnum
+
+
+def import_service(package_name: str, service_name: str):
+    if service_name.endswith("Request"):
+        service_name = service_name[:-7]
+        _, _, req = armw.import_service_(package_name, service_name)
+        return req
+    elif service_name.endswith("Response"):
+        service_name = service_name[:-8]
+        _, resp, _ = armw.import_service_(package_name, service_name)
+        return resp
+    else:
+        return armw.import_service_(package_name, service_name)
+
+
+def import_message_from_name(name):
+    pkg, msg = armw.package_and_msg_name(name)
+    return armw.import_message(pkg, msg)
+
+
+def NODE() -> armw.base_node.BaseNode:
+    return armw.globals.NODE
+
+
+def fill_time(dest_object, source_object) -> armw.Time:
+    """
+    Attempts to get around some time weirdness by doing lots of python weirdness
+    """
+
+    seconds_fields = ["sec", "secs", "seconds"]
+    nanoseconds_fields = ["nsec", "nsecs", "nanosec", "nanosecs", "nanoseconds"]
+
+    seconds = None
+    nanoseconds = None
+
+    for field in seconds_fields:
+        if hasattr(source_object, field):
+            seconds = getattr(source_object, field)
+            break
+
+    for field in nanoseconds_fields:
+        if hasattr(source_object, field):
+            nanoseconds = getattr(source_object, field)
+            break
+
+    if seconds is None:
+        raise Exception(f"Source {source_object} does not have field encoding seconds")
+    if nanoseconds is None:
+        raise Exception(f"Source {source_object} does not have field encoding nanoseconds")
+
+    wrote_seconds = False
+    wrote_nanoseconds = False
+
+    for field in seconds_fields:
+        if hasattr(dest_object, field):
+            setattr(dest_object, field, seconds)
+            wrote_seconds = True
+            break
+
+    for field in nanoseconds_fields:
+        if hasattr(dest_object, field):
+            setattr(dest_object, field, nanoseconds)
+            wrote_nanoseconds = True
+            break
+
+    if not wrote_seconds:
+        raise Exception(f"Destination {dest_object} does not have field encoding seconds")
+    if not wrote_nanoseconds:
+        raise Exception(f"Destination {source_object} does not have field encoding nanoseconds")
+
+    return dest_object
+
+
+def get_time_object(time_object):
+    if "duration" in str(type(time_object)).lower():
+        return fill_time(armw.Duration(), time_object)
+    else:
+        return fill_time(armw.Time(), time_object)

armw-0.1.0a0/armw/_version.py

@@ -0,0 +1,16 @@
+# file generated by setuptools_scm
+# don't change, don't track in version control
+TYPE_CHECKING = False
+if TYPE_CHECKING:
+    from typing import Tuple, Union
+    VERSION_TUPLE = Tuple[Union[int, str], ...]
+else:
+    VERSION_TUPLE = object
+
+version: str
+__version__: str
+__version_tuple__: VERSION_TUPLE
+version_tuple: VERSION_TUPLE
+
+__version__ = version = '0.1.0a0'
+__version_tuple__ = version_tuple = (0, 1, 0)

armw-0.1.0a0/armw/base_node.py

@@ -0,0 +1,110 @@
+"""
+Base node class that contains all the interfaces needed for the abstract middleware
+"""
+
+import armw.globals
+
+
+class BaseNode(object):
+    def __init__(self):
+        # Set the global node object before we run the rest of the constructor
+        armw.globals.NODE = self
+
+        self.update_rate = 1
+
+    def spin(self):
+        raise NotImplementedError
+
+    def loop_once(self):
+        pass
+
+    def shutdown(self):
+        pass
+
+    def log(self, level, message):
+        """
+        Abstract function to log to console
+        :param level: 0 for info, 1 for warn, 2 for error
+        :param message: String to log
+        :return: None
+        """
+        raise NotImplementedError
+
+    def log_debug(self, message, *args):
+        self.log(-1, message % args)
+
+    def log_info(self, message, *args):
+        self.log(0, message % args)
+
+    def log_warn(self, message, *args):
+        self.log(1, message % args)
+
+    def log_error(self, message, *args):
+        self.log(2, message % args)
+
+    def log_fatal(self, message, *args):
+        self.log(3, message % args)
+
+    def log_throttle(self, interval: int, level: int, message: str):
+        raise NotImplementedError
+
+    def log_debug_throttle(self, interval, message, *args):
+        self.log_throttle(interval, -1, message % args)
+
+    def log_info_throttle(self, interval, message, *args):
+        self.log_throttle(interval, 0, message % args)
+
+    def log_warn_throttle(self, interval, message, *args):
+        self.log_throttle(interval, 1, message % args)
+
+    def log_error_throttle(self, interval, message, *args):
+        self.log_throttle(interval, 2, message % args)
+
+    def get_name(self):
+        raise NotImplementedError
+
+    def get_namespace(self):
+        raise NotImplementedError
+
+    def search_param(self, parameter_name):
+        raise NotImplementedError
+
+    def has_param(self, parameter_name):
+        raise NotImplementedError
+
+    def read_param(self, parameter_name):
+        """
+        Virtual method.
+        Override this for each middleware type
+        """
+        raise NotImplementedError
+
+    def get_param(self, parameter_name, default=None):
+        """
+        Returns the value for a given parameter name, or None if it can't find it
+        """
+        raise NotImplementedError
+
+    def set_param(self, parameter_name, value):
+        raise NotImplementedError
+
+    def delete_param(self, parameter_name):
+        raise NotImplementedError
+
+    def publish(self, topic: str, message_type, queue_size=1, latch=False):
+        raise NotImplementedError
+
+    def subscribe(self, topic: str, message_type, callback, queue_size=1):
+        raise NotImplementedError
+
+    def create_service_server(self, name: str, service_type, callback):
+        raise NotImplementedError
+
+    def create_service_client(self, name: str, service_type):
+        raise NotImplementedError
+
+    def wait_for_message(self, topic, message_type, timeout=None):
+        raise NotImplementedError
+
+    def wait_for_service(self, name, timeout=None):
+        raise NotImplementedError

armw-0.1.0a0/armw/base_parameter_server.py

@@ -0,0 +1,20 @@
+"""
+Base class to provide a reasonable API to set up runtime-reconfigurable parameters
+"""
+
+from armw.base_node import BaseNode
+
+
+class BaseParameterServer(object):
+    def __init__(self, node_object: BaseNode, namespace: str = ""):
+        self.node = node_object
+        self.namespace = namespace
+
+    def add_parameter(self, name, description, default_value, min_value=None, max_value=None, parameter_type=None):
+        raise NotImplementedError
+
+    def start(self, callback):
+        raise NotImplementedError
+
+
+

armw-0.1.0a0/armw/doc_int_enum.py

@@ -0,0 +1,64 @@
+from enum import IntEnum
+
+
+class DocIntEnum(IntEnum):
+    def __new__(cls, value, doc=None):
+        self = int.__new__(cls, value)  # calling super().__new__(value) here would fail
+        self._value_ = value
+        if doc is not None:
+            self.__doc__ = doc
+        else:
+            self.__doc__ = ''
+        return self
+
+    @property
+    def info(self):
+        return f'{self.__class__}:{self.name}:[{self.value}]: {self.__doc__}'
+
+    @classmethod
+    def _get_field_index(cls, key: int):
+        try:
+            member_list = list(cls.__members__.items())
+            for i in range(len(member_list)):  # Have to search through, because we might not have sequential values
+                member = member_list[i]
+                value = int(member[1])
+                if value == key:
+                    return i
+            return None
+        except IndexError:
+            return None
+
+    @classmethod
+    def get_field(cls, key: int):
+        index = cls._get_field_index(key)
+
+        if index is not None:
+            return list(cls.__members__.items())[index][1]
+        else:
+            return None
+
+    @classmethod
+    def get_field_names(cls):
+        fields = list(cls.__members__.items())
+        return [str(field[0]) for field in fields]
+
+    @classmethod
+    def get_field_name(cls, key: int):
+        index = cls._get_field_index(key)
+
+        if index is not None:
+            return list(cls.__members__.items())[index][0]
+        else:
+            return None
+
+    @classmethod
+    def get_description(cls, key: int):
+        """
+        Attempts to return the best description of a field that it can
+        """
+
+        doc = cls.get_field(key).__doc__
+        if len(doc) > 0:
+            return doc
+        else:
+            return cls.get_field_name(key)

armw-0.1.0a0/armw/globals.py

@@ -0,0 +1,2 @@
+NODE = None
+NODE_NAME = ""

armw-0.1.0a0/armw/ros1/interfaces.py

@@ -0,0 +1,27 @@
+"""
+Functions to handle importing interface definitions
+"""
+
+import importlib
+
+import roslib.message
+
+
+def import_message(package_name: str, message_name: str):
+    return getattr(importlib.import_module(f"{package_name}.msg"), message_name)
+
+
+def import_service_(package_name: str, service_name: str):
+    service_class = getattr(importlib.import_module(f"{package_name}.srv"), service_name)
+    service_request = getattr(importlib.import_module(f"{package_name}.srv"), f"{service_name}Request")
+    service_response = getattr(importlib.import_module(f"{package_name}.srv"), f"{service_name}Response")
+
+    return service_class, service_response, service_request
+
+
+def deserialze_anymsg(msg_data):
+    topic_type = msg_data._connection_header['type']
+    topic_class = roslib.message.get_message_class(topic_type)
+    msg = topic_class()
+    msg.deserialize(msg_data._buff)
+    return msg

armw-0.1.0a0/armw/ros1/ros1_node.py

@@ -0,0 +1,112 @@
+"""
+ROS1 version
+"""
+
+import rospy
+
+import armw.globals
+from armw.base_node import BaseNode
+
+
+class Ros1Node(BaseNode):
+    def __init__(self):
+        super().__init__()
+
+    def spin(self):
+        # Issue status periodically.
+        rate = armw.Rate(self.update_rate)
+        while not armw.is_shutdown():
+            self.loop_once()
+            rate.sleep()
+        self.shutdown()
+
+    def log(self, level: int, message: str):
+        if level == -1:
+            rospy.logdebug(message)
+        elif level == 0:
+            rospy.loginfo(message)
+        elif level == 1:
+            rospy.logwarn(message)
+        elif level == 2:
+            rospy.logerr(message)
+        elif level == 3:
+            rospy.logfatal(message)
+
+    def log_throttle(self, interval: int, level: int, message: str):
+        if level == -1:
+            rospy.logdebug_throttle(interval, message)
+        elif level == 0:
+            rospy.loginfo_throttle(interval, message)
+        elif level == 1:
+            rospy.logwarn_throttle(interval, message)
+        elif level == 2:
+            rospy.logerr_throttle(interval, message)
+
+    def get_name(self):
+        return rospy.get_name()
+
+    def get_namespace(self):
+        return rospy.get_namespace()
+
+    def search_param(self, parameter_name):
+        return rospy.search_param(parameter_name)
+
+    def has_param(self, parameter_name):
+        return rospy.has_param(parameter_name)
+
+    def get_param(self, parameter_name, default=None):
+        """
+        Returns the value for a given parameter name, or None if it can't find it
+        """
+        return rospy.get_param(parameter_name, default)
+
+    def set_param(self, parameter_name, value):
+        rospy.set_param(parameter_name, value)
+
+    def delete_param(self, parameter_name):
+        rospy.delete_param(parameter_name)
+
+    def publish(self, topic: str, message_type, queue_size=1, latch=False):
+        return rospy.Publisher(topic, message_type, queue_size=queue_size, latch=latch)
+
+    def subscribe(self, topic: str, message_type, callback, queue_size=1):
+        if message_type is None:
+            from rospy import AnyMsg
+            message_type = AnyMsg
+
+        return rospy.Subscriber(topic, message_type, callback)
+
+    def create_service_server(self, name, service_type, callback):
+        return rospy.Service(name, service_type, callback)
+
+    def create_service_client(self, name: str, service_type):
+        return rospy.ServiceProxy(name, service_type)
+
+    def wait_for_message(self, topic, message_type, timeout=None):
+        if message_type is None:
+            from rospy import AnyMsg
+            message_type = AnyMsg
+
+        if timeout is None:
+            return rospy.wait_for_message(topic, message_type)
+        else:
+            return rospy.wait_for_message(topic, message_type, timeout=timeout)
+
+    def wait_for_service(self, name, timeout=None):
+        if timeout is None:
+            rospy.wait_for_service(name)
+        else:
+            return rospy.wait_for_service(name, timeout=timeout)
+
+
+def main(node_class: type(BaseNode), node_name: str):
+    rospy.init_node(node_name)
+
+    armw.globals.NODE_NAME = node_name
+    armw.globals.NODE = node_class()
+
+    try:
+        armw.globals.NODE.spin()
+        rospy.loginfo(f"{node_name} shutdown")
+    except rospy.ROSInterruptException:
+        rospy.loginfo(f"{node_name} shutdown (interrupt)")

armw-0.1.0a0/armw/ros1/ros1_parameter_server.py

@@ -0,0 +1,27 @@
+import inspect
+
+from ddynamic_reconfigure_python.ddynamic_reconfigure import DDynamicReconfigure
+
+from armw.base_node import BaseNode
+from armw.doc_int_enum import DocIntEnum
+from armw.base_parameter_server import BaseParameterServer
+
+
+class Ros1ParameterServer(BaseParameterServer):
+    def __init__(self, node_object: BaseNode, namespace: str = ""):
+        super().__init__(node_object, namespace)
+
+        self.server = DDynamicReconfigure(namespace)
+
+    def add_parameter(self, name, description, default_value, min_value=None, max_value=None, parameter_type=None):
+        edit_method = ""
+        if inspect.isclass(parameter_type) and issubclass(parameter_type, DocIntEnum):
+            option_list = []
+            for value in parameter_type:
+                option_list.append(self.server.const(parameter_type.get_field_name(value), "int", int(value), parameter_type.get_description(value)))
+            edit_method = self.server.enum(option_list, parameter_type.__name__)
+
+        self.server.add_variable(name, description, default=default_value, min=min_value, max=max_value, edit_method=edit_method)
+
+    def start(self, callback):
+        self.server.start(callback)

armw-0.1.0a0/armw/ros2/interfaces.py

@@ -0,0 +1,18 @@
+"""
+Functions to handle importing interface definitions
+"""
+
+import importlib
+
+
+def import_message(package_name: str, message_name: str):
+    return getattr(importlib.import_module(f"{package_name}.msg"), message_name)
+
+
+def import_service_(package_name: str, service_name: str):
+    service_class = getattr(importlib.import_module(f"{package_name}.srv"), service_name)
+    return service_class, service_class.Response, service_class.Request
+
+
+def deserialze_anymsg(msg_data):
+    return msg_data

armw-0.1.0a0/armw/ros2/ros2_node.py

@@ -0,0 +1,205 @@
+"""
+ROS2 version
+"""
+
+import time
+import rclpy
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, QoSDurabilityPolicy
+
+from armw.base_node import BaseNode
+import armw.globals
+import armw.ros2.wait_for_message
+
+executor: rclpy.executors.MultiThreadedExecutor = None
+
+
+class ServiceProxy(object):
+    def __init__(self, service_name, service_type, callback_group):
+        self._client = armw.NODE().create_client(service_type, service_name, callback_group=callback_group)
+        self.service_class = service_type.Request
+
+    # This causes a crash, so I'm going to leave it off
+    # def __del__(self):
+    #     armw.NODE().destroy_client(self._client)
+
+    def __call__(self, *args, **kwargs):
+        if len(args) > 0:
+            req = args[0]
+        else:
+            req = self.service_class(**kwargs)
+
+        future = self._client.call_async(req)
+        executor.spin_until_future_complete(future)
+        return future.result()
+
+
+class Ros2Node(BaseNode, Node):
+    def __init__(self):
+        # Run parent constructors
+        Node.__init__(self, armw.globals.NODE_NAME, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True)
+        BaseNode.__init__(self)
+
+        self.timer_callback_group = MutuallyExclusiveCallbackGroup()
+        self.service_callback_group = MutuallyExclusiveCallbackGroup()
+
+        # Need so do some silly stuff to make services work
+        self.service_callbacks = {}
+
+    def spin(self):
+        timer_period = 1.0 / self.update_rate
+        self.timer = self.create_timer(timer_period, self.loop_once, callback_group=self.timer_callback_group)
+
+    def log(self, level: int, message: str):
+        if level == -1:
+            self.get_logger().debug(message)
+        elif level == 0:
+            self.get_logger().info(message)
+        elif level == 1:
+            self.get_logger().warn(message)
+        elif level == 2:
+            self.get_logger().error(message)
+        elif level == 3:
+            self.get_logger().fatal(message)
+
+    def log_throttle(self, interval: int, level: int, message: str):
+        if level == -1:
+            self.get_logger().debug(message, throttle_duration_sec=interval)
+        elif level == 0:
+            self.get_logger().info(message, throttle_duration_sec=interval)
+        elif level == 1:
+            self.get_logger().warn(message, throttle_duration_sec=interval)
+        elif level == 2:
+            self.get_logger().error(message, throttle_duration_sec=interval)
+        elif level == 3:
+            self.get_logger().fatal(message, throttle_duration_sec=interval)
+
+    def get_name(self):
+        return Node.get_name(self)
+
+    def get_namespace(self):
+        return Node.get_namespace(self)
+
+    def search_param(self, parameter_name):
+        """
+        This isn't really a thing in ROS2
+        """
+
+        return parameter_name
+
+    def get_param(self, parameter_name, default=None):
+        """
+        Returns the value for a given parameter name, or None if it can't find it
+        """
+
+        if parameter_name[0] == "~":
+            parameter_name = parameter_name[1:]
+        parameter_name = parameter_name.replace("/", ".")
+
+        try:
+            param_value = self.get_parameter(parameter_name).get_parameter_value()
+            if param_value.type == 0:  # Parameter not set
+                return default
+            return rclpy.parameter.parameter_value_to_python(param_value)
+        except Exception as e:
+            self.log_error(f"Unable to read parameter {parameter_name}: {e}")
+            return default
+
+    def set_param(self, parameter_name, value):
+        if parameter_name[0] == "~":
+            parameter_name = parameter_name[1:]
+        parameter_name = parameter_name.replace("/", ".")
+
+        parameter = rclpy.Parameter(parameter_name, rclpy.Parameter.Type.from_parameter_value(value), value)
+        self.set_parameters([parameter])
+
+    def fix_topic_name(self, topic):
+        if topic[0] == "~":
+            topic = self.get_name() + topic[1:]
+
+        return topic
+
+    def publish(self, topic: str, message_type, queue_size=1, latch=False):
+        if latch:
+            qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
+        else:
+            qos = queue_size
+
+        return self.create_publisher(message_type, self.fix_topic_name(topic), qos)
+
+    def subscribe(self, topic: str, message_type, callback, queue_size=1):
+        if message_type is None:
+            from ros2topic.api import get_msg_class
+            message_type = get_msg_class(self, topic, blocking=True)
+
+        return self.create_subscription(message_type, self.fix_topic_name(topic), callback, queue_size)
+
+    def create_service_server(self, name, service_type, callback):
+        self.service_callbacks[name] = callback
+
+        return self.create_service(service_type, name, lambda req, resp: self.service_callback(req, resp, name))
+
+    def service_callback(self, request, response, name):
+        response = self.service_callbacks[name](request)
+        return response
+
+    def create_service_client(self, name: str, service_type):
+        # return self.create_client(service_type, name)
+        return ServiceProxy(name, service_type, self.service_callback_group)
+
+    def wait_for_message(self, topic_name, topic_type, timeout=None):
+        if timeout is None:
+            time_to_wait = 100000  # Essentially forever
+        else:
+            time_to_wait = timeout
+
+        self.log_warn(f"Waiting for message on {topic_name} for {time_to_wait} seconds")
+
+        if topic_type is None:
+            from ros2topic.api import get_msg_class
+            topic_type = get_msg_class(self, topic_name, blocking=True)
+
+        success, msg = armw.ros2.wait_for_message.wait_for_message(topic_type, self, topic_name, time_to_wait=time_to_wait)
+
+        self.log_warn(f"Got message on {topic_name}")
+        return msg
+
+    def wait_for_service(self, service_name, timeout=None):
+        """
+        This might not work correctly yet
+        """
+
+        # Resolve the name
+        if service_name[0] != "/":
+            service_name = self.get_namespace() + "/" + service_name
+
+        # Wait
+        self.log_warn(f"Waiting for service {service_name}")
+        while True:
+            for service in self.get_service_names_and_types():
+                if service[0] == service_name:
+                    self.log_warn(f"Got service {service_name}")
+                    return
+            time.sleep(0.1)
+
+
+def main(node_class, node_name):
+    global executor
+
+    rclpy.init()
+    executor = rclpy.executors.MultiThreadedExecutor()
+
+    armw.globals.NODE_NAME = node_name
+    armw.globals.NODE = node_class()
+
+    try:
+        armw.globals.NODE.spin()
+        executor.add_node(armw.globals.NODE)
+        executor.spin()
+        rclpy.shutdown()
+    except KeyboardInterrupt:
+        pass
+
+    print(f"{node_name} shutdown")
+    raise Exception("Ayy")  # For testing

armw-0.1.0a0/armw/ros2/ros2_parameter_server.py

@@ -0,0 +1,44 @@
+import inspect
+from typing import List
+
+import rclpy
+from rcl_interfaces.msg import SetParametersResult
+
+from armw.ros2.ros2_node import Ros2Node
+from armw.doc_int_enum import DocIntEnum
+from armw.base_parameter_server import BaseParameterServer
+
+
+class Ros2ParameterServer(BaseParameterServer):
+    def __init__(self, node_object: Ros2Node, namespace: str = ""):
+        super().__init__(node_object, namespace)
+        self.node = node_object
+
+        self.parameter_values = {}
+
+        self.callback = None
+
+    def add_parameter(self, name, description, default_value, min_value=None, max_value=None, parameter_type=None):
+        if inspect.isclass(parameter_type) and issubclass(parameter_type, DocIntEnum):
+            # TODO: no idea yet
+            pass
+
+        if not self.node.has_parameter(name):
+            self.node.declare_parameter(name, default_value)
+
+        self.parameter_values[name] = self.node.get_parameter(name).value
+
+    def start(self, callback):
+        self.node.add_on_set_parameters_callback(self.parameter_callback)
+
+        self.callback = callback
+
+    def parameter_callback(self, params: List[rclpy.Parameter]):
+        for param in params:
+            self.parameter_values[param.name] = param.value
+
+        if self.callback is not None:
+            self.callback(self.parameter_values, {})
+            return SetParametersResult(successful=True)
+        else:
+            return SetParametersResult(successful=False)

armw-0.1.0a0/armw/ros2/time.py

@@ -0,0 +1,112 @@
+import types
+
+import rclpy
+import rclpy.clock
+import rclpy.duration
+import builtin_interfaces
+import builtin_interfaces.msg
+import armw.globals
+
+
+def get_time():
+    if armw.globals.NODE is not None:
+        return armw.Time().now().to_sec()
+    else:
+        raise Exception("time is not initialized. Have you created a node?")
+
+
+class Duration(rclpy.duration.Duration):
+    def __init__(self, secs=0, nsecs=0):
+        super().__init__(nanoseconds=secs * 1000000000 + nsecs)
+        self.to_nsec = types.MethodType(lambda self: self.nanoseconds, self)
+        self.to_sec = types.MethodType(lambda self: self.nanoseconds / 1e9, self)
+        self.is_zero = types.MethodType(lambda self: self.nanoseconds == 0, self)
+        self.secs = secs
+        self.nsecs = nsecs
+
+    def __add__(self, other):
+        if isinstance(other, Time):
+            return armw.Time().from_sec(self.to_sec() + other.to_sec())
+        else:
+            Duration.__add__(self, other)
+
+    @classmethod
+    def from_sec(cls, secs):
+        return Duration(int(secs), nsecs=int((secs - int(secs)) * 1000000000))
+
+    @classmethod
+    def from_seconds(cls, secs):
+        return Duration(int(secs), nsecs=int((secs - int(secs)) * 1000000000))
+
+
+class Time(builtin_interfaces.msg.Time):
+    def __init__(self, secs=0, nsecs=0):
+        super().__init__(sec=secs, nanosec=nsecs)
+
+    def __add__(self, other):
+        if isinstance(other, rclpy.duration.Duration):
+            return armw.Time().from_sec(self.to_sec() + other.to_sec())
+        else:
+            Time.__add__(self, other)
+
+    def __sub__(self, other):
+        if isinstance(other, rclpy.duration.Duration):
+            return armw.Time().from_sec(self.to_sec() - other.to_sec())
+        else:
+            Time.__add__(self, other)
+
+    def __gt__(self, other):
+        return self.to_sec() > other.to_sec()
+
+    def __lt__(self, other):
+        return self.to_sec() < other.to_sec()
+
+    def __ge__(self, other):
+        return self.to_sec() >= other.to_sec()
+
+    def __le__(self, other):
+        return self.to_sec() <= other.to_sec()
+
+    def __eq__(self, other):
+        return self.to_sec() == other.to_sec()
+
+    def __ne__(self, other):
+        return self.to_sec() != other.to_sec()
+
+    def to_sec(self):
+        return self.sec + float(self.nanosec) / 1000000000
+
+    @classmethod
+    def from_sec(cls, secs: float):
+        return Time(secs=int(secs), nsecs=int((secs - int(secs)) * 1000000000))
+
+    @classmethod
+    def from_seconds(cls, secs: float):
+        return Time().from_sec(secs=secs)
+
+    @classmethod
+    def now(cls):
+        s = rclpy.clock.Clock().now().seconds_nanoseconds()
+        return Time(secs=s[0], nsecs=s[1])
+
+
+class Rate(object):
+    def __init__(self, hz):
+        self._rate = armw.globals.NODE.create_rate(hz)
+
+    def __del__(self):
+        armw.globals.NODE.destroy_rate(self._rate)
+
+    def sleep(self):
+        self._rate.sleep()
+
+
+if __name__ == '__main__':
+    a = Time(3, 5)
+    b = Duration(5, 6)
+
+    print(a)
+    print(b)
+
+    print(a + b)
+    print(b + a)

armw-0.1.0a0/armw/ros2/util.py

@@ -0,0 +1,18 @@
+def canonicalize_name(name):
+    """
+    Put name in canonical form. Double slashes '//' are removed and
+    name is returned without any trailing slash, e.g. /foo/bar
+    @param name: ROS name
+    @type  name: str
+    """
+
+    # Straight up copied from here: https://docs.ros.org/en/melodic/api/rospy/html/rospy.names-pysrc.html
+
+    SEP = '/'
+
+    if not name or name == SEP:
+        return name
+    elif name[0] == SEP:
+        return '/' + '/'.join([x for x in name.split(SEP) if x])
+    else:
+        return '/'.join([x for x in name.split(SEP) if x])

armw-0.1.0a0/armw/ros2/wait_for_message.py

@@ -0,0 +1,52 @@
+# Straight up copied from the ros2 rolling distro
+
+from typing import Union
+
+from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
+from rclpy.signals import SignalHandlerGuardCondition
+from rclpy.utilities import timeout_sec_to_nsec
+
+
+def wait_for_message(msg_type, node: 'Node', topic: str, *, qos_profile: Union[QoSProfile, int] = 1, time_to_wait=-1):
+    """
+    Wait for the next incoming message.
+
+    :param msg_type: message type
+    :param node: node to initialize the subscription on
+    :param topic: topic name to wait for message
+    :param qos_profile: QoS profile to use for the subscription
+    :param time_to_wait: seconds to wait before returning
+    :returns: (True, msg) if a message was successfully received, (False, None) if message
+        could not be obtained or shutdown was triggered asynchronously on the context.
+    """
+    context = node.context
+    wait_set = _rclpy.WaitSet(1, 1, 0, 0, 0, 0, context.handle)
+    wait_set.clear_entities()
+
+    sub = node.create_subscription(msg_type, topic, lambda _: None, qos_profile=qos_profile)
+    try:
+        wait_set.add_subscription(sub.handle)
+        sigint_gc = SignalHandlerGuardCondition(context=context)
+        wait_set.add_guard_condition(sigint_gc.handle)
+
+        timeout_nsec = timeout_sec_to_nsec(time_to_wait)
+        wait_set.wait(timeout_nsec)
+
+        subs_ready = wait_set.get_ready_entities('subscription')
+        guards_ready = wait_set.get_ready_entities('guard_condition')
+
+        if guards_ready:
+            if sigint_gc.handle.pointer in guards_ready:
+                return False, None
+
+        if subs_ready:
+            if sub.handle.pointer in subs_ready:
+                msg_info = sub.handle.take_message(sub.msg_type, sub.raw)
+                if msg_info is not None:
+                    return True, msg_info[0]
+    finally:
+        node.destroy_subscription(sub)
+
+    return False, None

armw-0.1.0a0/armw/ros_common.py

@@ -0,0 +1,13 @@
+def package_and_msg_name(name):
+    """
+    https://docs.ros.org/en/diamondback/api/roslib/html/python/roslib.names-pysrc.html#package_resource_name
+    """
+
+    if "/" in name:
+        val = tuple(name.split("/"))
+        if len(val) != 2:
+            raise ValueError("invalid name [%s]" % name)
+        else:
+            return val
+    else:
+        return '', name

armw-0.1.0a0/armw.egg-info/PKG-INFO

@@ -0,0 +1,16 @@
+Metadata-Version: 2.1
+Name: armw
+Version: 0.1.0a0
+Summary: ARMW
+Author-email: Nathan McGuire <nmcguire@whoi.edu>
+Project-URL: homepage, http://acomms.whoi.edu
+Keywords: Library,Python Modules,Scientific/Engineering
+Classifier: Development Status :: 5 - Production/Stable
+Classifier: Intended Audience :: Developers
+Classifier: Intended Audience :: Science/Research
+Classifier: Programming Language :: Python :: 3.6
+Classifier: Topic :: Software Development :: Libraries :: Python Modules
+Classifier: Topic :: Scientific/Engineering
+Classifier: License :: OSI Approved :: GNU Lesser General Public License v3 or later (LGPLv3+)
+Requires-Python: >=3.6
+Description-Content-Type: text/plain

armw-0.1.0a0/armw.egg-info/SOURCES.txt

@@ -0,0 +1,25 @@
+.gitignore
+README.md
+pyproject.toml
+armw/__init__.py
+armw/_version.py
+armw/base_node.py
+armw/base_parameter_server.py
+armw/doc_int_enum.py
+armw/globals.py
+armw/ros_common.py
+armw.egg-info/PKG-INFO
+armw.egg-info/SOURCES.txt
+armw.egg-info/dependency_links.txt
+armw.egg-info/top_level.txt
+armw/ros1/__init__.py
+armw/ros1/interfaces.py
+armw/ros1/ros1_node.py
+armw/ros1/ros1_parameter_server.py
+armw/ros2/__init__.py
+armw/ros2/interfaces.py
+armw/ros2/ros2_node.py
+armw/ros2/ros2_parameter_server.py
+armw/ros2/time.py
+armw/ros2/util.py
+armw/ros2/wait_for_message.py

armw-0.1.0a0/armw.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

armw-0.1.0a0/armw.egg-info/top_level.txt

@@ -0,0 +1 @@
+armw

armw-0.1.0a0/pyproject.toml

@@ -0,0 +1,37 @@
+[project]
+name = "armw"
+dynamic = ["version"]
+description = "ARMW"
+readme = "README.txt"
+keywords = ["Library", "Python Modules", "Scientific/Engineering"]
+classifiers = [
+    "Development Status :: 5 - Production/Stable",
+    "Intended Audience :: Developers",
+    "Intended Audience :: Science/Research",
+    "Programming Language :: Python :: 3.6",
+    "Topic :: Software Development :: Libraries :: Python Modules",
+    "Topic :: Scientific/Engineering",
+    "License :: OSI Approved :: GNU Lesser General Public License v3 or later (LGPLv3+)"
+]
+requires-python = ">=3.6"
+dependencies = []
+
+[[project.authors]]
+name = "Nathan McGuire"
+email = "nmcguire@whoi.edu"
+
+[project.urls]
+homepage = "http://acomms.whoi.edu"
+
+[project.scripts]
+# No scripts for now; we may add scripts in /bin in a future release
+
+[project.optional-dependencies]
+# No optional dependencies
+
+[build-system]
+requires = ["setuptools>=64", "wheel", "setuptools_scm>=8"]
+build-backend = "setuptools.build_meta"
+
+[tool.setuptools_scm]
+write_to = "armw/_version.py"

armw-0.1.0a0/setup.cfg

@@ -0,0 +1,4 @@
+[egg_info]
+tag_build = 
+tag_date = 0
+