First implementation of ROS2 to Mass connector (#1)

README.md

@@ -1 +1,33 @@
-# ros2mass
+# ros2-to-mass-amr-interop
+
+Configuration-based ROS2 package for sending Mass [AMR Standard messages](https://github.com/MassRobotics-AMR/AMR_Interop_Standard) to complaint receivers.
+
+## Installing
+
+Make sure `ros2` is installed properly. Clone this repository inside your `src` folder on your local workspace and build the package:
+
+```bash
+mkdir -p ~/ros2_ws/src && cd ros2_ws/
+git clone https://github.com/inorbit-ai/ros2-to-mass-amr-interop.git ./src
+colcon build --packages-select ros2-to-mass-amr-interop
+```
+
+## Running the node
+
+The node takes the Mass AMR config file path as parameter. If not provided, it is assumed the file is on the current directory.
+
+```bash
+# Source the local overlay by running `. install/setup.sh` if
+# using bash or `. install/setup.zsh` if using zsh.
+ros2 run ros2_to_mass_amr_interop ros2_to_mass_node \
+    --ros-args -p config_file:=/path/to/config.yaml
+```
+
+## Running tests
+
+On you local workspace:
+
+```bash
+colcon test --packages-select ros2-to-mass-amr-interop
+colcon test-result --verbose
+```

package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ros2-to-mass-amr-interop</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">leandro</maintainer>
+  <license>TODO: License declaration</license>
+
+  <exec_depend>python3-websockets</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

ros2_to_mass_amr_interop/__init__.py

@@ -0,0 +1,59 @@
+import logging
+import websockets
+import json
+import asyncio
+from pathlib import Path
+from rclpy.node import Node
+from .config import MassAMRInteropConfig
+from .messages import IdentityReport
+
+logging.basicConfig(level=logging.DEBUG)
+
+
+class MassAMRInteropNode(Node):
+    """ROS node implementing WebSocket communication to Mass.
+
+    The node configuration is obtained from a configuration file
+    that can be provided externally. Then it subscribes to various
+    topics and sends relevant data to a Mass server by using a
+    WebSocket connection.
+
+    Args:
+        Node (obj:`str`, optional): configuration file path. Defaults to `./config.yaml`.
+    """
+    def __init__(self, **kwargs) -> None:
+        super().__init__(node_name=self.__class__.__name__, **kwargs)
+        self.logger = logging.getLogger(self.__class__.__name__)
+
+        self.declare_parameter('config_file', './config.yaml')
+        config_file = self.get_parameter('config_file').get_parameter_value().string_value
+        config_file = Path(config_file).resolve()
+        if not config_file.is_file():
+            raise ValueError(f"Configuration file '{config_file}' doesn't exist!")
+
+        self.logger.info(f"Using configuration file '{config_file}'")
+        self._config = MassAMRInteropConfig(str(config_file))
+
+        self._uri = self._config.server
+        self.logger.debug(f"Connecting to Mass server '{self._uri}'")
+        self._wss_conn = websockets.connect(self._uri)
+        self.logger.debug(f"Connection successful!")
+
+    async def _send_identity_report(self):
+
+        identity_report_data = {
+            'uuid': self._config.get_parameter_value('uuid'),
+            'manufacturer_name': self._config.get_parameter_value('manufacturerName'),
+            'robot_model': self._config.get_parameter_value('robotModel'),
+            'robot_serial_number': self._config.get_parameter_value('robotSerialNumber'),
+            'base_robot_envelop': self._config.get_parameter_value('baseRobotEnvelope'),
+        }
+
+        identity_report = IdentityReport(**identity_report_data)
+
+        async with self._wss_conn as websocket:
+            await websocket.send(json.dumps(identity_report.data))
+
+    def send_identity_report(self):
+        loop = asyncio.get_event_loop()
+        loop.run_until_complete(self._send_identity_report())

ros2_to_mass_amr_interop/config.py

@@ -0,0 +1,135 @@
+import yaml
+import logging
+import os
+
+# Config files may have non static values that are
+# ought to be extracted from a different source
+# other than the config file
+CFG_PARAMETER_STATIC = "static"
+CFG_PARAMETER_ROS_TOPIC = "rosTopic"
+CFG_PARAMETER_ROS_PARAMETER = "rosParameter"
+CFG_PARAMETER_ENVVAR = "envVar"
+
+SUPPORTED_EXTERNAL_VALUES = [
+    CFG_PARAMETER_ROS_TOPIC,
+    CFG_PARAMETER_ROS_PARAMETER,
+    CFG_PARAMETER_ENVVAR
+]
+
+
+class MassAMRInteropConfig:
+    """
+    Configuration file parsing and value gathering.
+
+    Parses yaml configuration file and deals with parameters
+    with values that are not static i.e. parameter values that
+    are obtained from environment variables or ROS2 topics.
+
+    Attributes
+    ----------
+        server (str): Mass WebSocket server URI
+        mapping (:obj:`dict`): parameter name and value mapping
+
+    """
+
+    def __init__(self, path=None) -> None:
+        self.logger = logging.getLogger(__class__.__name__)
+        _config = self._load(path)
+
+        self.server = _config['server']
+        self.mapping = _config['mapping']
+
+    def _load(self, path) -> None:
+        config = dict()
+        with open(path, "r") as fd:
+            try:
+                config = yaml.safe_load(fd)
+            except yaml.YAMLError as ex:
+                self.logger.error("Failed to parse YAML config file", ex)
+
+        self.logger.debug(f"Config file '{path}' loaded")
+
+        # Ignoring config file root key value as it's not relevant
+        k = next(iter(config))
+        config = config[k]
+        return config
+
+    def get_parameter_source(self, name):
+        """
+        Return parameter source.
+
+        Args:
+        ----
+            name (str): parameter name
+
+        Raises
+        ------
+            ValueError: when parameter does not exist or it is invalid
+
+        Returns
+        -------
+            str: parameter source
+
+        """
+        if isinstance(self.mapping[name], str):
+            return CFG_PARAMETER_STATIC
+
+        if isinstance(self.mapping[name], dict):
+            # Evaluate is the parameter is non static
+            if 'valueFrom' in self.mapping[name]:
+                param_source = self.mapping[name]['valueFrom']
+
+                # param_source is a dict whose first key
+                # is the external parameter type
+                if not isinstance(param_source, dict):
+                    raise ValueError(f"Invalid 'valueFrom' configuration: '{name}'")
+                param_source = next(iter(param_source))
+
+                if param_source in SUPPORTED_EXTERNAL_VALUES:
+                    return param_source
+
+        # If no supported external valueFrom configs were found,
+        # assume that the parameter is static if it is an object
+        if isinstance(self.mapping[name], dict):
+            return CFG_PARAMETER_STATIC
+
+        raise ValueError("Invalid parameter")
+
+    def get_parameter_value(self, name):
+        """
+        Return configuration parameter value.
+
+        It also deals with the complexity of getting
+        the value from different sources.
+
+        Args:
+        ----
+            name (Union[str, dict]): configuration parameter value
+
+        Raises
+        ------
+            ValueError: when parameter value does not exist or it is invalid
+
+        Returns
+        -------
+            str: parameter value
+
+        """
+        param_source = self.get_parameter_source(name)
+        self.logger.debug(f"Parameter '{name}' source: {param_source}")
+
+        if param_source == CFG_PARAMETER_STATIC:
+            return self.mapping[name]
+
+        if param_source == CFG_PARAMETER_ENVVAR:
+            envvar_name = self.mapping[name]['valueFrom'][param_source]
+            param_value = os.getenv(envvar_name)
+            if not param_value:
+                raise ValueError(f"Empty or undefined environment variable: '{envvar_name}'")
+            return param_value
+
+        if param_source == CFG_PARAMETER_ROS_TOPIC:
+            return self.mapping[name]['valueFrom'][param_source]
+
+        if param_source == CFG_PARAMETER_ROS_PARAMETER:
+            return self.mapping[name]['valueFrom'][param_source]