diff --git a/rise_motion_dev_ws/src/py_sdo_serializer/package.xml b/rise_motion_dev_ws/src/py_sdo_serializer/package.xml
new file mode 100644
index 0000000..04bed26
--- /dev/null
+++ b/rise_motion_dev_ws/src/py_sdo_serializer/package.xml
@@ -0,0 +1,18 @@
+
+
+
+ py_sdo_serializer
+ 0.0.0
+ Offers functions to serialize and deserialize EtherCAT base data types.
+ a
+ TODO: License declaration
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/__init__.py b/rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/sdo_serializer.py b/rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/sdo_serializer.py
new file mode 100644
index 0000000..a9d00c5
--- /dev/null
+++ b/rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/sdo_serializer.py
@@ -0,0 +1,404 @@
+"""Functions to serialize/deserialize a value according to EtherCAT data type.
+
+The data type can be identified by its index, name, or base data type
+string. Providing one is sufficient.
+
+For definitions of supported base data types and encoding see see ETG.1000.6
+and ETG.1020 at https://www.ethercat.org/en/downloads.html.
+"""
+import struct
+from dataclasses import dataclass
+
+# ---------------------------------------------------------------------------
+# Record type
+# ---------------------------------------------------------------------------
+
+
+@dataclass(frozen=True)
+class TypeInfo:
+ """Stores attributes of EtherCAT base data types from ETG.1020.
+
+ For definitions of supported base data types and encoding
+ see see ETG.1000.6, ETG.1020 at https://www.ethercat.org/en/downloads.html.
+ """
+
+ index: int # EtherCAT object-dictionary type index
+ name: str # ETG long name (e.g. 'INTEGER16')
+ base_data_type: str # IEC / short name (e.g. 'INT')
+ bit_size: int # Canonical bit width
+ serialize_fn: str # Name of the serialization subfunction to call
+ deserialize_fn: str # Name of the deserialization subfunction to call
+
+
+# ---------------------------------------------------------------------------
+# Master table
+# ---------------------------------------------------------------------------
+
+BASE_DATA_TYPES: dict[int, TypeInfo] = {
+
+ # Boolean / generic word types
+ 0x0001: TypeInfo(0x0001, 'BOOLEAN', 'BOOL', 1, 'serialize_bool', 'deserialize_bool'),
+ 0x001E: TypeInfo(0x001E, 'BYTE', 'BYTE', 8, 'serialize_bitn', 'deserialize_bitn'),
+ 0x001F: TypeInfo(0x001F, 'WORD', 'WORD', 16, 'serialize_bitn', 'deserialize_bitn'),
+ 0x0020: TypeInfo(0x0020, 'DWORD', 'DWORD', 32, 'serialize_bitn', 'deserialize_bitn'),
+
+ # Time types (48-bit, special structure)
+ 0x000C: TypeInfo(0x000C, 'TIME_OF_DAY', 'TIME_OF_DAY', 48, 'serialize_time_of_day', 'deserialize_time48'),
+ 0x000D: TypeInfo(0x000D, 'TIME_DIFFERENCE', 'TIME_DIFFERENCE', 48, 'serialize_time_difference', 'deserialize_time48'),
+
+ # Bit strings BIT1 - BIT16
+ 0x0030: TypeInfo(0x0030, 'BIT1', 'BIT1', 1, 'serialize_bitn', 'deserialize_bitn'),
+ 0x0031: TypeInfo(0x0031, 'BIT2', 'BIT2', 2, 'serialize_bitn', 'deserialize_bitn'),
+ 0x0032: TypeInfo(0x0032, 'BIT3', 'BIT3', 3, 'serialize_bitn', 'deserialize_bitn'),
+ 0x0033: TypeInfo(0x0033, 'BIT4', 'BIT4', 4, 'serialize_bitn', 'deserialize_bitn'),
+ 0x0034: TypeInfo(0x0034, 'BIT5', 'BIT5', 5, 'serialize_bitn', 'deserialize_bitn'),
+ 0x0035: TypeInfo(0x0035, 'BIT6', 'BIT6', 6, 'serialize_bitn', 'deserialize_bitn'),
+ 0x0036: TypeInfo(0x0036, 'BIT7', 'BIT7', 7, 'serialize_bitn', 'deserialize_bitn'),
+ 0x0037: TypeInfo(0x0037, 'BIT8', 'BIT8', 8, 'serialize_bitn', 'deserialize_bitn'),
+ 0x0038: TypeInfo(0x0038, 'BIT9', 'BIT9', 9, 'serialize_bitn', 'deserialize_bitn'),
+ 0x0039: TypeInfo(0x0039, 'BIT10', 'BIT10', 10, 'serialize_bitn', 'deserialize_bitn'),
+ 0x003A: TypeInfo(0x003A, 'BIT11', 'BIT11', 11, 'serialize_bitn', 'deserialize_bitn'),
+ 0x003B: TypeInfo(0x003B, 'BIT12', 'BIT12', 12, 'serialize_bitn', 'deserialize_bitn'),
+ 0x003C: TypeInfo(0x003C, 'BIT13', 'BIT13', 13, 'serialize_bitn', 'deserialize_bitn'),
+ 0x003D: TypeInfo(0x003D, 'BIT14', 'BIT14', 14, 'serialize_bitn', 'deserialize_bitn'),
+ 0x003E: TypeInfo(0x003E, 'BIT15', 'BIT15', 15, 'serialize_bitn', 'deserialize_bitn'),
+ 0x003F: TypeInfo(0x003F, 'BIT16', 'BIT16', 16, 'serialize_bitn', 'deserialize_bitn'),
+
+ # Bit arrays
+ 0x002D: TypeInfo(0x002D, 'BITARR8', 'BITARR8', 8, 'serialize_bitn', 'deserialize_bitn'),
+ 0x002E: TypeInfo(0x002E, 'BITARR16', 'BITARR16', 16, 'serialize_bitn', 'deserialize_bitn'),
+ 0x002F: TypeInfo(0x002F, 'BITARR32', 'BITARR32', 32, 'serialize_bitn', 'deserialize_bitn'),
+
+ # Signed integers
+ 0x0002: TypeInfo(0x0002, 'INTEGER8', 'SINT', 8, 'serialize_int', 'deserialize_int'),
+ 0x0003: TypeInfo(0x0003, 'INTEGER16', 'INT', 16, 'serialize_int', 'deserialize_int'),
+ 0x0010: TypeInfo(0x0010, 'INTEGER24', 'INT24', 24, 'serialize_int', 'deserialize_int'),
+ 0x0004: TypeInfo(0x0004, 'INTEGER32', 'DINT', 32, 'serialize_int', 'deserialize_int'),
+ 0x0012: TypeInfo(0x0012, 'INTEGER40', 'INT40', 40, 'serialize_int', 'deserialize_int'),
+ 0x0013: TypeInfo(0x0013, 'INTEGER48', 'INT48', 48, 'serialize_int', 'deserialize_int'),
+ 0x0014: TypeInfo(0x0014, 'INTEGER56', 'INT56', 56, 'serialize_int', 'deserialize_int'),
+ 0x0015: TypeInfo(0x0015, 'INTEGER64', 'LINT', 64, 'serialize_int', 'deserialize_int'),
+
+ # Unsigned integers
+ 0x0005: TypeInfo(0x0005, 'UNSIGNED8', 'USINT', 8, 'serialize_uint', 'deserialize_uint'),
+ 0x0006: TypeInfo(0x0006, 'UNSIGNED16', 'UINT', 16, 'serialize_uint', 'deserialize_uint'),
+ 0x0016: TypeInfo(0x0016, 'UNSIGNED24', 'UINT24', 24, 'serialize_uint', 'deserialize_uint'),
+ 0x0007: TypeInfo(0x0007, 'UNSIGNED32', 'UDINT', 32, 'serialize_uint', 'deserialize_uint'),
+ 0x0018: TypeInfo(0x0018, 'UNSIGNED40', 'UINT40', 40, 'serialize_uint', 'deserialize_uint'),
+ 0x0019: TypeInfo(0x0019, 'UNSIGNED48', 'UINT48', 48, 'serialize_uint', 'deserialize_uint'),
+ 0x001A: TypeInfo(0x001A, 'UNSIGNED56', 'UINT56', 56, 'serialize_uint', 'deserialize_uint'),
+ 0x001B: TypeInfo(0x001B, 'UNSIGNED64', 'ULINT', 64, 'serialize_uint', 'deserialize_uint'),
+
+ # Floating point
+ 0x0008: TypeInfo(0x0008, 'REAL32', 'REAL', 32, 'serialize_float', 'deserialize_float'),
+ 0x0011: TypeInfo(0x0011, 'REAL64', 'LREAL', 64, 'serialize_float', 'deserialize_float'),
+
+ # GUID (according to specifications value is stored as a 128-bit integer)
+ 0x001D: TypeInfo(0x001D, 'GUID', 'GUID', 128, 'serialize_int', 'deserialize_int'),
+
+ # - Base Data Types with variable length -
+ # Strings
+ 0x0009: TypeInfo(0x0009, 'VISIBLE_STRING', 'STRING(n)', 8, 'serialize_visible_string', 'deserialize_visible_string'), # 8*(n)),
+ 0x0268: TypeInfo(0x0268, 'UNICODE_STRING', 'WSTRING(n)', 16, 'serialize_unicode_string', 'deserialize_unicode_string'), # 16*(n)
+
+ # Octet field
+ 0x000A: TypeInfo(0x000A, 'OCTET_STRING', 'ARRAY [0..n] OF BYTE', 8, 'serialize_bitn', 'deserialize_bitn'),#8*(n+1)
+ 0x000B: TypeInfo(0x000B, 'ARRAY_OF_UINT', 'ARRAY [0..n] OF UINT', 16, 'serialize_uint', 'deserialize_uint'),#16*(n+1)
+ 0x0260: TypeInfo(0x0260, 'ARRAY_OF_INT', 'ARRAY [0..n] OF INT', 16, 'serialize_int', 'deserialize_int'),#16*(n+1)
+ 0x0261: TypeInfo(0x0261, 'ARRAY_OF_SINT', 'ARRAY [0..n] OF SINT', 8, 'serialize_int', 'deserialize_int'),#8*(n+1)
+ 0x0262: TypeInfo(0x0262, 'ARRAY_OF_DINT', 'ARRAY [0..n] OF DINT', 32, 'serialize_int', 'deserialize_int'),#32*(n+1)
+ 0x0263: TypeInfo(0x0263, 'ARRAY_OF_UDINT', 'ARRAY [0..n] OF UDINT', 32, 'serialize_uint', 'deserialize_uint'),#32*(n+1)
+ 0x0264: TypeInfo(0x0264, 'ARRAY_OF_BITARR8', 'ARRAY [0..n] OF BITARR8', 8, 'serialize_bitn', 'deserialize_bitn'),#8*(n+1)
+ 0x0265: TypeInfo(0x0265, 'ARRAY_OF_BITARR16', 'ARRAY [0..n] OF BITARR16', 16, 'serialize_bitn', 'deserialize_bitn'),#16*(n+1)
+ 0x0266: TypeInfo(0x0266, 'ARRAY_OF_BITARR32', 'ARRAY [0..n] OF BITARR32', 32, 'serialize_bitn', 'deserialize_bitn'),#32*(n+1)
+ 0x0267: TypeInfo(0x0267, 'ARRAY_OF_USINT', 'ARRAY [0..n] OF USINT', 8, 'serialize_uint', 'deserialize_uint'),#8*(n+1)
+ 0x0269: TypeInfo(0x0269, 'ARRAY_OF_REAL', 'ARRAY [0..n] OF REAL', 32, 'serialize_float', 'deserialize_float'),#32*(n+1)
+ 0x026A: TypeInfo(0x026A, 'ARRAY_OF_LREAL', 'ARRAY [0..n] OF LREAL', 64, 'serialize_float', 'deserialize_float'),#64*(n+1)
+}
+
+
+# ---------------------------------------------------------------------------
+# Secondary lookup dicts (built once at import time)
+# ---------------------------------------------------------------------------
+
+BY_NAME: dict[str, TypeInfo] = {
+ info.name: info for info in BASE_DATA_TYPES.values()
+}
+
+BY_BASE_DATA_TYPE: dict[str, TypeInfo] = {
+ info.base_data_type: info for info in BASE_DATA_TYPES.values()
+}
+
+
+# ---------------------------------------------------------------------------
+# Convenience accessor
+# ---------------------------------------------------------------------------
+
+def get_type_info(
+ *,
+ index: int | None = None,
+ name: str | None = None,
+ base_data_type: str | None = None,
+) -> TypeInfo:
+ """
+ Retrieve a TypeInfo record by any one of the three keys.
+
+ Examples
+ --------
+ get_type_info(index=0x0003)
+ get_type_info(name='INTEGER16')
+ get_type_info(base_data_type='INT')
+ """
+ if index is not None:
+ try:
+ return BASE_DATA_TYPES[index]
+ except KeyError:
+ raise KeyError(
+ f"No EtherCAT type with index {index:#04x}"
+ ) from None
+
+ if name is not None:
+ try:
+ return BY_NAME[name]
+ except KeyError:
+ raise KeyError(
+ f"No EtherCAT type with name '{name}'"
+ ) from None
+
+ if base_data_type is not None:
+ try:
+ return BY_BASE_DATA_TYPE[base_data_type]
+ except KeyError:
+ raise KeyError(
+ f"No EtherCAT type with base_data_type '{base_data_type}'"
+ ) from None
+
+ raise ValueError('Provide at least one of: index, name, base_data_type')
+
+
+# ---------------------------------------------------------------------------
+# Generic functions
+# ---------------------------------------------------------------------------
+
+def serialize(
+ value,
+ *,
+ index: int | None = None,
+ name: str | None = None,
+ base_data_type: str | None = None,
+) -> list[int]:
+ """Serialize a value according to an EtherCAT base data type.
+
+ The data type can be identified by its index, name, or base data type
+ string. Providing one is sufficient.
+
+ For definitions of supported base data types and encoding
+ see see ETG.1000.6 and ETG.1020 at https://www.ethercat.org/en/downloads.html
+
+ :param value:
+ Value to serialize.
+ :param index:
+ EtherCAT data type index.
+ :param name:
+ EtherCAT data type name.
+ :param base_data_type:
+ Data type making up this base data type. Called 'Type' in
+ Somanet Circulo Object Dictionary reference.
+ :returns:
+ Serialized value as a list of bytes.
+ :rtype:
+ list[int]
+ """
+ try:
+ object_info = get_type_info(index=index, name=name, base_data_type=base_data_type)
+
+ # serialize a base data type
+ if object_info.base_data_type[0:5] != 'ARRAY':
+ return globals()[object_info.serialize_fn](
+ value, object_info.bit_size)
+ # serialize a base data type list (imagine this: base_data_type[])
+ else:
+ out = []
+ for item in value:
+ out.extend(globals()[object_info.serialize_fn](
+ item, object_info.bit_size))
+ return out
+ except:
+ return -1
+
+def deserialize(
+ serialized_value: list[int],
+ *,
+ index: int | None = None,
+ name: str | None = None,
+ base_data_type: str | None = None,
+):
+ """Deserializes a list of bytes representing an EtherCAT base data type.
+
+ The data type can be identified by its index, name, or base data type
+ string. Providing one is sufficient.
+
+ For definitions of supported base data types and encoding
+ see ETG.1000.6 and ETG.1020 at https://www.ethercat.org/en/downloads.html
+
+ :param serialized_value:
+ Bytes to deserialize.
+ :param index:
+ EtherCAT data type index.
+ :param name:
+ EtherCAT data type name.
+ :param base_data_type:
+ Data type making up this base data type. Called 'Type' in
+ Somanet Circulo Object Dictionary reference.
+ :returns:
+ Deserialized value as a fitting python data type.
+ """
+ try:
+ if not isinstance(serialized_value, list) or not all(isinstance(b, int) for b in serialized_value):
+ raise TypeError(f"serialized_value must be a list[int] not {type(serialized_value)} {serialized_value}")
+ object_info = get_type_info(index=index, name=name, base_data_type=base_data_type)
+ byte_len = (object_info.bit_size + 7) // 8
+ isArray = object_info.base_data_type[0:5] == 'ARRAY'
+
+ # check if size of variably sized serialized object is plausible
+ if object_info.name[-6:] == 'STRING' or isArray:
+ if len(serialized_value)%byte_len != 0:
+ raise ValueError(
+ f"Number of bytes needed to contain variably sized list of Base Data Types must be"
+ 'evenly divisible by the byte-size of those Base Data Types.')
+ # check if size of serialized object is correct
+ else:
+ if byte_len != len(serialized_value):
+ raise ValueError(
+ f"Number of bytes needed to contain object_info.bit_size must be equal to serialized_value length.")
+
+ # deserialize a serialized base data type
+ if not isArray:
+ return globals()[object_info.deserialize_fn](serialized_value, object_info.bit_size)
+ # deserialize a serialized base data type array (imagine this: base_data_type[])
+ else:
+ out = []
+ for i in range(0, len(serialized_value), byte_len):
+ out.append(globals()[object_info.deserialize_fn](serialized_value[i:i+byte_len], object_info.bit_size))
+ return out
+ except:
+ return -1
+
+# ---------------------------------------------------------------------------
+# Specific functions (called by generic functions)
+# ---------------------------------------------------------------------------
+
+
+def serialize_bitn(val, bit_s: int) -> list[int]:
+ """Serialize a bit-string into a list[int]."""
+ if isinstance(val, str):
+ val = int(val, 2)
+ elif not isinstance(val, int):
+ raise TypeError(f"Input value must be either int or str, not {type(val)} {val}.")
+
+ byte_len = (bit_s + 7) // 8
+ return list(val.to_bytes(byte_len, byteorder='little'))
+
+
+def deserialize_bitn(ser_val: list[int], bit_s: int) -> str:
+ """Deserialize a list[int] into a bit-string."""
+ value = int.from_bytes(bytes(ser_val), byteorder='little')
+ return bin(value)[2:].zfill(bit_s)
+
+
+def serialize_int(val: int, bit_s: int) -> list[int]:
+ """Serialize a signed int into a list[int]."""
+ if not isinstance(val, int):
+ raise TypeError(f"Input value must be an int, not {type(val)} {val}.")
+ byte_len = (bit_s + 7) // 8
+ return list(val.to_bytes(byte_len, byteorder='little', signed=True))
+
+
+def deserialize_int(ser_val: list[int], bit_s: int) -> int:
+ """Deserialize a list[int] into a signed int."""
+ return int.from_bytes(bytes(ser_val), byteorder='little', signed=True)
+
+
+def serialize_uint(val: int, bit_s: int) -> list[int]:
+ """Serialize an unsigned int into a list[int]."""
+ if not isinstance(val, int):
+ raise TypeError(f"Input value must be an int, not {type(val)} {val}.")
+ byte_len = (bit_s + 7) // 8
+ return list(val.to_bytes(byte_len, byteorder='little'))
+
+
+def deserialize_uint(ser_val: list[int], bit_s: int) -> int:
+ """Deserialize a list[int] into an unsigned int."""
+ return int.from_bytes(bytes(ser_val), byteorder='little')
+
+
+def serialize_bool(val, bit_s: int) -> list[int]:
+ """Serialize a bool into a list[int]."""
+ if (not isinstance(val, bool)) and (val not in [1, 0]):
+ raise TypeError(f"Input value must be a bool or the int 1 or 0, not {type(val)} {val}.")
+ if val:
+ return serialize_bitn(0xff, bit_s)
+ else:
+ return serialize_bitn(0x00, bit_s)
+
+
+def deserialize_bool(ser_val: list[int], bit_s: int) -> bool:
+ """Deserialize a list[int] into a bool."""
+ return 0 != ser_val[0]
+
+
+def serialize_float(val, bit_s: int) -> list[int]:
+ """Serialize a float into a list[int]."""
+ if not isinstance(val, float):
+ raise TypeError(f"Input value must be a float, not {type(val)} {val}.")
+ return list(struct.pack(f"<{'f' if bit_s == 32 else 'd'}", val))
+
+
+def deserialize_float(ser_val: list[int], bit_s: int) -> float:
+ """Deserialize a list[int] into a float."""
+ return struct.unpack(f"<{'f' if bit_s == 32 else 'd'}", bytes(ser_val))[0]
+
+
+def serialize_time_of_day(val, bit_s: int) -> list[int]:
+ """Serialize tuple of ints (ms since midnight, days since 01.01.1984)."""
+ if (len(val) != 2) or (not isinstance(val[0], int)) or (not isinstance(val[1], int)):
+ raise TypeError(f"There needs to be a ms and a day value. They must be ints in a tuple, not {type(val)} {val}")
+ if val[0] > (1<<28)-1:
+ raise OverflowError('the 4 most significant bits of number of ms since midnight need to be 0')
+ return list(val[0].to_bytes(4, byteorder='big')) + list(val[1].to_bytes(2, byteorder='big'))
+
+
+def serialize_time_difference(val, bit_s: int) -> list[int]:
+ """Serialize tuple of ints (ms, days)."""
+ if (len(val) != 2) or (not isinstance(val[0], int)) or (not isinstance(val[1], int)):
+ raise TypeError(f"There needs to be a ms and a day value. They must be ints in a tuple, not {type(val)} {val}")
+ return list(val[0].to_bytes(4, byteorder='big')) + list(val[1].to_bytes(2, byteorder='big'))
+
+
+def deserialize_time48(ser_val: list[int], bit_s: int) -> tuple[int]:
+ """Deserialize list[int] to tuple of ints (ms, days)."""
+ return (int.from_bytes(ser_val[:4], byteorder='big'), int.from_bytes(ser_val[4:], byteorder='big'))
+
+
+def serialize_visible_string(val: str, bit_s: int) -> list[int]:
+ """Serialize an UTF-8 encoded string into a list[int]."""
+ if not isinstance(val, str):
+ raise TypeError(f"Input value must be str, not {type(val)} {val}")
+ return list(val.encode('UTF-8'))
+
+
+def deserialize_visible_string(ser_val: list[int], bit_s: int) -> str:
+ """Deserialize a list[int] into an UTF-8 encoded string."""
+ return bytes(ser_val).decode('UTF-8')
+
+
+def serialize_unicode_string(val: str, bit_s: int) -> list[int]:
+ """Serialize a utf_16_le encoded string into a list[int]."""
+ if not isinstance(val, str):
+ raise TypeError(f"Input value must be str, not {type(val)} {val}")
+ return list(val.encode('utf_16_le'))
+
+
+def deserialize_unicode_string(ser_val: list[int], bit_s: int) -> str:
+ """Deerialize a list[int] into a utf_16_le encoded string."""
+ return bytes(ser_val).decode('utf_16_le')
diff --git a/rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/temp_test_node.py b/rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/temp_test_node.py
new file mode 100644
index 0000000..d80bbbe
--- /dev/null
+++ b/rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/temp_test_node.py
@@ -0,0 +1,165 @@
+#!/usr/bin/env python3
+
+# execute this using
+# python3 path/to/this/file
+
+import sys
+import rclpy
+from rclpy.node import Node
+from rise_motion_messages.msg import MotorPositions
+from rise_motion_messages.srv import EnableEthercatSrv, SDOReadSrv
+from rise_motion_dev_ws.src.py_sdo_serialization.sdo_serializer import deserialize, serialize
+
+
+class TestNode(Node):
+ def __init__(self, increment: int):
+ super().__init__("python_sdo_test_node")
+ self.increment = increment
+ self.valid_positions = False
+ self.motor_pos: list[int] = []
+
+ self.get_logger().info("Starting TestNode")
+
+ self.input_sub = self.create_subscription(
+ MotorPositions,
+ "motor_feedback",
+ self._feedback_cb,
+ 10,
+ )
+
+ self.output_pub = self.create_publisher(MotorPositions, "motor_commands", 10)
+
+ self.publish_timer = self.create_timer(0.001, self._publish_cb) # 1 ms
+
+ self.enable_client = self.create_client(EnableEthercatSrv, "enable_ethercat")
+
+ def __del__(self):
+ self.get_logger().info("Bye :)")
+
+ # ---------------------------------------------------------------------------
+ # Callbacks
+ # ---------------------------------------------------------------------------
+
+ def _feedback_cb(self, msg: MotorPositions):
+ if not self.valid_positions:
+ self.get_logger().info("Got feedback")
+ self.valid_positions = True
+ self.motor_pos = list(msg.positions)
+
+ def _publish_cb(self):
+ if not self.valid_positions:
+ return
+ response = MotorPositions()
+ response.positions = [p + self.increment for p in self.motor_pos]
+ self.output_pub.publish(response)
+
+ # ---------------------------------------------------------------------------
+ # Service calls
+ # ---------------------------------------------------------------------------
+
+ def request_enable_ethercat(self) -> bool:
+ """Returns True on success (mirrors the C++ int == 0 success check)."""
+ self.get_logger().info(f"Incrementing motor position with {self.increment}")
+ self.get_logger().info("Requesting Enable Ethercat")
+
+ while not self.enable_client.wait_for_service(timeout_sec=1.0):
+ if not rclpy.ok():
+ self.get_logger().error("Interrupted while waiting for enable_ethercat service.")
+ return False
+ self.get_logger().info("Waiting for enable_ethercat service...")
+
+ request = EnableEthercatSrv.Request()
+ request.enable = True
+
+ future = self.enable_client.call_async(request)
+ rclpy.spin_until_future_complete(self, future)
+
+ if future.result() is None:
+ self.get_logger().error("enable_ethercat service call failed.")
+ return False
+
+ self.get_logger().info("Done requesting")
+ return bool(future.result().status_enable)
+
+ def sdo_read(
+ self,
+ device_id: int,
+ index: int,
+ subindex: int,
+ value_type: int = 0,
+ *,
+ type_name: str | None = None,
+ base_data_type: str | None = None,
+ ):
+ """
+ Call the sdo_read service and deserialize the result.
+
+ Returns the deserialized value, or None on error.
+ """
+ client = self.create_client(SDOReadSrv, "sdo_read")
+
+ while not client.wait_for_service(timeout_sec=1.0):
+ if not rclpy.ok():
+ self.get_logger().error("Interrupted while waiting for sdo_read service.")
+ return None
+ self.get_logger().info("Waiting for sdo_read service...")
+
+ request = SDOReadSrv.Request()
+ request.device_id = device_id
+ request.index = index
+ request.subindex = subindex
+ request.value_type = value_type
+
+ future = client.call_async(request)
+ rclpy.spin_until_future_complete(self, future)
+
+ if future.result() is None:
+ self.get_logger().error("sdo_read service call failed.")
+ return None
+
+ raw: list[int] = list(future.result().value)
+ result = deserialize(raw, name=type_name, base_data_type=base_data_type)
+
+ if result == -1:
+ self.get_logger().error("sdo deserialization failed for index 0x%04X sub %d", index, subindex)
+ return None
+
+ return result
+
+
+# ---------------------------------------------------------------------------
+# Entry point
+# ---------------------------------------------------------------------------
+
+def main():
+ increment = int(sys.argv[1]) if len(sys.argv) >= 2 else 10
+
+ rclpy.init()
+ node = TestNode(increment)
+
+ # Keep calling until EtherCAT is enabled
+ while not node.request_enable_ethercat():
+ pass
+
+ for p in [
+ [1,0x1008,0,"VISIBLE_STRING"], [1,0x1000,0,"UDINT"],
+ [1,0x1005,0,"DINT"], [1,0x1018,1,"UDINT"]]:
+ # Read device name (object 0x1008, sub 0) as a VISIBLE_STRING
+ result = node.sdo_read(
+ device_id=p[0],
+ index=p[1],
+ subindex=p[2],
+ type_name=p[3],
+ )
+
+ if result == -1:
+ rclpy.get_logger("rclcpp").error(f"SDO read {p[1]} {p[3]} failed")
+ else:
+ node.get_logger().info(f"sdo read value {p[1]} {p[3]}: %s", str(result))
+
+ rclpy.spin(node)
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/rise_motion_dev_ws/src/py_sdo_serializer/resource/py_sdo_serializer b/rise_motion_dev_ws/src/py_sdo_serializer/resource/py_sdo_serializer
new file mode 100644
index 0000000..e69de29
diff --git a/rise_motion_dev_ws/src/py_sdo_serializer/setup.cfg b/rise_motion_dev_ws/src/py_sdo_serializer/setup.cfg
new file mode 100644
index 0000000..aa20d50
--- /dev/null
+++ b/rise_motion_dev_ws/src/py_sdo_serializer/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/py_sdo_serializer
+[install]
+install_scripts=$base/lib/py_sdo_serializer
diff --git a/rise_motion_dev_ws/src/py_sdo_serializer/setup.py b/rise_motion_dev_ws/src/py_sdo_serializer/setup.py
new file mode 100644
index 0000000..f568759
--- /dev/null
+++ b/rise_motion_dev_ws/src/py_sdo_serializer/setup.py
@@ -0,0 +1,30 @@
+from setuptools import find_packages, setup
+
+package_name = 'py_sdo_serializer'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='a',
+ maintainer_email='anton.ohler@gmail.com',
+ description='Offers functions to serialize and deserialize EtherCAT base data types.',
+ license='TODO: License declaration',
+ extras_require={
+ 'test': [
+ 'pytest',
+ ],
+ },
+ entry_points={
+ 'console_scripts': [
+ ],
+ },
+ tests_require=['pytest'],
+)
diff --git a/rise_motion_dev_ws/src/py_sdo_serializer/test/test_copyright.py b/rise_motion_dev_ws/src/py_sdo_serializer/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/rise_motion_dev_ws/src/py_sdo_serializer/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/rise_motion_dev_ws/src/py_sdo_serializer/test/test_flake8.py b/rise_motion_dev_ws/src/py_sdo_serializer/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/rise_motion_dev_ws/src/py_sdo_serializer/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/rise_motion_dev_ws/src/py_sdo_serializer/test/test_pep257.py b/rise_motion_dev_ws/src/py_sdo_serializer/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/rise_motion_dev_ws/src/py_sdo_serializer/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/rise_motion_dev_ws/src/py_sdo_serializer/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/py_sdo_serializer/test/test_sdo_serializer.py
new file mode 100644
index 0000000..061f8aa
--- /dev/null
+++ b/rise_motion_dev_ws/src/py_sdo_serializer/test/test_sdo_serializer.py
@@ -0,0 +1,900 @@
+"""Tests for sdo_serializer.py module."""
+import pytest
+import random
+from math import isnan
+from py_sdo_serializer.sdo_serializer import serialize, deserialize
+
+# ---------------------------------------------------------------------------
+# Bit strings BIT1 - BIT16 tests
+# ---------------------------------------------------------------------------
+
+# --- round-trip tests ---
+
+@pytest.mark.parametrize("bit_width", range(1, 17))
+def test_roundtrip_random_bitstrings(bit_width: int):
+ """Serializing then deserializing a random valid value returns the original."""
+ max_val = (1 << bit_width) - 1
+ value = format(random.randint(0, max_val), f"0{bit_width}b")
+ dtype = f"BIT{bit_width}"
+ assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype)
+
+
+@pytest.mark.parametrize("bit_width", range(1, 17))
+def test_roundtrip_min_bitstrings(bit_width: int):
+ """Zero survives a round-trip for every bit width."""
+ value = "0".zfill(bit_width)
+ dtype = f"BIT{bit_width}"
+ assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype)
+
+
+@pytest.mark.parametrize("bit_width", range(1, 17))
+def test_roundtrip_max_bitstrings(bit_width: int):
+ """All-ones value survives a round-trip for every bit width."""
+ value = "1" * bit_width
+ dtype = f"BIT{bit_width}"
+ assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype)
+
+
+# --- Explicit serialization checks (known input -> known list[int]) ---
+
+@pytest.mark.parametrize("value, dtype, expected", [
+ ("0", "BIT1", [0x00]),
+ ("1", "BIT1", [0x01]),
+ ("11", "BIT2", [0x03]),
+ ("10", "BIT2", [0x02]),
+ ("1111111111111111", "BIT16", [0xff, 0xff]),
+ ("0000000000000000", "BIT16", [0x00, 0x00]),
+ ("1000000000000000", "BIT16", [0x00, 0x80]), # MSB only
+])
+def test_serialize_known_values_bitstrings(value: str, dtype: str, expected: list[int]):
+ assert expected == serialize(value, base_data_type=dtype)
+
+
+# --- Explicit deserialization checks (known list[int] -> known output) ---
+
+@pytest.mark.parametrize("serialized, dtype, expected_value", [
+ ([0x00], "BIT1", "0"),
+ ([0x01], "BIT1", "1"),
+ ([0x03], "BIT2", "11"),
+ ([0x02], "BIT2", "10"),
+ ([0xff, 0xff], "BIT16", "1111111111111111"),
+ ([0x00, 0x00], "BIT16", "0000000000000000"),
+])
+def test_deserialize_known_values_bitstrings(serialized: list[int], dtype: str, expected_value: str):
+ assert expected_value == deserialize(serialized, base_data_type=dtype)
+
+
+# ---------------------------------------------------------------------------
+# BITARR8, BITARR16, BITARR32, BYTE, WORD, DWORD tests
+# ---------------------------------------------------------------------------
+
+# --- round-trip tests ---
+
+@pytest.mark.parametrize("bit_width, dtype", [
+ (8, "BITARR8"),(16, "BITARR16"),(32, "BITARR32"),
+ (8, "BYTE"),(16, "WORD"),(32, "DWORD")])
+def test_roundtrip_random_bitarr_word(bit_width: int, dtype: str):
+ """Serializing then deserializing a random valid value returns the original."""
+ max_val = (1 << bit_width) - 1
+ value = format(random.randint(0, max_val), f"0{bit_width}b")
+ assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype)
+
+
+@pytest.mark.parametrize("bit_width, dtype", [
+ (8, "BITARR8"),(16, "BITARR16"),(32, "BITARR32"),
+ (8, "BYTE"),(16, "WORD"),(32, "DWORD")])
+def test_roundtrip_min_bitarr_word(bit_width: int, dtype: str):
+ """Zero survives a round-trip for every bit width."""
+ value = "0".zfill(bit_width)
+ assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype)
+
+
+@pytest.mark.parametrize("bit_width, dtype", [
+ (8, "BITARR8"),(16, "BITARR16"),(32, "BITARR32"),
+ (8, "BYTE"),(16, "WORD"),(32, "DWORD")])
+def test_roundtrip_max_bitarr_word(bit_width: int, dtype: str):
+ """All-ones value survives a round-trip for every bit width."""
+ value = "1" * bit_width
+ assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype)
+
+
+# --- Explicit serialization checks (known input -> known list[int]) ---
+
+@pytest.mark.parametrize("value, dtype, expected", [
+ ("10101100", "BITARR8", [0xac]),
+ ("01010101", "BITARR8", [0x55]),
+ ("1100101001110001", "BITARR16", [0x71, 0xca]),
+ ("0011110000101110", "BITARR16", [0x2e, 0x3c]),
+ ("10010000111100001010101001101101", "BITARR32", [0x6d, 0xaa, 0xf0, 0x90]),
+ ("01101111000101011000001111110000", "BITARR32", [0xf0, 0x83, 0x15, 0x6f]),
+ ("10101100", "BYTE", [0xac]),
+ ("01010101", "BYTE", [0x55]),
+ ("1100101001110001", "WORD", [0x71, 0xca]),
+ ("0011110000101110", "WORD", [0x2e, 0x3c]),
+ ("10010000111100001010101001101101", "DWORD", [0x6d, 0xaa, 0xf0, 0x90]),
+ ("01101111000101011000001111110000", "DWORD", [0xf0, 0x83, 0x15, 0x6f]),
+])
+def test_serialize_known_values_bitarr_word(value: str, dtype: str, expected: list[int]):
+ assert expected == serialize(value, base_data_type=dtype)
+
+
+# --- Explicit deserialization checks (known list[int] -> known output) ---
+
+@pytest.mark.parametrize("serialized, dtype, expected_value", [
+ ([0xac], "BITARR8", "10101100"),
+ ([0x55], "BITARR8", "01010101"),
+ ([0x71, 0xca], "BITARR16", "1100101001110001"),
+ ([0x2e, 0x3c], "BITARR16", "0011110000101110"),
+ ([0x6d, 0xaa, 0xf0, 0x90], "BITARR32", "10010000111100001010101001101101"),
+ ([0xf0, 0x83, 0x15, 0x6f], "BITARR32", "01101111000101011000001111110000"),
+ ([0xac], "BYTE", "10101100"),
+ ([0x55], "BYTE", "01010101"),
+ ([0x71, 0xca], "WORD", "1100101001110001"),
+ ([0x2e, 0x3c], "WORD", "0011110000101110"),
+ ([0x6d, 0xaa, 0xf0, 0x90], "DWORD", "10010000111100001010101001101101"),
+ ([0xf0, 0x83, 0x15, 0x6f], "DWORD", "01101111000101011000001111110000"),
+])
+def test_deserialize_known_values_bitarr_word(serialized: list[int], dtype: str, expected_value: str):
+ assert expected_value == deserialize(serialized, base_data_type=dtype)
+
+
+# ---------------------------------------------------------------------------
+# Signed integers 8, 16, 24, 32, 40, 48, 56, 64 tests
+# ---------------------------------------------------------------------------
+
+# --- round-trip tests ---
+
+@pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64])
+def test_roundtrip_random_sint(bit_width: int):
+ """Serializing then deserializing a random valid value returns the original."""
+ max_val = 1 << (bit_width-1)
+ value = random.randint(-max_val, (max_val-1))
+ dtype = f"INTEGER{bit_width}"
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64])
+def test_roundtrip_zero_sint(bit_width: int):
+ """Zero survives a round-trip for every integer size."""
+ value = 0
+ dtype = f"INTEGER{bit_width}"
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64])
+def test_roundtrip_max_sint(bit_width: int):
+ """max-value survives a round-trip for every integer size."""
+ max_val = 1 << (bit_width-1)
+ value = max_val-1
+ dtype = f"INTEGER{bit_width}"
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64])
+def test_roundtrip_min_sint(bit_width: int):
+ """min-value survives a round-trip for every integer size."""
+ max_val = 1 << (bit_width-1)
+ value = -max_val
+ dtype = f"INTEGER{bit_width}"
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+# --- Explicit serialization checks (known input -> known list[int]) ---
+
+@pytest.mark.parametrize("value, dtype, expected", [
+ (-(1 << 3), "INTEGER8", [0xf8]),
+ ((1 << 12), "INTEGER16", [0x00, 0x10]),
+ (-(1 << 22), "INTEGER24", [0x00, 0x00, 0xc0]),
+ ((1 << 2), "INTEGER32", [0x04, 0x00, 0x00, 0x00]),
+ (-(1 << 20), "INTEGER40", [0x00, 0x00, 0xf0, 0xff, 0xff]),
+ ((1 << 40), "INTEGER48", [0x00, 0x00, 0x00, 0x00, 0x00, 0x01]),
+ (-(1 << 40), "INTEGER56", [0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff]),
+ ((1 << 62), "INTEGER64", [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40]),
+])
+def test_serialize_known_values_sint(value: int, dtype: str, expected: list[int]):
+ assert expected == serialize(value, name=dtype)
+
+
+# --- Explicit deserialization checks (known list[int] -> known output) ---
+
+@pytest.mark.parametrize("serialized, dtype, expected_value", [
+ ([0xf8], "INTEGER8", -(1 << 3)),
+ ([0x00, 0x10], "INTEGER16", (1 << 12)),
+ ([0x00, 0x00, 0xc0], "INTEGER24", -(1 << 22)),
+ ([0x04, 0x00, 0x00, 0x00], "INTEGER32", (1 << 2)),
+ ([0x00, 0x00, 0xf0, 0xff, 0xff], "INTEGER40", -(1 << 20)),
+ ([0x00, 0x00, 0x00, 0x00, 0x00, 0x01], "INTEGER48", (1 << 40)),
+ ([0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff], "INTEGER56", -(1 << 40)),
+ ([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40], "INTEGER64", (1 << 62)),
+])
+def test_deserialize_known_values_sint(serialized: list[int], dtype: str, expected_value: int):
+ assert expected_value == deserialize(serialized, name=dtype)
+
+
+# ---------------------------------------------------------------------------
+# Unsigned integers 8, 16, 24, 32, 40, 48, 56, 64 tests
+# ---------------------------------------------------------------------------
+
+# --- round-trip tests ---
+
+@pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64])
+def test_roundtrip_random_uint(bit_width: int):
+ """Serializing then deserializing a random valid value returns the original."""
+ max_val = (1 << bit_width)-1
+ value = random.randint(0, max_val)
+ dtype = f"UNSIGNED{bit_width}"
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64])
+def test_roundtrip_min_uint(bit_width: int):
+ """Zero survives a round-trip for every unsigned integer size."""
+ value = 0
+ dtype = f"UNSIGNED{bit_width}"
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64])
+def test_roundtrip_max_uint(bit_width: int):
+ """max-value survives a round-trip for every unsigned integer size."""
+ max_val = (1 << bit_width)-1
+ value = max_val
+ dtype = f"UNSIGNED{bit_width}"
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+# ---------------------------------------------------------------------------
+# Bool tests
+# ---------------------------------------------------------------------------
+
+# --- round-trip test ---
+
+def test_roundtrip_random_bool():
+ """Serializing then deserializing a random valid value returns the original."""
+ value = random.choice([True, False, 1, 0])
+ assert value == deserialize(serialize(value, base_data_type="BOOL"), base_data_type="BOOL")
+
+
+# --- Explicit serialization checks (known input -> known list[int]) ---
+
+@pytest.mark.parametrize("value, dtype, expected", [
+ (False, "BOOL", [0x00]),
+ (True, "BOOL", [0xff]),
+ (0, "BOOL", [0x00]),
+ (1, "BOOL", [0xff]),
+])
+def test_serialize_known_values_bool(value, dtype: str, expected: list[int]):
+ assert expected == serialize(value, base_data_type=dtype)
+
+
+# --- Explicit deserialization checks (known list[int] -> known output) ---
+
+@pytest.mark.parametrize("serialized, dtype, expected_value", [
+ ([0x00], "BOOL", False),
+ ([0x01], "BOOL", True),
+ ([0x00], "BOOL", 0),
+ ([0x01], "BOOL", 1),
+])
+def test_deserialize_known_values_bool(serialized: list[int], dtype: str, expected_value):
+ assert expected_value == deserialize(serialized, base_data_type=dtype)
+
+
+# ---------------------------------------------------------------------------
+# Floating-point REAL32, REAL64 tests
+# ---------------------------------------------------------------------------
+
+# --- round-trip tests ---
+
+@pytest.mark.parametrize(("dtype", "value"), [
+ ("REAL32", random.uniform(-3.4e38, 3.4e38)),
+ ("REAL64", random.uniform(-1.7e308, 1.7e308)),
+])
+def test_roundtrip_random_real(dtype: str, value: float):
+ """Serializing then deserializing a random valid float returns the original."""
+ result = deserialize(serialize(value, name=dtype), name=dtype)
+ if dtype == "REAL32":
+ assert result == pytest.approx(value, rel=1e-6) # Python uses 64 bit so we lose some precision on the roundtrip
+ else:
+ assert result == value
+
+
+@pytest.mark.parametrize("dtype", ["REAL32", "REAL64"])
+def test_roundtrip_zero_real(dtype: str):
+ """Zero survives a round-trip for every floating-point size."""
+ value = 0.0
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize(("dtype", "value"), [
+ ("REAL32", float("inf")),
+ ("REAL32", float("-inf")),
+ ("REAL64", float("inf")),
+ ("REAL64", float("-inf")),
+])
+def test_roundtrip_infinity_real(dtype: str, value: float):
+ """Infinity values survive a round-trip."""
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize("dtype", ["REAL32", "REAL64"])
+def test_roundtrip_nan_real(dtype: str):
+ """NaN survives a round-trip."""
+ value = float("nan")
+ result = deserialize(serialize(value, name=dtype), name=dtype)
+ assert isnan(result)
+
+
+# ---------------------------------------------------------------------------
+# TIME_OF_DAY, TIME_DIFFERENCE tests
+# ---------------------------------------------------------------------------
+
+# --- round-trip tests ---
+
+@pytest.mark.parametrize("dtype", ["TIME_OF_DAY", "TIME_DIFFERENCE"])
+def test_roundtrip_random_time48(dtype: str):
+ """Serializing then deserializing a random valid value returns the original."""
+ ms = random.randint(0, (1 << 28) - 1)
+ days = random.randint(0, (1 << 16) - 1)
+ value = (ms, days)
+ assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype)
+
+
+@pytest.mark.parametrize("dtype", ["TIME_OF_DAY", "TIME_DIFFERENCE"])
+def test_roundtrip_min_time48(dtype: str):
+ """Zero ms and days survive a round-trip for both data types."""
+ assert (0, 0) == deserialize(serialize((0, 0), base_data_type=dtype), base_data_type=dtype)
+
+
+@pytest.mark.parametrize("dtype", ["TIME_OF_DAY", "TIME_DIFFERENCE"])
+def test_roundtrip_max_time48(dtype: str):
+ """Max-value ms and days survive a round-trip for both data types."""
+ ms = (1 << 28) - 1
+ days = (1 << 16) - 1
+ value = (ms, days)
+ assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype)
+
+
+# --- Explicit serialization checks (known input -> known list[int]) ---
+
+@pytest.mark.parametrize("value, dtype, expected", [
+ ((5000, 5000), "TIME_OF_DAY", [0x00, 0x00, 0x13, 0x88, 0x13, 0x88]),
+ ((1000, 1000), "TIME_DIFFERENCE", [0x00, 0x00, 0x03, 0xE8, 0x03, 0xE8]),
+])
+def test_serialize_known_values_time48(value: int, dtype: str, expected: list[int]):
+ assert expected == serialize(value, name=dtype)
+
+
+# --- Explicit deserialization checks (known list[int] -> known output) ---
+
+@pytest.mark.parametrize("serialized, dtype, expected_value", [
+ ([0x00, 0x00, 0x13, 0x88, 0x13, 0x88], "TIME_OF_DAY", (5000, 5000)),
+ ([0x00, 0x00, 0x03, 0xE8, 0x03, 0xE8], "TIME_DIFFERENCE", (1000, 1000)),
+])
+def test_deserialize_known_values_time48(serialized: list[int], dtype: str, expected_value: int):
+ assert expected_value == deserialize(serialized, name=dtype)
+
+
+# ---------------------------------------------------------------------------
+# GUID tests
+# ---------------------------------------------------------------------------
+
+# --- round-trip test ---
+
+def test_roundtrip_random_guid():
+ """Serializing then deserializing a random valid value returns the original."""
+ max_val = 1 << (128-1)
+ value = random.randint(-max_val, (max_val-1))
+ assert value == deserialize(serialize(value, base_data_type="GUID"), base_data_type="GUID")
+
+
+# ---------------------------------------------------------------------------
+# VISIBLE_STRING (ASCII, 1 byte per char) tests
+# ---------------------------------------------------------------------------
+
+# --- round-trip tests ---
+
+@pytest.mark.parametrize("value", [
+ "hello",
+ "Hello, World!",
+ "EtherCAT",
+ "test 123",
+ "", # empty string
+ "A", # single char
+ " ", # space
+ "!@#$%^&*()", # special ASCII chars
+ "a" * 100, # long string
+])
+def test_roundtrip_visible_string(value: str):
+ """Serializing then deserializing a string returns the original."""
+ assert value == deserialize(serialize(value, name="VISIBLE_STRING"), name="VISIBLE_STRING")
+
+
+# --- Explicit serialization checks (known input -> known list[int]) ---
+
+@pytest.mark.parametrize("value, expected", [
+ ("A", [0x41]),
+ ("AB", [0x41, 0x42]),
+ ("hi", [0x68, 0x69]),
+ ("", []),
+ ("\x00", [0x00]), # null byte
+ (" ", [0x20]), # space
+ ("ABC", [0x41, 0x42, 0x43]),
+])
+def test_serialize_known_values_visible_string(value: str, expected: list[int]):
+ assert expected == serialize(value, name="VISIBLE_STRING")
+
+
+# --- Explicit deserialization checks (known list[int] -> known output) ---
+
+@pytest.mark.parametrize("serialized, expected_value", [
+ ([0x41], "A"),
+ ([0x41, 0x42], "AB"),
+ ([0x68, 0x69], "hi"),
+ ([], ""),
+ ([0x00], "\x00"),
+ ([0x20], " "),
+ ([0x41, 0x42, 0x43], "ABC"),
+ ([0x63, 0x61, 0x66, 0xC3, 0xA9], "café"), # 2-byte sequence: é = 0xC3 0xA9
+ ([0xE6, 0x97, 0xA5, 0xE6, 0x9C, 0xAC, 0xE8, 0xAA, 0x9E], "日本語"), # 3-byte sequences
+ ([0xF0, 0x9F, 0x98, 0x80], "😀"), # 4-byte sequence
+])
+def test_deserialize_known_values_visible_string(serialized: list[int], expected_value: str):
+ assert expected_value == deserialize(serialized, name="VISIBLE_STRING")
+
+
+# --- Invalid input ---
+
+@pytest.mark.parametrize("value", [
+ ["h", "b"],
+ 5
+])
+def test_serialize_visible_string_rejects_invalid(value: str):
+ """Byte sequences that are not valid UTF-8 should raise."""
+ assert -1 == serialize(value, name="VISIBLE_STRING")
+
+
+# ---------------------------------------------------------------------------
+# UNICODE_STRING (UTF-16-LE, 2 bytes per char) tests
+# ---------------------------------------------------------------------------
+
+# --- round-trip tests ---
+
+@pytest.mark.parametrize("value", [
+ "hello",
+ "Hello, World!",
+ "", # empty string
+ "A", # single char
+ "café", # accented characters
+ "日本語", # CJK characters
+ "EtherCAT 🚀", # emoji (surrogate pair in UTF-16)
+ "a" * 100, # long string
+])
+def test_roundtrip_unicode_string(value: str):
+ """Serializing then deserializing a unicode string returns the original."""
+ assert value == deserialize(serialize(value, name="UNICODE_STRING"), name="UNICODE_STRING")
+
+
+# --- Explicit serialization checks (known input -> known list[int]) ---
+
+@pytest.mark.parametrize("value, expected", [
+ ("A", [0x41, 0x00]), # U+0041, LE
+ ("AB", [0x41, 0x00, 0x42, 0x00]), # two ASCII chars
+ ("", []), # empty
+ (" ", [0x20, 0x00]), # space
+ ("é", [0xe9, 0x00]), # U+00E9
+ ("中", [0x2d, 0x4e]), # U+4E2D
+])
+def test_serialize_known_values_unicode_string(value: str, expected: list[int]):
+ assert expected == serialize(value, name="UNICODE_STRING")
+
+
+# --- Explicit deserialization checks (known list[int] -> known output) ---
+
+@pytest.mark.parametrize("serialized, expected_value", [
+ ([0x41, 0x00], "A"),
+ ([0x41, 0x00, 0x42, 0x00], "AB"),
+ ([], ""),
+ ([0x20, 0x00], " "),
+ ([0xe9, 0x00], "é"),
+ ([0x2d, 0x4e], "中"),
+])
+def test_deserialize_known_values_unicode_string(serialized: list[int], expected_value: str):
+ assert expected_value == deserialize(serialized, name="UNICODE_STRING")
+
+
+# --- Byte count is always even ---
+
+@pytest.mark.parametrize("value", ["A", "AB", "ABC", "日本語"])
+def test_unicode_string_serialized_length_is_even(value: str):
+ """UTF-16-LE encoding always produces an even number of bytes."""
+ assert len(serialize(value, name="UNICODE_STRING")) % 2 == 0
+
+
+# --- Odd-length input is rejected ---
+
+def test_deserialize_unicode_string_rejects_odd_length():
+ """An odd number of bytes cannot be valid UTF-16-LE."""
+ assert -1 == deserialize([0x41], name="UNICODE_STRING")
+
+
+# ---------------------------------------------------------------------------
+# OCTET_STRING and ARRAY_OF_BITARRn tests (arrays of bit strings)
+# ---------------------------------------------------------------------------
+
+# --- round-trip tests ---
+
+@pytest.mark.parametrize("dtype, bit_width", [
+ ("OCTET_STRING", 8),
+ ("ARRAY_OF_BITARR8", 8),
+ ("ARRAY_OF_BITARR16", 16),
+ ("ARRAY_OF_BITARR32", 32),
+])
+def test_roundtrip_random_bitarr_array(dtype: str, bit_width: int):
+ """Serializing then deserializing a random array of bit strings returns the original."""
+ max_val = (1 << bit_width) - 1
+ value = [format(random.randint(0, max_val), f"0{bit_width}b") for _ in range(random.randint(1, 8))]
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize("dtype, bit_width", [
+ ("OCTET_STRING", 8),
+ ("ARRAY_OF_BITARR8", 8),
+ ("ARRAY_OF_BITARR16", 16),
+ ("ARRAY_OF_BITARR32", 32),
+])
+def test_roundtrip_single_element_bitarr_array(dtype: str, bit_width: int):
+ """Single-element array survives a round-trip."""
+ value = ["0" * bit_width]
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize("dtype, bit_width", [
+ ("OCTET_STRING", 8),
+ ("ARRAY_OF_BITARR8", 8),
+ ("ARRAY_OF_BITARR16", 16),
+ ("ARRAY_OF_BITARR32", 32),
+])
+def test_roundtrip_all_ones_bitarr_array(dtype: str, bit_width: int):
+ """All-ones array survives a round-trip."""
+ value = ["1" * bit_width] * 4
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+# --- Explicit serialization checks ---
+
+@pytest.mark.parametrize("value, dtype, expected", [
+ # OCTET_STRING / ARRAY_OF_BITARR8: 1 byte per element
+ (["10101100"], "OCTET_STRING", [0xac]),
+ (["10101100", "01010101"], "OCTET_STRING", [0xac, 0x55]),
+ (["00000000", "11111111"], "OCTET_STRING", [0x00, 0xff]),
+ (["10101100"], "ARRAY_OF_BITARR8", [0xac]),
+ (["10101100", "01010101"], "ARRAY_OF_BITARR8", [0xac, 0x55]),
+ # ARRAY_OF_BITARR16: 2 bytes per element
+ (["1100101001110001"], "ARRAY_OF_BITARR16", [0x71, 0xca]),
+ (["1100101001110001", "0011110000101110"], "ARRAY_OF_BITARR16", [0x71, 0xca, 0x2e, 0x3c]),
+ # ARRAY_OF_BITARR32: 4 bytes per element
+ (["10010000111100001010101001101101"], "ARRAY_OF_BITARR32", [0x6d, 0xaa, 0xf0, 0x90]),
+ (["10010000111100001010101001101101",
+ "01101111000101011000001111110000"], "ARRAY_OF_BITARR32", [0x6d, 0xaa, 0xf0, 0x90,
+ 0xf0, 0x83, 0x15, 0x6f]),
+])
+def test_serialize_known_values_bitarr_array(value: list, dtype: str, expected: list[int]):
+ assert expected == serialize(value, name=dtype)
+
+
+# --- Explicit deserialization checks ---
+
+@pytest.mark.parametrize("serialized, dtype, expected_value", [
+ ([0xac], "OCTET_STRING", ["10101100"]),
+ ([0xac, 0x55], "OCTET_STRING", ["10101100", "01010101"]),
+ ([0x00, 0xff], "OCTET_STRING", ["00000000", "11111111"]),
+ ([0xac], "ARRAY_OF_BITARR8", ["10101100"]),
+ ([0xac, 0x55], "ARRAY_OF_BITARR8", ["10101100", "01010101"]),
+ ([0x71, 0xca], "ARRAY_OF_BITARR16", ["1100101001110001"]),
+ ([0x71, 0xca, 0x2e, 0x3c], "ARRAY_OF_BITARR16", ["1100101001110001", "0011110000101110"]),
+ ([0x6d, 0xaa, 0xf0, 0x90], "ARRAY_OF_BITARR32", ["10010000111100001010101001101101"]),
+ ([0x6d, 0xaa, 0xf0, 0x90,
+ 0xf0, 0x83, 0x15, 0x6f], "ARRAY_OF_BITARR32", ["10010000111100001010101001101101",
+ "01101111000101011000001111110000"]),
+])
+def test_deserialize_known_values_bitarr_array(serialized: list[int], dtype: str, expected_value: list):
+ assert expected_value == deserialize(serialized, name=dtype)
+
+
+# ---------------------------------------------------------------------------
+# ARRAY_OF_UINT, ARRAY_OF_UDINT, ARRAY_OF_USINT tests (unsigned int arrays)
+# ---------------------------------------------------------------------------
+
+# --- round-trip tests ---
+
+@pytest.mark.parametrize("dtype, bit_width", [
+ ("ARRAY_OF_USINT", 8),
+ ("ARRAY_OF_UINT", 16),
+ ("ARRAY_OF_UDINT", 32),
+])
+def test_roundtrip_random_uint_array(dtype: str, bit_width: int):
+ """Serializing then deserializing a random array of uints returns the original."""
+ max_val = (1 << bit_width) - 1
+ value = [random.randint(0, max_val) for _ in range(random.randint(1, 8))]
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize("dtype", ["ARRAY_OF_USINT", "ARRAY_OF_UINT", "ARRAY_OF_UDINT"])
+def test_roundtrip_zeros_uint_array(dtype: str):
+ """Array of zeros survives a round-trip."""
+ value = [0, 0, 0, 0]
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize("dtype, bit_width", [
+ ("ARRAY_OF_USINT", 8),
+ ("ARRAY_OF_UINT", 16),
+ ("ARRAY_OF_UDINT", 32),
+])
+def test_roundtrip_max_uint_array(dtype: str, bit_width: int):
+ """Array of max values survives a round-trip."""
+ max_val = (1 << bit_width) - 1
+ value = [max_val] * 4
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+# --- Explicit serialization checks ---
+
+@pytest.mark.parametrize("value, dtype, expected", [
+ # ARRAY_OF_USINT: 1 byte per element
+ ([0], "ARRAY_OF_USINT", [0x00]),
+ ([255], "ARRAY_OF_USINT", [0xff]),
+ ([1, 2, 3], "ARRAY_OF_USINT", [0x01, 0x02, 0x03]),
+ ([0, 128, 255],"ARRAY_OF_USINT", [0x00, 0x80, 0xff]),
+ # ARRAY_OF_UINT: 2 bytes per element (little-endian)
+ ([0], "ARRAY_OF_UINT", [0x00, 0x00]),
+ ([1], "ARRAY_OF_UINT", [0x01, 0x00]),
+ ([256], "ARRAY_OF_UINT", [0x00, 0x01]),
+ ([1, 2], "ARRAY_OF_UINT", [0x01, 0x00, 0x02, 0x00]),
+ ([0x1234], "ARRAY_OF_UINT", [0x34, 0x12]),
+ # ARRAY_OF_UDINT: 4 bytes per element (little-endian)
+ ([0], "ARRAY_OF_UDINT", [0x00, 0x00, 0x00, 0x00]),
+ ([1], "ARRAY_OF_UDINT", [0x01, 0x00, 0x00, 0x00]),
+ ([0x12345678], "ARRAY_OF_UDINT", [0x78, 0x56, 0x34, 0x12]),
+ ([1, 2], "ARRAY_OF_UDINT", [0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00]),
+])
+def test_serialize_known_values_uint_array(value: list, dtype: str, expected: list[int]):
+ assert expected == serialize(value, name=dtype)
+
+
+# --- Explicit deserialization checks ---
+
+@pytest.mark.parametrize("serialized, dtype, expected_value", [
+ ([0x00], "ARRAY_OF_USINT", [0]),
+ ([0xff], "ARRAY_OF_USINT", [255]),
+ ([0x01, 0x02, 0x03], "ARRAY_OF_USINT", [1, 2, 3]),
+ ([0x00, 0x80, 0xff], "ARRAY_OF_USINT", [0, 128, 255]),
+ ([0x00, 0x00], "ARRAY_OF_UINT", [0]),
+ ([0x01, 0x00], "ARRAY_OF_UINT", [1]),
+ ([0x34, 0x12], "ARRAY_OF_UINT", [0x1234]),
+ ([0x01, 0x00, 0x02, 0x00], "ARRAY_OF_UINT", [1, 2]),
+ ([0x00, 0x00, 0x00, 0x00], "ARRAY_OF_UDINT", [0]),
+ ([0x78, 0x56, 0x34, 0x12], "ARRAY_OF_UDINT", [0x12345678]),
+ ([0x01, 0x00, 0x00, 0x00,
+ 0x02, 0x00, 0x00, 0x00], "ARRAY_OF_UDINT", [1, 2]),
+])
+def test_deserialize_known_values_uint_array(serialized: list[int], dtype: str, expected_value: list):
+ assert expected_value == deserialize(serialized, name=dtype)
+
+
+# ---------------------------------------------------------------------------
+# ARRAY_OF_INT, ARRAY_OF_SINT, ARRAY_OF_DINT tests (signed int arrays)
+# ---------------------------------------------------------------------------
+
+# --- round-trip tests ---
+
+@pytest.mark.parametrize("dtype, bit_width", [
+ ("ARRAY_OF_SINT", 8),
+ ("ARRAY_OF_INT", 16),
+ ("ARRAY_OF_DINT", 32),
+])
+def test_roundtrip_random_sint_array(dtype: str, bit_width: int):
+ """Serializing then deserializing a random array of signed ints returns the original."""
+ half = 1 << (bit_width - 1)
+ value = [random.randint(-half, half - 1) for _ in range(random.randint(1, 8))]
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize("dtype", ["ARRAY_OF_SINT", "ARRAY_OF_INT", "ARRAY_OF_DINT"])
+def test_roundtrip_zeros_sint_array(dtype: str):
+ """Array of zeros survives a round-trip."""
+ value = [0, 0, 0, 0]
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+@pytest.mark.parametrize("dtype, bit_width", [
+ ("ARRAY_OF_SINT", 8),
+ ("ARRAY_OF_INT", 16),
+ ("ARRAY_OF_DINT", 32),
+])
+def test_roundtrip_min_max_sint_array(dtype: str, bit_width: int):
+ """Array of min and max values survives a round-trip."""
+ half = 1 << (bit_width - 1)
+ value = [-half, half - 1]
+ assert value == deserialize(serialize(value, name=dtype), name=dtype)
+
+
+# --- Explicit serialization checks ---
+
+@pytest.mark.parametrize("value, dtype, expected", [
+ # ARRAY_OF_SINT: 1 byte per element (signed)
+ ([0], "ARRAY_OF_SINT", [0x00]),
+ ([-1], "ARRAY_OF_SINT", [0xff]),
+ ([127], "ARRAY_OF_SINT", [0x7f]),
+ ([-128], "ARRAY_OF_SINT", [0x80]),
+ ([-1, 1], "ARRAY_OF_SINT", [0xff, 0x01]),
+ # ARRAY_OF_INT: 2 bytes per element (signed, little-endian)
+ ([0], "ARRAY_OF_INT", [0x00, 0x00]),
+ ([-1], "ARRAY_OF_INT", [0xff, 0xff]),
+ ([1], "ARRAY_OF_INT", [0x01, 0x00]),
+ ([-1, 1], "ARRAY_OF_INT", [0xff, 0xff, 0x01, 0x00]),
+ ([(1 << 12)],"ARRAY_OF_INT", [0x00, 0x10]),
+ # ARRAY_OF_DINT: 4 bytes per element (signed, little-endian)
+ ([0], "ARRAY_OF_DINT", [0x00, 0x00, 0x00, 0x00]),
+ ([-1], "ARRAY_OF_DINT", [0xff, 0xff, 0xff, 0xff]),
+ ([1], "ARRAY_OF_DINT", [0x01, 0x00, 0x00, 0x00]),
+ ([-1, 1], "ARRAY_OF_DINT", [0xff, 0xff, 0xff, 0xff, 0x01, 0x00, 0x00, 0x00]),
+])
+def test_serialize_known_values_sint_array(value: list, dtype: str, expected: list[int]):
+ assert expected == serialize(value, name=dtype)
+
+
+# --- Explicit deserialization checks ---
+
+@pytest.mark.parametrize("serialized, dtype, expected_value", [
+ ([0x00], "ARRAY_OF_SINT", [0]),
+ ([0xff], "ARRAY_OF_SINT", [-1]),
+ ([0x7f], "ARRAY_OF_SINT", [127]),
+ ([0x80], "ARRAY_OF_SINT", [-128]),
+ ([0xff, 0x01], "ARRAY_OF_SINT", [-1, 1]),
+ ([0x00, 0x00], "ARRAY_OF_INT", [0]),
+ ([0xff, 0xff], "ARRAY_OF_INT", [-1]),
+ ([0xff, 0xff, 0x01, 0x00], "ARRAY_OF_INT", [-1, 1]),
+ ([0x00, 0x10], "ARRAY_OF_INT", [1 << 12]),
+ ([0x00, 0x00, 0x00, 0x00], "ARRAY_OF_DINT", [0]),
+ ([0xff, 0xff, 0xff, 0xff], "ARRAY_OF_DINT", [-1]),
+ ([0xff, 0xff, 0xff, 0xff,
+ 0x01, 0x00, 0x00, 0x00], "ARRAY_OF_DINT", [-1, 1]),
+])
+def test_deserialize_known_values_sint_array(serialized: list[int], dtype: str, expected_value: list):
+ assert expected_value == deserialize(serialized, name=dtype)
+
+
+# ---------------------------------------------------------------------------
+# ARRAY_OF_REAL, ARRAY_OF_LREAL tests (float arrays)
+# ---------------------------------------------------------------------------
+
+# --- round-trip tests ---
+
+@pytest.mark.parametrize("dtype, gen", [
+ ("ARRAY_OF_REAL", lambda: random.uniform(-3.4e38, 3.4e38)),
+ ("ARRAY_OF_LREAL", lambda: random.uniform(-1.7e308, 1.7e308)),
+])
+def test_roundtrip_random_float_array(dtype: str, gen):
+ """Serializing then deserializing a random float array returns the original."""
+ value = [gen() for _ in range(random.randint(1, 8))]
+ result = deserialize(serialize(value, name=dtype), name=dtype)
+ if dtype == "ARRAY_OF_REAL":
+ assert result == pytest.approx(value, rel=1e-6)
+ else:
+ assert result == value
+
+
+@pytest.mark.parametrize("dtype", ["ARRAY_OF_REAL", "ARRAY_OF_LREAL"])
+def test_roundtrip_zeros_float_array(dtype: str):
+ """Array of zeros survives a round-trip."""
+ value = [0.0, 0.0, 0.0]
+ assert deserialize(serialize(value, name=dtype), name=dtype) == value
+
+
+@pytest.mark.parametrize("dtype", ["ARRAY_OF_REAL", "ARRAY_OF_LREAL"])
+def test_roundtrip_inf_float_array(dtype: str):
+ """Infinity values survive a round-trip."""
+ value = [float("inf"), float("-inf")]
+ assert deserialize(serialize(value, name=dtype), name=dtype) == value
+
+
+@pytest.mark.parametrize("dtype", ["ARRAY_OF_REAL", "ARRAY_OF_LREAL"])
+def test_roundtrip_nan_float_array(dtype: str):
+ """NaN values survive a round-trip."""
+ value = [float("nan"), float("nan")]
+ result = deserialize(serialize(value, name=dtype), name=dtype)
+ assert all(isnan(r) for r in result)
+
+
+# --- Explicit serialization checks ---
+
+@pytest.mark.parametrize("value, dtype, expected", [
+ # ARRAY_OF_REAL: 4 bytes per element
+ ([0.0], "ARRAY_OF_REAL", [0x00, 0x00, 0x00, 0x00]),
+ ([1.0], "ARRAY_OF_REAL", [0x00, 0x00, 0x80, 0x3f]),
+ ([-1.0], "ARRAY_OF_REAL", [0x00, 0x00, 0x80, 0xbf]),
+ ([0.0, 1.0], "ARRAY_OF_REAL", [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x3f]),
+ # ARRAY_OF_LREAL: 8 bytes per element
+ ([0.0], "ARRAY_OF_LREAL", [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]),
+ ([1.0], "ARRAY_OF_LREAL", [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf0, 0x3f]),
+ ([-1.0], "ARRAY_OF_LREAL", [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf0, 0xbf]),
+])
+def test_serialize_known_values_float_array(value: list, dtype: str, expected: list[int]):
+ assert expected == serialize(value, name=dtype)
+
+
+# --- Explicit deserialization checks ---
+
+@pytest.mark.parametrize("serialized, dtype, expected_value", [
+ ([0x00, 0x00, 0x00, 0x00], "ARRAY_OF_REAL", [0.0]),
+ ([0x00, 0x00, 0x80, 0x3f], "ARRAY_OF_REAL", [1.0]),
+ ([0x00, 0x00, 0x80, 0xbf], "ARRAY_OF_REAL", [-1.0]),
+ ([0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x80, 0x3f], "ARRAY_OF_REAL", [0.0, 1.0]),
+ ([0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00], "ARRAY_OF_LREAL", [0.0]),
+ ([0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0xf0, 0x3f], "ARRAY_OF_LREAL", [1.0]),
+ ([0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0xf0, 0xbf], "ARRAY_OF_LREAL", [-1.0]),
+])
+def test_deserialize_known_values_float_array(serialized: list[int], dtype: str, expected_value: list):
+ assert expected_value == pytest.approx(deserialize(serialized, name=dtype))
+
+
+# ---------------------------------------------------------------------------
+# General array property tests (all array types)
+# ---------------------------------------------------------------------------
+
+ALL_ARRAY_TYPES = [
+ "OCTET_STRING",
+ "ARRAY_OF_UINT", "ARRAY_OF_INT", "ARRAY_OF_SINT", "ARRAY_OF_DINT", "ARRAY_OF_UDINT",
+ "ARRAY_OF_BITARR8", "ARRAY_OF_BITARR16", "ARRAY_OF_BITARR32",
+ "ARRAY_OF_USINT", "ARRAY_OF_REAL", "ARRAY_OF_LREAL",
+]
+
+ELEMENT_BYTE_WIDTH = {
+ "OCTET_STRING": 1,
+ "ARRAY_OF_USINT": 1, "ARRAY_OF_SINT": 1, "ARRAY_OF_BITARR8": 1,
+ "ARRAY_OF_UINT": 2, "ARRAY_OF_INT": 2, "ARRAY_OF_BITARR16": 2,
+ "ARRAY_OF_UDINT": 4, "ARRAY_OF_DINT": 4, "ARRAY_OF_BITARR32": 4, "ARRAY_OF_REAL": 4,
+ "ARRAY_OF_LREAL": 8,
+}
+
+ELEMENT_GENERATORS = {
+ "OCTET_STRING": lambda: format(random.randint(0, 0xff), "08b"),
+ "ARRAY_OF_USINT": lambda: random.randint(0, 0xff),
+ "ARRAY_OF_UINT": lambda: random.randint(0, 0xffff),
+ "ARRAY_OF_UDINT": lambda: random.randint(0, 0xffffffff),
+ "ARRAY_OF_SINT": lambda: random.randint(-128, 127),
+ "ARRAY_OF_INT": lambda: random.randint(-32768, 32767),
+ "ARRAY_OF_DINT": lambda: random.randint(-(1 << 31), (1 << 31) - 1),
+ "ARRAY_OF_BITARR8": lambda: format(random.randint(0, 0xff), "08b"),
+ "ARRAY_OF_BITARR16": lambda: format(random.randint(0, 0xffff), "016b"),
+ "ARRAY_OF_BITARR32": lambda: format(random.randint(0, 0xffffffff), "032b"),
+ "ARRAY_OF_REAL": lambda: random.uniform(-1e10, 1e10),
+ "ARRAY_OF_LREAL": lambda: random.uniform(-1e100, 1e100),
+}
+
+
+@pytest.mark.parametrize("dtype", ALL_ARRAY_TYPES)
+def test_serialized_length_matches_element_count(dtype: str):
+ """Serialized byte count equals number of elements × bytes per element."""
+ n = random.randint(1, 8)
+ gen = ELEMENT_GENERATORS[dtype]
+ value = [gen() for _ in range(n)]
+ result = serialize(value, name=dtype)
+ assert len(result) == n * ELEMENT_BYTE_WIDTH[dtype]
+
+
+@pytest.mark.parametrize("dtype", ALL_ARRAY_TYPES)
+def test_empty_array_serializes_to_empty(dtype: str):
+ """An empty array serializes to an empty list."""
+ assert [] == serialize([], name=dtype)
+
+
+@pytest.mark.parametrize("dtype", ALL_ARRAY_TYPES)
+def test_empty_array_deserializes_to_empty(dtype: str):
+ """An empty list deserializes to an empty array."""
+ assert [] == deserialize([], name=dtype)
\ No newline at end of file
diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt
index ce8a2e3..36fd103 100644
--- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt
+++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt
@@ -14,6 +14,13 @@ include_directories(include)
add_subdirectory(SOEM)
+# --- sdo_serializer library ---
+add_library(sdo_serializer_lib INTERFACE)
+target_include_directories(sdo_serializer_lib INTERFACE
+ $
+ $
+)
+
add_executable(
rise_motion_main
src/main.cpp
@@ -50,8 +57,11 @@ install(TARGETS
testing_node
DESTINATION lib/${PROJECT_NAME}
)
+
+
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
+ find_package(ament_cmake_gtest REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
@@ -62,4 +72,18 @@ if(BUILD_TESTING)
ament_lint_auto_find_test_dependencies()
endif()
+install(
+ DIRECTORY include/
+ DESTINATION include
+)
+
+install(
+ TARGETS sdo_serializer_lib
+ EXPORT export_sdo_serializer_lib
+ INCLUDES DESTINATION include
+)
+
+ament_export_targets(export_sdo_serializer_lib)
+ament_export_include_directories(include)
+
ament_package()
diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager.hpp
index 279b07b..3175940 100755
--- a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager.hpp
+++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager.hpp
@@ -26,7 +26,7 @@ class ECManager {
bool set_motor_values_apsa(const std::vector& motor_values);
// Wrappers for SOEM ecx_SDOwrite, ecx_SDOread
- bool sdo_read(uint16 device_id, uint16 index, uint8 subindex, std::vector& value);
+ bool sdo_read(uint16 device_id, uint16 index, uint8 subindex, std::vector& value, uint8 value_size);
bool sdo_write(uint16 device_id, uint16 index, uint8 subindex, std::vector& value);
private:
diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/result.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/result.hpp
new file mode 100644
index 0000000..f7c1584
--- /dev/null
+++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/result.hpp
@@ -0,0 +1,30 @@
+namespace rise
+{
+ template struct Result
+ {
+ T value{};
+ E error{};
+ bool hasValue = false;
+
+ static Result ok(T v)
+ {
+ Result result;
+ result.value = std::move(v);
+ result.hasValue = true;
+ return result;
+ }
+
+ static Result err(E e)
+ {
+ Result result;
+ result.error = std::move(e);
+ result.hasValue = false;
+ return result;
+ }
+
+ explicit operator bool() const
+ {
+ return hasValue;
+ }
+ };
+}
\ No newline at end of file
diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/sdo_serializer.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/sdo_serializer.hpp
new file mode 100644
index 0000000..ca6d80a
--- /dev/null
+++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/sdo_serializer.hpp
@@ -0,0 +1,1273 @@
+// library assumes little-endian
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace sdo
+{
+ enum class ErrorCode
+ {
+ None,
+ InvalidSize,
+ OutOfRange,
+ UnsupportedType,
+ CallFailed,
+ ServiceUnavailable
+ };
+
+ struct Error
+ {
+ ErrorCode code = ErrorCode::None;
+ std::string message = "";
+ };
+
+
+ // as equivalent of the EtherCAT TIME_OF_DAY data type
+ struct TimeOfDay
+ {
+ std::uint32_t ms_since_midnight;
+ std::uint16_t d_since_1984_01_01;
+
+ TimeOfDay() = default;
+ };
+
+ // as equivalent of the EtherCAT TIME_DIFFERENCE data type
+ struct TimeDifference
+ {
+ std::uint32_t ms;
+ std::uint16_t d;
+
+ TimeDifference() = default;
+ };
+
+ struct Int24
+ {
+ std::int32_t value;
+
+ Int24() = default;
+
+ Int24(std::int32_t v) : value(v)
+ {}
+
+ operator std::int32_t() const
+ {
+ return value;
+ }
+ };
+
+ struct Int40
+ {
+ std::int64_t value;
+
+ Int40() = default;
+
+ Int40(std::int64_t v) : value(v)
+ {}
+
+ operator std::int64_t() const
+ {
+ return value;
+ }
+ };
+
+ struct Int48
+ {
+ std::int64_t value;
+
+ Int48() = default;
+
+ Int48(std::int64_t v) : value(v)
+ {}
+
+ operator std::int64_t() const
+ {
+ return value;
+ }
+ };
+
+ struct Int56
+ {
+ std::int64_t value;
+
+ Int56() = default;
+
+ Int56(std::int64_t v) : value(v)
+ {}
+
+ operator std::int64_t() const
+ {
+ return value;
+ }
+ };
+
+ struct UInt24
+ {
+ std::uint32_t value;
+
+ UInt24() = default;
+
+ UInt24(std::uint32_t v) : value(v)
+ {}
+
+ operator std::uint32_t() const
+ {
+ return value;
+ }
+ };
+
+ struct UInt40
+ {
+ std::uint64_t value;
+
+ UInt40() = default;
+
+ UInt40(std::uint64_t v) : value(v)
+ {}
+
+ operator std::uint64_t() const
+ {
+ return value;
+ }
+ };
+
+ struct UInt48
+ {
+ std::uint64_t value;
+
+ UInt48() = default;
+
+ UInt48(std::uint64_t v) : value(v)
+ {}
+
+ operator std::uint64_t() const
+ {
+ return value;
+ }
+ };
+
+ struct UInt56
+ {
+ std::uint64_t value;
+
+ UInt56() = default;
+
+ UInt56(std::uint64_t v) : value(v)
+ {}
+
+ operator std::uint64_t() const
+ {
+ return value;
+ }
+ };
+
+ struct Guid
+ {
+ std::array bytes;
+
+ Guid(std::array b) : bytes(b){};
+ Guid() = default;
+
+ operator std::array() const
+ {
+ return bytes;
+ }
+ };
+
+ template struct STRING
+ {
+ std::string value;
+
+ STRING() = default;
+
+ STRING(std::string v) : value(std::move(v))
+ {}
+
+ operator std::string() const
+ {
+ return value;
+ }
+ };
+
+ template struct WSTRING
+ {
+ std::u16string value;
+
+ WSTRING() = default;
+
+ WSTRING(std::u16string v) : value(std::move(v))
+ {}
+
+ operator std::u16string() const
+ {
+ return value;
+ }
+ };
+
+ template struct ARRAY
+ {
+ std::vector value;
+
+ ARRAY() = default;
+
+ ARRAY(std::vector v) : value(std::move(v))
+ {}
+
+ operator std::vector() const
+ {
+ return value;
+ }
+ };
+
+
+ template using DeserializeResult = rise::Result;
+ using SerializeResult = rise::Result, sdo::Error>;
+
+ // IEC 61131-3 data types
+ using BOOL = bool;
+
+ using SINT = std::int8_t;
+ using INT = std::int16_t;
+ using DINT = std::int32_t;
+ using LINT = std::int64_t;
+
+ using USINT = std::uint8_t;
+ using UINT = std::uint16_t;
+ using UDINT = std::uint32_t;
+ using ULINT = std::uint64_t;
+
+ using REAL = float;
+ using LREAL = double;
+
+ using BYTE = std::uint8_t;
+ using WORD = std::uint16_t;
+ using DWORD = std::uint32_t;
+ using LWORD = std::uint64_t;
+
+ using DATE = sdo::TimeOfDay;
+ using TIME = sdo::TimeDifference;
+
+ // EtherCAT data types
+ using BOOLEAN = bool;
+
+ using INTEGER8 = std::int8_t;
+ using INTEGER16 = std::int16_t;
+ using INTEGER24 = sdo::Int24;
+ using INTEGER32 = std::int32_t;
+ using INTEGER40 = sdo::Int40;
+ using INTEGER48 = sdo::Int48;
+ using INTEGER56 = sdo::Int56;
+ using INTEGER64 = std::int64_t;
+
+ using UNSIGNED8 = std::uint8_t;
+ using UNSIGNED16 = std::uint16_t;
+ using UNSIGNED24 = sdo::UInt24;
+ using UNSIGNED32 = std::uint32_t;
+ using UNSIGNED40 = sdo::UInt40;
+ using UNSIGNED48 = sdo::UInt48;
+ using UNSIGNED56 = sdo::UInt56;
+ using UNSIGNED64 = std::uint64_t;
+
+ using BITARR8 = std::uint8_t;
+ using BITARR16 = std::uint16_t;
+ using BITARR32 = std::uint32_t;
+
+ using BIT1 = std::uint8_t;
+ using BIT2 = std::uint8_t;
+ using BIT3 = std::uint8_t;
+ using BIT4 = std::uint8_t;
+ using BIT5 = std::uint8_t;
+ using BIT6 = std::uint8_t;
+ using BIT7 = std::uint8_t;
+ using BIT8 = std::uint8_t;
+ using BIT9 = std::uint16_t;
+ using BIT10 = std::uint16_t;
+ using BIT11 = std::uint16_t;
+ using BIT12 = std::uint16_t;
+ using BIT13 = std::uint16_t;
+ using BIT14 = std::uint16_t;
+ using BIT15 = std::uint16_t;
+ using BIT16 = std::uint16_t;
+
+ using REAL32 = float;
+ using REAL64 = double;
+
+ using TIME_OF_DAY = sdo::TimeOfDay;
+ using TIME_DIFFERENCE = sdo::TimeDifference;
+
+ using GUID = sdo::Guid;
+ using DOMAIN = std::vector;
+
+ template using VISIBLE_STRING = STRING;
+ template using UNICODE_STRING = WSTRING;
+ template using OCTET_STRING = ARRAY;
+ template using ARRAY_OF_USINT = ARRAY;
+ template using ARRAY_OF_UINT = ARRAY;
+ template using ARRAY_OF_INT = ARRAY;
+ template using ARRAY_OF_SINT = ARRAY;
+ template using ARRAY_OF_DINT = ARRAY;
+ template using ARRAY_OF_UDINT = ARRAY;
+ template using ARRAY_OF_BITARR8 = ARRAY;
+ template using ARRAY_OF_BITARR16 = ARRAY;
+ template using ARRAY_OF_BITARR32 = ARRAY;
+}
+
+
+namespace sdo::helpers
+{
+ template inline constexpr bool always_false = false;
+
+ template struct is_string_type : std::false_type {};
+ template struct is_string_type> : std::true_type {};
+
+ template struct string_size;
+ template struct string_size>
+ {
+ static constexpr std::size_t size = T;
+ };
+
+ template struct is_wstring_type : std::false_type {};
+ template struct is_wstring_type> : std::true_type {};
+
+ template struct wstring_size;
+ template struct wstring_size>
+ {
+ static constexpr std::size_t size = T;
+ };
+
+ template struct is_array_type : std::false_type {};
+ template
+ struct is_array_type> : std::true_type {};
+
+ template struct array_size;
+ template struct array_size>
+ {
+ static constexpr std::size_t size = N;
+ };
+
+ template struct elementType;
+ template struct elementType>
+ {
+ using type = T;
+ };
+
+
+ inline std::optional check_size(
+ const std::vector& blob, std::size_t expectedSize, const char* type)
+ {
+ if (blob.size() != expectedSize){
+ return Error{
+ ErrorCode::InvalidSize,
+ std::string("deserialize<") + type + ">: expected " + std::to_string(expectedSize) +
+ " bytes, got " + std::to_string(blob.size())};
+ }
+
+ return std::nullopt;
+ }
+
+ template [[nodiscard]] inline DeserializeResult to_raw(
+ const std::vector& blob, std::size_t numBytes, std::size_t offset = 0)
+ {
+ static_assert(std::is_unsigned_v, "to_raw: T must be unsigned");
+ static_assert(sizeof(T) <= sizeof(std::uint64_t), "to_raw: T can be max uint64_t");
+
+ if (numBytes > sizeof(T))
+ {
+ return DeserializeResult::err({
+ ErrorCode::InvalidSize, "to_raw: numBytes does not fit T"});
+ }
+
+ if (offset + numBytes > blob.size())
+ {
+ return DeserializeResult::err({
+ ErrorCode::InvalidSize, "to_raw: blob has less bytes than numBytes"});
+ }
+
+ T raw = 0;
+
+ for (std::size_t i = 0; i < numBytes; ++i)
+ {
+ raw |= static_cast(blob[offset + i]) << (8 * i);
+ }
+
+ return DeserializeResult::ok(raw);
+ }
+
+ template [[nodiscard]] inline SerializeResult from_raw(
+ const T& raw, std::size_t numBytes)
+ {
+ static_assert(std::is_unsigned_v, "from_raw: T must be unsigned");
+
+ if (numBytes > sizeof(T))
+ {
+ SerializeResult::err({ErrorCode::InvalidSize, "from_raw: numBytes does not fit T"});
+ }
+
+ std::vector blob;
+ blob.reserve(numBytes);
+
+ for (std::size_t i = 0; i < numBytes; ++i)
+ {
+ blob.push_back(static_cast((raw >> (8 * i)) & 0xFF));
+ }
+
+ return SerializeResult::ok(blob);
+ }
+}
+
+namespace sdo
+{
+ // ---DESERIALIZATION---
+
+ template [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ using CleanT = std::remove_cv_t>;
+
+ if constexpr (sdo::helpers::is_array_type::value)
+ {
+ constexpr std::size_t numElements = sdo::helpers::array_size::size;
+ using ElementT = typename sdo::helpers::elementType::type;
+ constexpr std::size_t elementSize = sizeof(ElementT);
+ constexpr std::size_t byteSize = numElements * elementSize;
+
+ if (blob.size() > byteSize || blob.size() % elementSize != 0){
+ return DeserializeResult::err({
+ ErrorCode::InvalidSize, "deserialize>: invalid blob size"});
+ }
+
+ T result{};
+ result.value.reserve(blob.size() / elementSize);
+
+ for (std::size_t i = 0; i < blob.size(); i += elementSize){
+ if constexpr (elementSize == 1){
+ result.value.push_back(static_cast(blob[i]));
+ }
+ else if constexpr (elementSize == 2){
+ auto raw = sdo::helpers::to_raw(blob, 2, i);
+ if (!raw){
+ return DeserializeResult::err(raw.error);
+ }
+
+ result.value.push_back(static_cast(raw.value));
+ }
+ else if constexpr (elementSize == 4){
+ auto raw = sdo::helpers::to_raw(blob, 4, i);
+ if (!raw){
+ return DeserializeResult::err(raw.error);
+ }
+
+ if constexpr (std::is_same_v){
+ float element;
+ std::memcpy(&element, &raw.value, sizeof(element));
+ result.value.push_back(element);
+ }
+ else{
+ result.value.push_back(static_cast(raw.value));
+ }
+ }
+ else if constexpr (elementSize == 8){
+ auto raw = sdo::helpers::to_raw(blob, 8, i);
+ if (!raw){
+ return DeserializeResult::err(raw.error);
+ }
+
+ if constexpr (std::is_same_v){
+ double element;
+ std::memcpy(&element, &raw.value, sizeof(element));
+ result.value.push_back(element);
+ }
+ else{
+ result.value.push_back(static_cast(raw.value));
+ }
+ }
+ else{
+ return DeserializeResult::err({
+ ErrorCode::UnsupportedType,
+ "deserialize>: unsupported array type"});
+ }
+ }
+
+ return DeserializeResult::ok(std::move(result));
+ }
+ else if constexpr (sdo::helpers::is_wstring_type::value)
+ {
+ constexpr std::size_t size = sdo::helpers::wstring_size::size;
+
+ if (blob.size() > size * 2 || blob.size() % 2 != 0){
+ return DeserializeResult::err({
+ ErrorCode::InvalidSize,
+ "deserialize>: blob is larger than the expected size T"});
+ }
+
+ T str{};
+
+ for (std::size_t i = 0; i < blob.size(); i += 2){
+ std::uint16_t raw =
+ static_cast(blob[i]) |
+ static_cast(blob[i + 1]) << 8;
+
+ str.value.push_back(static_cast(raw));
+ }
+
+ // strip padding zeros
+ while (!str.value.empty() && str.value.back() == u'\0'){
+ str.value.pop_back();
+ }
+
+ return DeserializeResult::ok(str);
+ }
+ else if constexpr (sdo::helpers::is_string_type::value)
+ {
+
+ constexpr std::size_t size = sdo::helpers::string_size::size;
+
+ if (blob.size() > size){
+ return DeserializeResult::err({
+ ErrorCode::InvalidSize,
+ std::string("deserialize>: blob is larger than the expected size T. Expected: ") +
+ std::to_string(size) + ", got: " + std::to_string(blob.size())});
+ }
+
+ T str{};
+
+ str.value.assign(blob.begin(), blob.end());
+
+ // strip padding zeros
+ while (!str.value.empty() && str.value.back() == '\0'){
+ str.value.pop_back();
+ }
+
+ return DeserializeResult::ok(str);
+ }
+ else{
+ static_assert(sdo::helpers::always_false, "deserialize: unsupported type");
+ }
+ }
+
+ // -Boolean-
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 1, "bool")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ return DeserializeResult::ok(blob[0] != 0);
+ }
+
+ // -Signed Integer-
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 1, "int8_t")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ return DeserializeResult::ok(static_cast(blob[0]));
+ }
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 2, "int16_t")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ auto raw = sdo::helpers::to_raw(blob, 2);
+ if(!raw){
+ return DeserializeResult::err(raw.error);
+ }
+
+ auto result = static_cast(raw.value);
+
+ return DeserializeResult::ok(result);
+ }
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 4, "int32_t")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ auto raw = sdo::helpers::to_raw(blob, 4);
+ if(!raw){
+ return DeserializeResult::err(raw.error);
+ }
+
+ auto result = static_cast(raw.value);
+
+ return DeserializeResult::ok(result);
+ }
+
+ // -Unsigned Integer / raw data / bit arrays / bit strings-
+
+ // use for UNSIGNED8, BYTE, BITARR8, BIT1-BIT8
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 1, "uint8_t")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ return DeserializeResult::ok(static_cast(blob[0]));
+ }
+
+ // use for UNSIGNED16, WORD, BITARR16, BIT9-BIT16
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 2, "uint16_t")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ return sdo::helpers::to_raw(blob, 2);
+ }
+
+ // use for UNSIGNED32, DWORD, BITARR32
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 4, "uint32_t")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ return sdo::helpers::to_raw(blob, 4);
+ }
+
+ // -Floating Point-
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 4, "float")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ auto raw = sdo::helpers::to_raw(blob, 4);
+ if(!raw){
+ return DeserializeResult::err(raw.error);
+ }
+
+ float value;
+ std::memcpy(&value, &raw.value, sizeof(value));
+
+ return DeserializeResult::ok(value);
+ }
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 8, "double")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ auto raw = sdo::helpers::to_raw(blob, 8);
+ if(!raw){
+ return DeserializeResult::err(raw.error);
+ }
+
+ double value;
+ std::memcpy(&value, &raw.value, sizeof(value));
+
+ return DeserializeResult::ok(value);
+ }
+
+ // -Time-
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 6, "TimeOfDay")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ TimeOfDay value{};
+
+ auto ms_raw = sdo::helpers::to_raw(blob, 4);
+ if(!ms_raw){
+ return DeserializeResult::err(ms_raw.error);
+ }
+ value.ms_since_midnight = ms_raw.value;
+
+ auto d_raw = sdo::helpers::to_raw(blob, 2, 4);
+ if(!d_raw){
+ return DeserializeResult::err(d_raw.error);
+ }
+ value.d_since_1984_01_01 = d_raw.value;
+
+ return DeserializeResult::ok(value);
+ }
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 6, "TimeDifference")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ TimeDifference value{};
+
+ auto ms_raw = sdo::helpers::to_raw(blob, 4);
+ if(!ms_raw){
+ return DeserializeResult::err(ms_raw.error);
+ }
+ // the upper 4 bits of ms are reserved
+ value.ms = ms_raw.value & 0x0FFFFFFF;
+
+ auto d_raw = sdo::helpers::to_raw(blob, 2, 4);
+ if(!d_raw){
+ return DeserializeResult::err(d_raw.error);
+ }
+ value.d = d_raw.value;
+
+ return DeserializeResult::ok(value);
+ }
+
+ // -Domain-
+ // (returns raw blob as equivalent to the EtherCAT Domain data type)
+
+ template <> [[nodiscard]] inline DeserializeResult> deserialize>(
+ const std::vector& blob)
+ {
+ return DeserializeResult>::ok(blob);
+ }
+
+ // -Extended Signed Integer-
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 3, "Int24")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ auto raw = sdo::helpers::to_raw(blob, 3);
+ if(!raw){
+ return DeserializeResult::err(raw.error);
+ }
+
+ if (raw.value & 0x00800000){
+ raw.value |= 0xFF000000;
+ }
+
+ return DeserializeResult::ok(Int24{static_cast(raw.value)});
+ }
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 5, "Int40")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ auto raw = sdo::helpers::to_raw(blob, 5);
+ if(!raw){
+ return DeserializeResult::err(raw.error);
+ }
+
+ if (raw.value & 0x0000008000000000ULL)
+ {
+ raw.value |= 0xFFFFFF0000000000ULL;
+ }
+
+ return DeserializeResult::ok(Int40{static_cast(raw.value)});
+ }
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 6, "Int48")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ auto raw = sdo::helpers::to_raw(blob, 6);
+ if(!raw){
+ return DeserializeResult::err(raw.error);
+ }
+
+ if (raw.value & 0x0000800000000000ULL)
+ {
+ raw.value |= 0xFFFF000000000000ULL;
+ }
+
+ return DeserializeResult::ok(Int48{static_cast(raw.value)});
+ }
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 7, "Int56")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ auto raw = sdo::helpers::to_raw(blob, 7);
+ if(!raw){
+ return DeserializeResult::err(raw.error);
+ }
+
+ if (raw.value & 0x0080000000000000ULL)
+ {
+ raw.value |= 0xFF00000000000000ULL;
+ }
+
+ return DeserializeResult::ok(Int56{static_cast(raw.value)});
+ }
+
+ template <> [[nodiscard]] inline DeserializeResult deserialize(
+ const std::vector& blob)
+ {
+ if (auto error = sdo::helpers::check_size(blob, 8, "int64_t")){
+ return sdo::DeserializeResult::err(*error);
+ }
+
+ auto raw = sdo::helpers::to_raw(blob, 8);
+ if(!raw){
+ return DeserializeResult::err(raw.error);
+ }
+
+ return DeserializeResult::ok(static_cast(raw.value));
+ }
+
+ // -Extended Unsigned Integer-
+
+ template <> [[nodiscard]] inline DeserializeResult