From f35d27c0871e680fe61389cfb13488dcaa95ea3d Mon Sep 17 00:00:00 2001 From: Leandro Date: Thu, 17 Jun 2021 11:56:47 -0300 Subject: [PATCH] Initial implementation for status message reporting (#2) --- .gitignore | 3 + .vscode/settings.json | 9 + README.md | 3 +- package.xml | 13 + ros2_to_mass_amr_interop/__init__.py | 349 ++++++++++++++++-- ros2_to_mass_amr_interop/config.py | 61 ++- ros2_to_mass_amr_interop/messages/__init__.py | 176 ++++++++- ...{AMR_Interop_Standard.json => schema.json} | 0 ros2_to_mass_amr_interop/ros2_to_mass_node.py | 3 +- sample_config.yaml | 61 +-- setup.py | 2 + test/test_data/config.yaml | 68 ---- test/test_mass_interop.py | 5 +- test/test_mass_interop_config.py | 29 +- 14 files changed, 600 insertions(+), 182 deletions(-) create mode 100644 .vscode/settings.json rename ros2_to_mass_amr_interop/messages/{AMR_Interop_Standard.json => schema.json} (100%) delete mode 100644 test/test_data/config.yaml diff --git a/.gitignore b/.gitignore index 35d74bb..ac5970d 100644 --- a/.gitignore +++ b/.gitignore @@ -49,3 +49,6 @@ qtcreator-* # Catkin custom files CATKIN_IGNORE + +# Python +.pytest_cache/ \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..aedfc3c --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,9 @@ +{ + "python.pythonPath": "/usr/bin/python3", + "python.testing.pytestArgs": [ + "test" + ], + "python.testing.unittestEnabled": false, + "python.testing.nosetestsEnabled": false, + "python.testing.pytestEnabled": true +} \ No newline at end of file diff --git a/README.md b/README.md index 46c809e..99003ff 100644 --- a/README.md +++ b/README.md @@ -19,8 +19,9 @@ The node takes the Mass AMR config file path as parameter. If not provided, it i ```bash # Source the local overlay by running `. install/setup.sh` if # using bash or `. install/setup.zsh` if using zsh. +# Also, install dependencies by running `rosdep install --ignore-src --from-paths src/` ros2 run ros2_to_mass_amr_interop ros2_to_mass_node \ - --ros-args -p config_file:=/path/to/config.yaml + --ros-args -p config_file:=/path/to/config.yaml --log-level debug ``` ## Running tests diff --git a/package.xml b/package.xml index 490f232..ae2186a 100644 --- a/package.xml +++ b/package.xml @@ -9,11 +9,24 @@ python3-websockets rclpy + tf2 + tf2_bullet + tf2_eigen + tf2_eigen_kdl + tf2_geometry_msgs + tf2_kdl + tf2_msgs + tf2_py + tf2_ros + tf2_sensor_msgs + tf2_tools ament_copyright ament_flake8 ament_pep257 python3-pytest + python3-pep8 + python3-jsonschema ament_python diff --git a/ros2_to_mass_amr_interop/__init__.py b/ros2_to_mass_amr_interop/__init__.py index 4f79388..ca4fd74 100644 --- a/ros2_to_mass_amr_interop/__init__.py +++ b/ros2_to_mass_amr_interop/__init__.py @@ -1,59 +1,336 @@ -import logging import websockets import json import asyncio +from time import sleep +from concurrent.futures import ThreadPoolExecutor from pathlib import Path +from functools import partial +from tf2_kdl import PyKDL + +from std_msgs import msg as ros_std_msgs +from geometry_msgs import msg as ros_geometry_msgs +from sensor_msgs import msg as ros_sensor_msgs + from rclpy.node import Node +from .config import CFG_PARAMETER_LOCAL +from .config import CFG_PARAMETER_ENVVAR +from .config import CFG_PARAMETER_ROS_TOPIC +from .config import CFG_PARAMETER_ROS_PARAMETER # noqa: F401 from .config import MassAMRInteropConfig -from .messages import IdentityReport -logging.basicConfig(level=logging.DEBUG) +from .config import STATUS_REPORT_INTERVAL + +from .messages import MASS_REPORT_UUID +from .messages import IdentityReport +from .messages import StatusReport class MassAMRInteropNode(Node): - """ROS node implementing WebSocket communication to Mass. + """ + ROS node implementing WebSocket communication to Mass. + + The node configuration is obtained from a configuration file that can + be provided externally. On initialization, the node subscribes to various + topics and sends relevant data to a MassRobotics AMR InterOp standard + capable server by using a WebSocket connection. + + Instances for both Identity and Status reports are kept internally as Node + attributes and are updated when relevant data is processed. + TODO: implement watchdog for node configuration file changes. - 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. + Attributes + ---------- + mass_identity_report (IdentityReport): Instance of Mass Identity Report + mass_status_report (StatusReport): Instance of Mass Status Report - 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__) + # Get Node logger instance + self.logger = self.get_logger() + # Declare Node configuration parameter. Defaults to './config.yaml' if no + # ``config_file`` parameter is provided. Provide the parameter when running + # the node by using ``--ros-args -p config_file:=/path/to/config.yaml`` 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)) + config_file_param = self.get_parameter(name='config_file') + config_file_path = config_file_param.get_parameter_value().string_value + self._config = self._read_config_file(config_file_path=config_file_path) + # Websocket connection 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'), + self._wss_conn = None + + # Create an instance of both report types + _uuid = self._config.get_parameter_value(MASS_REPORT_UUID) + self.mass_identity_report = IdentityReport(uuid=_uuid) + self.mass_status_report = StatusReport(uuid=_uuid) + + self._process_config() + + # ThreadPool for running other tasks + self._ex = ThreadPoolExecutor() + + self.loop = asyncio.get_event_loop() + self.loop.run_in_executor(self._ex, self._status_publisher_thread) + self.loop.run_until_complete(self._run()) + + async def _run(self): + await self._async_connect() + await self._async_send_report(self.mass_identity_report) + + def _status_publisher_thread(self): + # The main event loop is only used for running coroutines from + # callbacks. However, it's not possible to start it because it + # blocks the Node thread and the ROS callbacks are never executed. + loop = asyncio.new_event_loop() + self.logger.debug("Starting status publisher thread") + + def send_status(): + while True: + loop.run_until_complete(self._async_send_report(self.mass_status_report)) + self.logger.debug(f"Status report sent. Waiting ...") + sleep(STATUS_REPORT_INTERVAL) + + loop.create_task(send_status()) + + def _process_config(self): + """ + Update object state based on configuration. + + Populate Identity Report object with parameters from configuration + and register callbacks for ROS Topics. + + TODO: re-run on configuration file changes + TODO: deregister callbacks on configuration changes + """ + # Update status report with local parameters + for param_name in self._config.parameters_by_source[CFG_PARAMETER_LOCAL]: + param_value = self._config.get_parameter_value(param_name) + self.mass_identity_report.update_parameter(name=param_name, value=param_value) + + for param_name in self._config.parameters_by_source[CFG_PARAMETER_ENVVAR]: + param_value = self._config.get_parameter_value(param_name) + self.mass_identity_report.update_parameter(name=param_name, value=param_value) + + # Register callbacks for rosTopic parameters + for param_name in self._config.parameters_by_source[CFG_PARAMETER_ROS_TOPIC]: + topic_name = self._config.get_parameter_value(name=param_name) + self.register_mass_adapter(param_name, topic_name) + + async def _async_connect(self): + self.logger.debug(f"Connecting to server '{self._uri}'") + self._wss_conn = await websockets.connect(self._uri) + self.logger.debug(f"Connected to Mass server '{self._uri}'") + return self._wss_conn + + async def _async_send_report(self, mass_object): + """ + Send MassRobotics object to MassRobotics server. + + Sends MassRobotics object data to MassRobotics server + by using WebSockets connection. + + Args: + ---- + mass_object (:obj:`MassObject`): Identity or Status report + + """ + self.logger.debug(f"Sending object ({type(mass_object)}): {mass_object.data}") + try: + await self._wss_conn.ensure_open() + except (Exception, + websockets.exceptions.ConnectionClosed, + websockets.exceptions.ConnectionClosedError): + self.logger.info(f"Reconnecting to server: {self._uri}") + await self._async_connect() + + try: + await self._wss_conn.send(json.dumps(mass_object.data)) + except Exception as ex: + self.logger.info(f"Error while sending status report: {ex}") + + def _read_config_file(self, config_file_path): + config_file_path = Path(config_file_path).resolve() + if not config_file_path.is_file(): + raise ValueError(f"Configuration file '{config_file_path}' doesn't exist!") + + self.logger.info(f"Using configuration file '{config_file_path}'") + return MassAMRInteropConfig(str(config_file_path)) + + def _get_frame_id_from_header(self, msg): + msg_frame_id = msg.header.frame_id + frame_id = self._config.mappings['rosFrameToPlanarDatumUUID'].get(msg_frame_id) + if not frame_id: + self.logger.warning(f"Couldn't find mapping for frame '{msg_frame_id}': {msg}") + frame_id = "00000000-0000-0000-0000-000000000000" + return frame_id + + def _callback_pose_stamped_msg(self, param_name, msg_field, data): + + self.logger.debug(f"Processing '{type(data)}' message: {data}") + if msg_field: + self.logger.warning(f"Parameter {param_name} doesn't support `msgField`. Ignoring.") + + frame_id = self._get_frame_id_from_header(data) + + pose_position = data.pose.position # Point + pose_orientation = data.pose.orientation # Quaternion + self.mass_status_report.data[param_name] = { + "x": pose_position.x, + "y": pose_position.y, + "z": pose_position.z, + "angle": { + "x": pose_orientation.x, + "y": pose_orientation.y, + "z": pose_orientation.z, + "w": pose_orientation.w + }, + "planarDatum": frame_id + } + + def _callback_battery_state_msg(self, param_name, msg_field, data): + + self.logger.debug(f"Processing '{type(data)}' message: {data}") + try: + self.mass_status_report.data[param_name] = getattr(data, msg_field) + except AttributeError: + self.logger.error(f"Message field '{msg_field}' on message of " + f"type '{type(data)}' doesn't exist") + + def _callback_twist_stamped_msg(self, param_name, msg_field, data): + + self.logger.debug(f"Processing '{type(data)}' message: {data}") + if msg_field: + self.logger.warning(f"Parameter {param_name} doesn't support `msgField`. Ignoring.") + + frame_id = self._get_frame_id_from_header(data) + + twist = data.twist + + linear_vel = PyKDL.Vector( + x=twist.linear.x, + y=twist.linear.y, + z=twist.linear.z).Norm() + quat = PyKDL.Rotation.EulerZYX( + Alfa=twist.angular.z, + Beta=twist.angular.y, + Gamma=twist.angular.x).GetQuaternion() + + self.mass_status_report.data[param_name] = { + "linear": linear_vel, + "angle": { + "x": quat[0], + "y": quat[1], + "z": quat[2], + "w": quat[3], + }, + "planarDatum": frame_id } - identity_report = IdentityReport(**identity_report_data) + def _callback_string_msg(self, param_name, msg_field, data): + + self.logger.debug(f"Processing '{type(data)}' message: {data}") + if msg_field: + self.logger.warning(f"Parameter {param_name} doesn't support `msgField`. Ignoring.") + + self.mass_status_report.data[param_name] = data.data + + def register_mass_adapter(self, param_name, topic_name): + """ + Register callbacks for parameters with source ROS topic. + + Creates node callbacks for ROS topics based on ROS topic parameters + defined on the configuration file. The ``msgType`` configuration field + is used determine Node callback message type. As the ``msgType`` is a + string, this method tries to determine message type class as it is + required to register the callback. + + Args + ---- + param_name (str): configuration parameter name + topic_name (str): topic name callback will be register + + Returns + ------- + boolean: wheter callback registration was successful or not + + """ + self.logger.debug(f"Registering callback to topic '{topic_name}'") + + # Topic/message type is expected to contain package name e.g. + # ``geometry_msgs/msg/Twist``. + topic_type = self._config.get_ros_topic_parameter_type(name=param_name) + topic_type_package = topic_type.split("/")[0] + topic_type_name = topic_type.split("/")[-1] + + # In some cases, a single message of certain type may have fields that map + # to multiple MassRobotics report fields. For this scenarios, an additional + # ``msgField`` callback parameter can be provided, which makes the callback + # to extract a single field from the message that is processed. + # For example, a message of type ``sensors_msgs/msg/BatteryState`` contains + # several fields but we are only interested in ``percentage`` as it translates + # directly to MassRobotics status object field ``batteryPercentage``. + # batteryPercentage: + # valueFrom: + # rosTopic: /good_sensors/bat + # msgType: sensor_msgs/msg/BatteryState + # msgField: percentage + msg_field = self._config.get_ros_topic_parameter_msg_field(name=param_name) + + # Map message package with corresponding module + msgs_types = { + 'geometry_msgs': ros_geometry_msgs, + 'sensor_msgs': ros_sensor_msgs, + 'std_msgs': ros_std_msgs + } + + # Try to determine callback message type class + try: + topic_type_t = getattr(msgs_types[topic_type_package], topic_type_name) + except (AttributeError, KeyError): + # If the message type is not supported do not register any callback + self.logger.error(f"Undefined topic type '{topic_type}'. Ignoring...") + return False + + self.logger.debug(f"Binding parameter '{param_name}' with topic '{topic_name}'") + + callback = None + if param_name == 'velocity': + # ros2 topic pub --once /good_sensors/vel geometry_msgs/msg/TwistStamped "{twist: {linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}}" # noqa: E501 + # ros2 topic pub --once /good_sensors/vel geometry_msgs/msg/TwistStamped "{header: {frame_id: 'floor1'}, twist: {linear: {x: 1, y: 2, z: 3}, angular: {x: 1, y: 1, z: 1}}}" # noqa: E501 + callback = partial(self._callback_twist_stamped_msg, param_name, msg_field) + self.logger.info(f"Registerd callback for parameter '{param_name}' (TwistStamped)") + if param_name == 'batteryPercentage': + # ros2 topic pub --once /good_sensors/bat sensor_msgs/msg/BatteryState "{percentage: 91.3}" # noqa: E501 + callback = partial(self._callback_battery_state_msg, param_name, msg_field) + self.logger.info(f"Registerd callback for parameter '{param_name}' (BatteryState)") + if param_name == 'location': + # ros2 topic pub --once /move_base_simple/goal geometry_msgs/msg/PoseStamped "{pose: {position: {x: 2.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 1.8, w: 1}}}" # noqa: E501 + # ros2 topic pub --once /move_base_simple/goal geometry_msgs/msg/PoseStamped "{header: {frame_id: 'floor1'}, pose: {position: {x: 2.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 1.8, w: 1}}}" # noqa: E501 + callback = partial(self._callback_pose_stamped_msg, param_name, msg_field) + self.logger.info(f"Registerd callback for parameter '{param_name}' (PoseStamped)") + + # if param_name doesn't have any specific callback, fallback to string + if not callback and topic_type_t is ros_std_msgs.String: + callback = partial(self._callback_string_msg, param_name, msg_field) + self.logger.info(f"Registered callback for parameter '{param_name}' (String)") + + # TODO: add all remaining 'scalar' types + if not callback and topic_type_t in (ros_std_msgs.Float32, ros_std_msgs.Float64): + callback = partial(self._callback_string_msg, param_name, msg_field) + self.logger.info(f"Registered callback for parameter '{param_name}' (Number)") + + if not callback: + self.logger.error(f"Callback for parameter '{param_name}' " + f"({topic_type}) was not found.") + return False - async with self._wss_conn as websocket: - await websocket.send(json.dumps(identity_report.data)) + self.subscription = self.create_subscription( + msg_type=topic_type_t, + topic=topic_name, + callback=callback, + qos_profile=10) - def send_identity_report(self): - loop = asyncio.get_event_loop() - loop.run_until_complete(self._send_identity_report()) + return True diff --git a/ros2_to_mass_amr_interop/config.py b/ros2_to_mass_amr_interop/config.py index a2636a0..7ac9571 100644 --- a/ros2_to_mass_amr_interop/config.py +++ b/ros2_to_mass_amr_interop/config.py @@ -1,11 +1,12 @@ +from collections import defaultdict import yaml import logging import os -# Config files may have non static values that are +# Config files may have non local values that are # ought to be extracted from a different source # other than the config file -CFG_PARAMETER_STATIC = "static" +CFG_PARAMETER_LOCAL = "local" CFG_PARAMETER_ROS_TOPIC = "rosTopic" CFG_PARAMETER_ROS_PARAMETER = "rosParameter" CFG_PARAMETER_ENVVAR = "envVar" @@ -16,19 +17,21 @@ CFG_PARAMETER_ENVVAR ] +STATUS_REPORT_INTERVAL = 1 + 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 + with values that are not local 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 + mappings (:obj:`dict`): parameter name and value mapping """ @@ -37,7 +40,9 @@ def __init__(self, path=None) -> None: _config = self._load(path) self.server = _config['server'] - self.mapping = _config['mapping'] + self.mappings = _config['mappings'] + self.parameters_by_source = defaultdict(list) + self._parse_config(self.mappings) def _load(self, path) -> None: config = dict() @@ -54,6 +59,11 @@ def _load(self, path) -> None: config = config[k] return config + def _parse_config(self, mappings): + for parameter_name in mappings.keys(): + parameter_source = self.get_parameter_source(name=parameter_name) + self.parameters_by_source[parameter_source].append(parameter_name) + def get_parameter_source(self, name): """ Return parameter source. @@ -71,13 +81,15 @@ def get_parameter_source(self, name): str: parameter source """ - if isinstance(self.mapping[name], str): - return CFG_PARAMETER_STATIC + if isinstance(self.mappings[name], str) or \ + isinstance(self.mappings[name], float) or \ + isinstance(self.mappings[name], int): + return CFG_PARAMETER_LOCAL - 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'] + if isinstance(self.mappings[name], dict): + # Evaluate is the parameter is non local + if 'valueFrom' in self.mappings[name]: + param_source = self.mappings[name]['valueFrom'] # param_source is a dict whose first key # is the external parameter type @@ -89,11 +101,11 @@ def get_parameter_source(self, name): 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 + # assume that the parameter is local if it is an object + if isinstance(self.mappings[name], dict): + return CFG_PARAMETER_LOCAL - raise ValueError("Invalid parameter") + raise ValueError(f"Couldn't determine parameter '{name}' source.") def get_parameter_value(self, name): """ @@ -118,18 +130,27 @@ def get_parameter_value(self, name): 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_LOCAL: + return self.mappings[name] if param_source == CFG_PARAMETER_ENVVAR: - envvar_name = self.mapping[name]['valueFrom'][param_source] + envvar_name = self.mappings[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] + return self.mappings[name]['valueFrom'][param_source] if param_source == CFG_PARAMETER_ROS_PARAMETER: - return self.mapping[name]['valueFrom'][param_source] + return self.mappings[name]['valueFrom'][param_source] + + def get_ros_topic_parameter_type(self, name): + return self.mappings[name]['valueFrom']['msgType'] + + def get_ros_topic_parameter_topic(self, name): + return self.mappings[name]['valueFrom']['rosTopic'] + + def get_ros_topic_parameter_msg_field(self, name): + return self.mappings[name]['valueFrom'].get('msgField') diff --git a/ros2_to_mass_amr_interop/messages/__init__.py b/ros2_to_mass_amr_interop/messages/__init__.py index 2767b9a..2e54a83 100644 --- a/ros2_to_mass_amr_interop/messages/__init__.py +++ b/ros2_to_mass_amr_interop/messages/__init__.py @@ -1,33 +1,169 @@ -import logging from datetime import datetime +from pathlib import Path +import json +import jsonschema + +# MassRobotics AMR Interop required properties +# for both Identity and Status report objects. +# Common properties +MASS_REPORT_UUID = 'uuid' +MASS_REPORT_TIMESTAMP = 'timestamp' +# Identity Report specific +MASS_REPORT_MANUFACTURER_NAME = 'manufacturerName' +MASS_REPORT_ROBOT_MODEL = 'robotModel' +MASS_REPORT_ROBOT_SERIAL_NUMBER = 'robotSerialNumber' +MASS_REPORT_BASE_ROBOT_ENVELOPE = 'baseRobotEnvelope' +# Status Report specific +MASS_REPORT_OPERATIONAL_STATE = 'operationalState' +MASS_REPORT_LOCATION = 'location' class MassObject: - def __init__(self) -> None: - pass + """ + Base class for MassRobotics AMR Interop messages. - def _validate_schema(self, mass_object_type): - pass + Attributes + ---------- + data (:obj:`dict`): parameter name and value mapping + """ -class IdentityReport(MassObject): - def __init__(self, uuid, manufacturer_name, - robot_model, robot_serial_number, - base_robot_envelop, **kwargs) -> None: - self.logger = logging.getLogger(self.__class__.__name__) - self.data = {} - self.data['uuid'] = uuid - self.data['manufacturerName'] = manufacturer_name - self.data['robotModel'] = robot_model - self.data['robotSerialNumber'] = robot_serial_number - self.data['baseRobotEnvelope'] = base_robot_envelop + def __init__(self, **kwargs) -> None: + + if MASS_REPORT_UUID not in kwargs: + raise ValueError(f"Missing mandatory IdentityReport parameter {MASS_REPORT_UUID}") + + self.data = {MASS_REPORT_UUID: kwargs[MASS_REPORT_UUID]} + self._update_timestamp() + def _update_timestamp(self): # As per Mass example, data format is ISO8601 # with timezone offset e.g. 2012-04-21T18:25:43-05:00 - self.data['timestamp'] = datetime.now().replace(microsecond=0).astimezone().isoformat() + self.data[MASS_REPORT_TIMESTAMP] = datetime.now() \ + .replace(microsecond=0).astimezone().isoformat() + + def update_parameter(self, name, value): + """ + Update object timestamp and paramater ``name`` with ``value``. + + The method creates/overrides the property named ``name`` with the + value ``value`` while also updates the ``timestamp`` property with + current time, using ISO 8601 format. + + TODO: support ``timestamp`` parameter for stamped messages to favor + timestamp from source rather than generating a new one. + + Args: + ---- + name (str): MassRobotics AMR message parameter name. + value (str): MassRobotics AMR message parameter value. + + """ + self.data[name] = value + self._update_timestamp() + + def _validate_schema(self): + cwd = Path(__file__).resolve().parent + jsonschema_def_path = str(cwd / 'schema.json') + with open(jsonschema_def_path, 'r') as fp: + schema = json.load(fp) + + # TODO: capture relevant exceptions and improve + # error reporting + jsonschema.validate(instance=self.data, schema=schema) + + +class IdentityReport(MassObject): + """ + MassRobotics AMR Interop Identity Report message class. + + Represents Identity Report messages that are sent to compatible receivers. + Objects are initialized with default values for all mandatory properties + to avoid schema errors if no mappings are available. + + Attributes + ---------- + data (:obj:`dict`): parameter name and value mapping + + """ + + def __init__(self, **kwargs) -> None: + super().__init__(**kwargs) + + # Identity Report required and optional properties + # TODO: get them directly from schema file + self.schema_properties = [ + "uuid", "timestamp", "manufacturerName", "robotModel", + "robotSerialNumber", "baseRobotEnvelope", "maxSpeed", + "maxRunTime", "emergencyContactInformation", "chargerType", + "supportVendorName", "supportVendorContactInformation", + "productDocumentation", "thumbnailImage", "cargoType", + "cargoMaxVolume", "cargoMaxWeight" + ] + + # Initialize Identity Report object with required properties, + # assigning default values if not provided. + self.data[MASS_REPORT_MANUFACTURER_NAME] = kwargs.get( + MASS_REPORT_MANUFACTURER_NAME, "unknown") + self.data[MASS_REPORT_ROBOT_MODEL] = kwargs.get( + MASS_REPORT_ROBOT_MODEL, "unknown") + self.data[MASS_REPORT_ROBOT_SERIAL_NUMBER] = kwargs.get( + MASS_REPORT_ROBOT_SERIAL_NUMBER, "unknown") + self.data[MASS_REPORT_BASE_ROBOT_ENVELOPE] = kwargs.get( + MASS_REPORT_BASE_ROBOT_ENVELOPE, {"x": 0, "y": 0}) + + # Add other optional identity report properties + for property_name, property_value in kwargs.items(): + self.data[property_name] = property_value + + self._validate_schema() + + +class StatusReport(MassObject): + """ + MassRobotics AMR Interop Status Report message class. + + Represents Status Report messages that are sent to compatible receivers. + Objects are initialized with default values for all mandatory properties + to avoid schema errors if no mappings are available. + + Attributes + ---------- + data (:obj:`dict`): parameter name and value mapping + + """ + + def __init__(self, **kwargs) -> None: + super().__init__(**kwargs) + + # Status Report required and optional properties + # TODO: get them directly from schema file + self.schema_properties = [ + "uuid", "timestamp", "operationalState", "location", + "velocity", "batteryPercentage", "remainingRunTime", + "loadPercentageStillAvailable", "errorCodes", + "destinations", "path" + ] + + # Initialize Status Report object with required properties, + # assigning default values if not provided. + self.data[MASS_REPORT_OPERATIONAL_STATE] = kwargs.get( + MASS_REPORT_OPERATIONAL_STATE, "idle") + self.data[MASS_REPORT_LOCATION] = kwargs.get( + MASS_REPORT_LOCATION, { + "x": 0, + "y": 0, + "angle": { + "w": 0, + "x": 0, + "y": 0, + "z": 0 + }, + "planarDatum": "00000000-0000-0000-0000-000000000000" + }) # Add other optional identity report properties - for k, v in kwargs: - self.data[k] = v + for property_name, property_value in kwargs.items(): + self.data[property_name] = property_value - self._validate_schema(mass_object_type='identityReport') + self._validate_schema() diff --git a/ros2_to_mass_amr_interop/messages/AMR_Interop_Standard.json b/ros2_to_mass_amr_interop/messages/schema.json similarity index 100% rename from ros2_to_mass_amr_interop/messages/AMR_Interop_Standard.json rename to ros2_to_mass_amr_interop/messages/schema.json diff --git a/ros2_to_mass_amr_interop/ros2_to_mass_node.py b/ros2_to_mass_amr_interop/ros2_to_mass_node.py index bb0763e..712b6e4 100644 --- a/ros2_to_mass_amr_interop/ros2_to_mass_node.py +++ b/ros2_to_mass_amr_interop/ros2_to_mass_node.py @@ -3,11 +3,12 @@ import rclpy from . import MassAMRInteropNode +# Interesting example https://github.com/clalancette/mtexec_example + def main(args=None): rclpy.init(args=args) node = MassAMRInteropNode() - node.send_identity_report() rclpy.spin(node) rclpy.shutdown() diff --git a/sample_config.yaml b/sample_config.yaml index d3f87bc..9c2b6be 100644 --- a/sample_config.yaml +++ b/sample_config.yaml @@ -3,17 +3,23 @@ # ================================================================== # # Parameters that are used to configure ROS2 node for connecting to Mass compatible servers. -# The `server` section expects a string with a WebSocket server URI, while the `mapping` section -# contains a list of paramaters for configuring the ROS2 node. Mandatory parameters are `uuid`, -# `manufacturerName`, `robotModel`, `robotSerialNumber` and `baseRobotEnvelope` (see full spec -# https://github.com/MassRobotics-AMR/AMR_Interop_Standard/blob/main/AMR_Interop_Standard.json). +# The `server` section expects a string with a WebSocket server URI, while the `mappings` section +# contains a list of paramaters for configuring the ROS2 node. As per AMR Interop Standard, +# mandatory parameters are `uuid`, `manufacturerName`, `robotModel`, `robotSerialNumber` and +# `baseRobotEnvelope` (full spec https://github.com/MassRobotics-AMR/AMR_Interop_Standard/). # -# In addition to static values i.e. strings or objects, the `mapping` section supports a variety of +# Translation to AMR Interop Standard messages might be direct (i.e. a string to a report message +# field) or complex in case of ROS2 message having data that maps to many AMR report message fields. +# For this reason, some configuration parameters below expect a particular ROS2 message type e.g. +# fields on a ROS2 message of type `sensor_msgs/msg/BatteryState` are translated into AMR Interop +# Status Report fields `batteryPercentage`, `remainingRunTime` and `loadPercentageStillAvailable`. +# +# In addition to local values i.e. strings or objects, the `mappings` section supports a variety of # sources from where the parameter value can be obtained: `envVar`, `rosTopic` and `rosParameter`. config: - server: "wss://localhost:3000" - mapping: + server: "ws://localhost:3000" + mappings: # Identity uuid: valueFrom: @@ -22,9 +28,9 @@ config: robotModel: "spoony1.0" robotSerialNumber: "2172837" baseRobotEnvelope: - x: 2 - y: 1 - z: 3 + x: 2 + y: 1 + z: 3 maxSpeed: 2.5 maxRunTime: 8 emergencyContactInformation: "555-5555" @@ -35,45 +41,46 @@ config: thumbnailImage: "https://spoon.lift/media/spoony1.0.png" cargoType: "Anything solid or liquid" cargoMaxVolume: - value: - x: 2 - y: 2 - z: 1 - cargoMaxWeight: 4000 + x: 2 + y: 2 + z: 1 + cargoMaxWeight: "4000" # Status operationalState: valueFrom: rosTopic: /we_b_robots/mode - # std_msgs/String - pose: + msgType: std_msgs/String + location: valueFrom: - rosTopic: /magic_nav/current_pose - # geometry_msgs/PoseStamped + rosTopic: /move_base_simple/goal + msgType: geometry_msgs/msg/PoseStamped velocity: valueFrom: rosTopic: /good_sensors/vel - # geometry_msgs/TwistStamped + msgType: geometry_msgs/msg/TwistStamped batteryPercentage: valueFrom: rosTopic: /good_sensors/bat - # std_msgs/Float32 + msgType: sensor_msgs/msg/BatteryState + msgField: percentage remainingRunTime: valueFrom: - rosTopic: /we_b_robots/remaining_time - # std_msgs/Float32 + rosTopic: /good_sensors/bat + msgType: sensor_msgs/msg/BatteryState + msgField: remaining loadPercentageStillAvailable: valueFrom: - rosTopic: /spoonlift/available_capacity_percentage - # std_msgs/Float32 + rosTopic: /good_sensors/load + msgType: std_msgs/Float32 errorCodes: TBD / Juli / Flor destinations: valueFrom: rosTopic: /we_b_robots/destinations - # nav_msgs/Path + msgType: nav_msgs/Path path: valueFrom: rosTopic: /magic_nav/path - # nav_msgs/Path + msgType: nav_msgs/Path rosFrameToPlanarDatumUUID: # required since Mass expects frames to be referenced using uuids map: "096522ad-61fa-4796-9b31-e35b0f8d0b26" diff --git a/setup.py b/setup.py index 8989c00..d779c86 100644 --- a/setup.py +++ b/setup.py @@ -5,6 +5,8 @@ setup( name=package_name, packages=find_packages(), + package_data={'': ['schema.json']}, + include_package_data=True, version='0.1.0', data_files=[ ('share/ament_index/resource_index/packages', diff --git a/test/test_data/config.yaml b/test/test_data/config.yaml deleted file mode 100644 index 30ab8f8..0000000 --- a/test/test_data/config.yaml +++ /dev/null @@ -1,68 +0,0 @@ -config: - server: "wss://mass.inorbit.ai/receiver/aGVsbG8gd29ybGQgSSBsaWtlIHJvYm90cwo=" - mapping: - # Identity - uuid: - valueFrom: - envVar: MY_UUID - manufacturerName: Spoonlift - robotModel: "spoony1.0" - robotSerialNumber: "2172837" - baseRobotEnvelope: - x: 2 - y: 1 - z: 3 - maxSpeed: 2.5 - maxRunTime: 8 - emergencyContactInformation: "555-5555" - chargerType: "24V plus" - supportVendorName: "We-B-Robots" - supportVendorContactInformation: "support@we-b-robots.com" - productDocumentation: "https://spoon.lift/support/docs/spoony1.0" - thumbnailImage: "https://spoon.lift/media/spoony1.0.png" - cargoType: "Anything solid or liquid" - cargoMaxVolume: - value: - x: 2 - y: 2 - z: 1 - cargoMaxWeight: 4000 - # Status - operationalState: - valueFrom: - rosTopic: /we_b_robots/mode - # std_msgs/String - pose: - valueFrom: - rosTopic: /magic_nav/current_pose - # geometry_msgs/PoseStamped - velocity: - valueFrom: - rosTopic: /good_sensors/vel - # geometry_msgs/TwistStamped - batteryPercentage: - valueFrom: - rosTopic: /good_sensors/bat - # std_msgs/Float32 - remainingRunTime: - valueFrom: - rosTopic: /we_b_robots/remaining_time - # std_msgs/Float32 - loadPercentageStillAvailable: - valueFrom: - rosTopic: /spoonlift/available_capacity_percentage - # std_msgs/Float32 - errorCodes: TBD / Juli / Flor - destinations: - valueFrom: - rosTopic: /we_b_robots/destinations - # nav_msgs/Path - path: - valueFrom: - rosTopic: /magic_nav/path - # nav_msgs/Path - rosFrameToPlanarDatumUUID: - # required since Mass expects frames to be referenced using uuids - map: "096522ad-61fa-4796-9b31-e35b0f8d0b26" - floor1: "096522ad-61fa-4796-9b31-e35b0f8d0b26" - floor2: "6ec7a6d0-21a9-4f04-b680-e7c640a0687e" diff --git a/test/test_mass_interop.py b/test/test_mass_interop.py index 8bf189d..f058b8b 100644 --- a/test/test_mass_interop.py +++ b/test/test_mass_interop.py @@ -5,11 +5,12 @@ from ros2_to_mass_amr_interop import MassAMRInteropNode cwd = Path(__file__).resolve().parent -config_file_test = cwd / 'test_data' / 'config.yaml' +config_file_test = Path(cwd).parent / "sample_config.yaml" -def test_mass_config_load(): +def test_mass_config_load(monkeypatch): rclpy.init() + monkeypatch.setenv("MY_UUID", "foo") # Environment variable used on config file MassAMRInteropNode(parameter_overrides=[ Parameter("config_file", value=str(config_file_test)) ]) diff --git a/test/test_mass_interop_config.py b/test/test_mass_interop_config.py index 221f45e..d52a71e 100644 --- a/test/test_mass_interop_config.py +++ b/test/test_mass_interop_config.py @@ -1,7 +1,7 @@ import pytest from pathlib import Path from ros2_to_mass_amr_interop.config import MassAMRInteropConfig -from ros2_to_mass_amr_interop.config import CFG_PARAMETER_STATIC +from ros2_to_mass_amr_interop.config import CFG_PARAMETER_LOCAL from ros2_to_mass_amr_interop.config import CFG_PARAMETER_ROS_TOPIC from ros2_to_mass_amr_interop.config import CFG_PARAMETER_ENVVAR @@ -9,18 +9,19 @@ def test_mass_config_load(): - cfg_file_path = Path(cwd) / "test_data" / "config.yaml" - assert MassAMRInteropConfig(str(cfg_file_path)).mapping != {} + cfg_file_path = Path(cwd).parent / "sample_config.yaml" + assert MassAMRInteropConfig(str(cfg_file_path)).mappings != {} @pytest.mark.parametrize("param_name, param_type", [ ("uuid", CFG_PARAMETER_ENVVAR), - ("robotModel", CFG_PARAMETER_STATIC), + ("robotModel", CFG_PARAMETER_LOCAL), ("operationalState", CFG_PARAMETER_ROS_TOPIC), - ("baseRobotEnvelope", CFG_PARAMETER_STATIC) + ("baseRobotEnvelope", CFG_PARAMETER_LOCAL), + ("maxSpeed", CFG_PARAMETER_LOCAL) ]) def test_mass_config_get_parameter_type(param_name, param_type): - cfg_file_path = Path(cwd) / "test_data" / "config.yaml" + cfg_file_path = Path(cwd).parent / "sample_config.yaml" mass_config = MassAMRInteropConfig(str(cfg_file_path)) assert mass_config.get_parameter_source(param_name) == param_type @@ -33,6 +34,20 @@ def test_mass_config_get_parameter_type(param_name, param_type): ]) def test_mass_config_get_parameter_value(monkeypatch, param_name, value): monkeypatch.setenv("MY_UUID", "foo") # Environment variable used on config file - cfg_file_path = Path(cwd) / "test_data" / "config.yaml" + cfg_file_path = Path(cwd).parent / "sample_config.yaml" mass_config = MassAMRInteropConfig(str(cfg_file_path)) assert mass_config.get_parameter_value(param_name) == value + + +@pytest.mark.parametrize("param_name, source", [ + ("uuid", CFG_PARAMETER_ENVVAR), + ("robotModel", CFG_PARAMETER_LOCAL), + ("operationalState", CFG_PARAMETER_ROS_TOPIC), + ("baseRobotEnvelope", CFG_PARAMETER_LOCAL), + ("maxSpeed", CFG_PARAMETER_LOCAL) +]) +def test_mass_config_get_parameters_by_source(monkeypatch, param_name, source): + monkeypatch.setenv("MY_UUID", "foo") # Environment variable used on config file + cfg_file_path = Path(cwd).parent / "sample_config.yaml" + mass_config = MassAMRInteropConfig(str(cfg_file_path)) + assert param_name in mass_config.parameters_by_source[source]