From a7cc61ca8c11afb45475723dc5950027e397548f Mon Sep 17 00:00:00 2001 From: Anton Date: Fri, 22 May 2026 17:48:39 +0200 Subject: [PATCH 01/53] SDO: response->value_type = request->value_type --- rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp index 89bad70..c979098 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -131,7 +131,7 @@ void EthercatNode::sdoReadServiceCallback( response->index = request->index; response->subindex = request->subindex; response->value = value; - response->value_type = 0; + response->value_type = request->value_type; } void EthercatNode::sdoWriteServiceCallback( const std::shared_ptr @@ -156,5 +156,5 @@ void EthercatNode::sdoWriteServiceCallback( response->device_id = request->device_id; response->index = request->index; response->subindex = request->subindex; - response->value_type = 0; + response->value_type = request->value_type; } From 4fe2bbfe11de9e198757accdab647d3dbd3228b8 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sat, 23 May 2026 16:45:16 +0000 Subject: [PATCH 02/53] add c++ library for deserializing uint8 arrays --- .../include/rise_motion/sdo_serializer.hpp | 418 ++++++++++++++++++ 1 file changed, 418 insertions(+) create mode 100644 rise_motion_dev_ws/src/rise_motion/include/rise_motion/sdo_serializer.hpp 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..02d90e2 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/sdo_serializer.hpp @@ -0,0 +1,418 @@ +// library assumes little-endian + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + + +namespace sdo::helpers +{ + template inline constexpr bool always_false = false; + + inline void check_size(const std::vector& blob, std::size_t expectedSize, const char* type) + { + if (blob.size() != expectedSize){ + throw std::invalid_argument(std::string("deserialize<") + type + + ">: expected " + std::to_string(expectedSize) + " bytes, got " + std::to_string(blob.size())); + } + } + + inline std::uint16_t to_16bit_raw(const std::vector& blob, std::size_t offset = 0) + { + std::uint16_t raw = + static_cast(blob[offset + 0]) | + static_cast(blob[offset + 1]) << 8; + + return raw; + } + + inline std::uint32_t to_24bit_raw(const std::vector& blob, std::size_t offset = 0) + { + std::uint32_t raw = + static_cast(blob[offset + 0]) | + static_cast(blob[offset + 1]) << 8 | + static_cast(blob[offset + 2]) << 16; + + return raw; + } + + inline std::uint32_t to_32bit_raw(const std::vector& blob, std::size_t offset = 0) + { + std::uint32_t raw = + static_cast(blob[offset + 0]) | + static_cast(blob[offset + 1]) << 8 | + static_cast(blob[offset + 2]) << 16 | + static_cast(blob[offset + 3]) << 24; + + return raw; + } + + inline std::uint64_t to_40bit_raw(const std::vector& blob, std::size_t offset = 0) + { + std::uint64_t raw = + static_cast(blob[offset + 0]) | + static_cast(blob[offset + 1]) << 8 | + static_cast(blob[offset + 2]) << 16 | + static_cast(blob[offset + 3]) << 24 | + static_cast(blob[offset + 4]) << 32; + + return raw; + } + + inline std::uint64_t to_48bit_raw(const std::vector& blob, std::size_t offset = 0) + { + std::uint64_t raw = + static_cast(blob[offset + 0]) | + static_cast(blob[offset + 1]) << 8 | + static_cast(blob[offset + 2]) << 16 | + static_cast(blob[offset + 3]) << 24 | + static_cast(blob[offset + 4]) << 32 | + static_cast(blob[offset + 5]) << 40; + + return raw; + } + + inline std::uint64_t to_56bit_raw(const std::vector& blob, std::size_t offset = 0) + { + std::uint64_t raw = + static_cast(blob[offset + 0]) | + static_cast(blob[offset + 1]) << 8 | + static_cast(blob[offset + 2]) << 16 | + static_cast(blob[offset + 3]) << 24 | + static_cast(blob[offset + 4]) << 32 | + static_cast(blob[offset + 5]) << 40 | + static_cast(blob[offset + 6]) << 48; + + return raw; + } + + inline std::uint64_t to_64bit_raw(const std::vector& blob, std::size_t offset = 0) + { + std::uint64_t raw = + static_cast(blob[offset + 0]) | + static_cast(blob[offset + 1]) << 8 | + static_cast(blob[offset + 2]) << 16 | + static_cast(blob[offset + 3]) << 24 | + static_cast(blob[offset + 4]) << 32 | + static_cast(blob[offset + 5]) << 40 | + static_cast(blob[offset + 6]) << 48 | + static_cast(blob[offset + 7]) << 56; + + return raw; + } +} + +namespace sdo +{ + // 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; + }; + + // as equivalent of the EtherCAT TIME_DIFFERENCE data type + struct TimeDifference + { + std::uint32_t ms; + std::uint16_t d; + }; + + struct Int24 + { + std::int32_t value; + }; + + struct Int40 + { + std::int64_t value; + }; + + struct Int48 + { + std::int64_t value; + }; + + struct Int56 + { + std::int64_t value; + }; + + struct UInt24 + { + std::uint32_t value; + }; + + struct UInt40 + { + std::uint64_t value; + }; + + struct UInt48 + { + std::uint64_t value; + }; + + struct UInt56 + { + std::uint64_t value; + }; + + struct Guid + { + std::array bytes; + }; + + + template T deserialize(const std::vector&) + { + static_assert(sdo::helpers::always_false, "deserialize: unsupported type"); + } + + // Boolean + + template <> inline bool deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 1, "bool"); + + return blob[0] != 0; + } + + // Signed Integer + + template <> inline std::int8_t deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 1, "int8_t"); + + return static_cast(blob[0]); + } + + template <> inline std::int16_t deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 2, "int16_t"); + + std::int16_t value = static_cast(sdo::helpers::to_16bit_raw(blob)); + + return value; + } + + template <> inline std::int32_t deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 4, "int32_t"); + + std::int32_t value = static_cast(sdo::helpers::to_32bit_raw(blob)); + + return value; + } + + // Unsigned Integer / raw data / bit arrays / bit strings + + // use for UNSIGNED8, BYTE, BITARR8, BIT1-BIT8 + template <> inline std::uint8_t deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 1, "uint8_t"); + + return static_cast(blob[0]); + } + + // use for UNSIGNED16, WORD, BITARR16, BIT9-BIT16 + template <> inline std::uint16_t deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 2, "uint16_t"); + + return sdo::helpers::to_16bit_raw(blob); + } + + // use for UNSIGNED32, DWORD, BITARR32 + template <> inline std::uint32_t deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 4, "uint32_t"); + + return sdo::helpers::to_32bit_raw(blob); + } + + // Floating Point + + template <> inline float deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 4, "float"); + + std::uint32_t raw = sdo::helpers::to_32bit_raw(blob); + + float value; + std::memcpy(&value, &raw, sizeof(value)); + + return value; + } + + template <> inline double deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 8, "double"); + + std::uint64_t raw = sdo::helpers::to_64bit_raw(blob); + + double value; + std::memcpy(&value, &raw, sizeof(value)); + + return value; + } + + // Time + + template <> inline TimeOfDay deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 6, "TimeOfDay"); + + TimeOfDay value{}; + + value.ms_since_midnight = sdo::helpers::to_32bit_raw(blob); + value.d_since_1984_01_01 = sdo::helpers::to_16bit_raw(blob, 4); + + return value; + } + + template <> inline TimeDifference deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 6, "TimeDifference"); + + TimeDifference value{}; + + // the upper 4 bits of ms are reserved + value.ms = sdo::helpers::to_32bit_raw(blob) & 0x0FFFFFFF; + + value.d = sdo::helpers::to_16bit_raw(blob, 4); + + return value; + } + + // Domain (returns raw blob as equivalent to the EtherCAT Domain data type) + + template <> inline std::vector deserialize>( + const std::vector& blob) + { + return blob; + } + + // Extended Signed Integer + + template <> inline Int24 deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 3, "Int24"); + + std::uint32_t raw = sdo::helpers::to_24bit_raw(blob); + + if (raw & 0x00800000){ + raw |= 0xFF000000; + } + + return Int24{static_cast(raw)}; + } + + template <> inline Int40 deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 5, "Int40"); + + std::uint64_t raw = sdo::helpers::to_40bit_raw(blob); + + if (raw & 0x0000008000000000ULL) + { + raw |= 0xFFFFFF0000000000ULL; + } + + return Int40{static_cast(raw)}; + } + + template <> inline Int48 deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 6, "Int48"); + + std::uint64_t raw = sdo::helpers::to_48bit_raw(blob); + + if (raw & 0x0000800000000000ULL) + { + raw |= 0xFFFF000000000000ULL; + } + + return Int48{static_cast(raw)}; + } + + template <> inline Int56 deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 7, "Int56"); + + std::uint64_t raw = sdo::helpers::to_56bit_raw(blob); + + if (raw & 0x0080000000000000ULL) + { + raw |= 0xFF00000000000000ULL; + } + + return Int56{static_cast(raw)}; + } + + template <> inline std::int64_t deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 8, "int64_t"); + + return static_cast(sdo::helpers::to_64bit_raw(blob)); + } + + // Extended Unsigned Integer + + template <> inline UInt24 deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 3, "UInt24"); + + return UInt24{sdo::helpers::to_24bit_raw(blob)}; + } + + template <> inline UInt40 deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 5, "UInt40"); + + return UInt40{sdo::helpers::to_40bit_raw(blob)}; + } + + template <> inline UInt48 deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 6, "UInt48"); + + return UInt48{sdo::helpers::to_48bit_raw(blob)}; + } + + template <> inline UInt56 deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 7, "UInt56"); + + return UInt56{sdo::helpers::to_56bit_raw(blob)}; + } + + template <> inline std::uint64_t deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 8, "uint64_t"); + + return sdo::helpers::to_64bit_raw(blob); + } + + // GUID + + template <> inline Guid deserialize(const std::vector& blob) + { + sdo::helpers::check_size(blob, 16, "Guid"); + + Guid value{}; + + for (std::size_t i = 0; i < value.bytes.size(); ++i) + { + value.bytes[i] = blob[i]; + } + + return value; + } +} From 73586113fb1c663596fa8ac6979445680525487d Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sun, 24 May 2026 08:40:50 +0000 Subject: [PATCH 03/53] sdo_serializer.hpp: add serialization for bool and int8_t --- .../include/rise_motion/sdo_serializer.hpp | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) 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 index 02d90e2..c9ef0de 100644 --- 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 @@ -415,4 +415,26 @@ namespace sdo return value; } + + + + template std::vector serialize(const T& value) + { + static_assert(sdo::helpers::always_false, "serialize: unsupported type"); + return{}; + } + + // Boolean + + template <> inline std::vector serialize(const bool& value) + { + return {static_cast(value ? 1 : 0)}; + } + + // Signed Integer + + template <> inline std::vector serialize(const std::int8_t& value) + { + return {static_cast(value)}; + } } From 1325eb1352f3b2782a04af547b81367c4fb881c2 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sun, 24 May 2026 08:42:04 +0000 Subject: [PATCH 04/53] add tests for sdo_serializer --- .../src/rise_motion/CMakeLists.txt | 14 ++++++++ .../rise_motion/test/test_sdo_serializer.cpp | 32 +++++++++++++++++++ 2 files changed, 46 insertions(+) create mode 100644 rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index ce8a2e3..3f49c4f 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -14,6 +14,10 @@ include_directories(include) add_subdirectory(SOEM) +# --- sdo_serializer library --- +add_library(sdo_serializer_lib INTERFACE) +target_include_directories(sdo_serializer_lib INTERFACE include) + add_executable( rise_motion_main src/main.cpp @@ -52,6 +56,7 @@ install(TARGETS ) 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) @@ -60,6 +65,15 @@ if(BUILD_TESTING) # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + # --- sdo_serializer test --- + ament_add_gtest(test_sdo_serializer + test/test_sdo_serializer.cpp + ) + if(TARGET test_sdo_serializer) + target_link_libraries(test_sdo_serializer sdo_serializer_lib) + endif() + endif() ament_package() diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp new file mode 100644 index 0000000..c0b153c --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp @@ -0,0 +1,32 @@ +#include + +#include "rise_motion/sdo_serializer.hpp" + +TEST(SDOSerializationTest, Bool) +{ + bool value = true; + + std::vector blob = sdo::serialize(value); + auto result = sdo::deserialize(blob); + + ASSERT_EQ(result, value); +} + +TEST(SDOSerializationTest, Int8) +{ + std::int8_t value_zero = 0; + std::int8_t value_max = 127; + std::int8_t value_min = -128; + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero, value_zero); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max, value_max); + + blob = sdo::serialize(value_min); + auto result_min = sdo::deserialize(blob); + ASSERT_EQ(result_min, value_min); +} \ No newline at end of file From 6d98330a2388efb4b13ecf769cadbde6b664f392 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sun, 24 May 2026 11:30:06 +0000 Subject: [PATCH 05/53] sdo_serializer.hpp: add serialization for int, uint, float, time and domain --- .../include/rise_motion/sdo_serializer.hpp | 123 ++++++++++++++++++ 1 file changed, 123 insertions(+) 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 index c9ef0de..5f089f7 100644 --- 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 @@ -106,6 +106,39 @@ namespace sdo::helpers return raw; } + + + inline std::vector from_16bit_raw(const std::uint16_t& raw) + { + return { + static_cast(raw & 0xFF), + static_cast((raw >> 8) & 0xFF) + }; + } + + inline std::vector from_32bit_raw(const std::uint32_t& raw) + { + return { + static_cast(raw & 0xFF), + static_cast((raw >> 8) & 0xFF), + static_cast((raw >> 16) & 0xFF), + static_cast((raw >> 24) & 0xFF) + }; + } + + inline std::vector from_64bit_raw(const std::uint64_t& raw) + { + return { + static_cast(raw & 0xFF), + static_cast((raw >> 8) & 0xFF), + static_cast((raw >> 16) & 0xFF), + static_cast((raw >> 24) & 0xFF), + static_cast((raw >> 32) & 0xFF), + static_cast((raw >> 40) & 0xFF), + static_cast((raw >> 48) & 0xFF), + static_cast((raw >> 56) & 0xFF) + }; + } } namespace sdo @@ -437,4 +470,94 @@ namespace sdo { return {static_cast(value)}; } + + template <> inline std::vector serialize(const std::int16_t& value) + { + return sdo::helpers::from_16bit_raw(static_cast(value)); + } + + template <> inline std::vector serialize(const std::int32_t& value) + { + return sdo::helpers::from_32bit_raw(static_cast(value)); + } + + // Unsigned Integer / raw data / bit arrays / bit strings + + // use for UNSIGNED8, BYTE, BITARR8, BIT1-BIT8 + template <> inline std::vector serialize(const std::uint8_t& value) + { + return { value }; + } + + // use for UNSIGNED16, WORD, BITARR16, BIT9-BIT16 + template <> inline std::vector serialize(const std::uint16_t& value) + { + return sdo::helpers::from_16bit_raw(value); + } + + // use for UNSIGNED32, DWORD, BITARR32 + + template <> inline std::vector serialize(const std::uint32_t& value) + { + return sdo::helpers::from_32bit_raw(value); + } + + // Floating Point + + template <> inline std::vector serialize(const float& value) + { + std::uint32_t raw; + std::memcpy(&raw, &value, sizeof(raw)); + + return sdo::helpers::from_32bit_raw(raw); + } + + template <> inline std::vector serialize(const double& value) + { + std::uint64_t raw; + std::memcpy(&raw, &value, sizeof(raw)); + + return sdo::helpers::from_64bit_raw(raw); + } + + // Time + + template <> inline std::vector serialize(const TimeOfDay& value) + { + return { + static_cast(value.ms_since_midnight & 0xFF), + static_cast((value.ms_since_midnight >> 8) & 0xFF), + static_cast((value.ms_since_midnight >> 16) & 0xFF), + static_cast((value.ms_since_midnight >> 24) & 0xFF), + + static_cast(value.d_since_1984_01_01 & 0xFF), + static_cast((value.d_since_1984_01_01 >> 8) & 0xFF) + }; + } + + template <> inline std::vector serialize(const TimeDifference& value) + { + if (value.ms > 0x0FFFFFFF) + { + throw std::out_of_range("serialize: TimeDifference.ms exceeds 28 bit range"); + } + + return { + static_cast(value.ms & 0xFF), + static_cast((value.ms >> 8) & 0xFF), + static_cast((value.ms >> 16) & 0xFF), + static_cast((value.ms >> 24) & 0xFF), + + static_cast(value.d & 0xFF), + static_cast((value.d >> 8) & 0xFF) + }; + } + + // Domain (returns raw blob as equivalent to the EtherCAT Domain data type) + + template <> inline std::vector serialize>( + const std::vector& value) + { + return value; + } } From 32319e82d29ff916e5f8340a30edc2777cb32ec9 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sun, 24 May 2026 12:46:14 +0000 Subject: [PATCH 06/53] sdo_serializer.hpp: merge helper functions into generic function templates --- .../include/rise_motion/sdo_serializer.hpp | 183 ++++++------------ 1 file changed, 56 insertions(+), 127 deletions(-) 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 index 5f089f7..ed8822a 100644 --- 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 @@ -23,121 +23,50 @@ namespace sdo::helpers } } - inline std::uint16_t to_16bit_raw(const std::vector& blob, std::size_t offset = 0) + template inline T to_raw( + const std::vector& blob, std::size_t numBytes, std::size_t offset = 0) { - std::uint16_t raw = - static_cast(blob[offset + 0]) | - static_cast(blob[offset + 1]) << 8; - - return raw; - } + 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"); - inline std::uint32_t to_24bit_raw(const std::vector& blob, std::size_t offset = 0) - { - std::uint32_t raw = - static_cast(blob[offset + 0]) | - static_cast(blob[offset + 1]) << 8 | - static_cast(blob[offset + 2]) << 16; - - return raw; - } + if (numBytes > sizeof(T)) + { + throw std::invalid_argument("to_raw: numBytes does not fit T"); + } - inline std::uint32_t to_32bit_raw(const std::vector& blob, std::size_t offset = 0) - { - std::uint32_t raw = - static_cast(blob[offset + 0]) | - static_cast(blob[offset + 1]) << 8 | - static_cast(blob[offset + 2]) << 16 | - static_cast(blob[offset + 3]) << 24; - - return raw; - } + if (offset + numBytes > blob.size()) + { + throw std::out_of_range("to_raw: blob has less bytes than numBytes"); + } - inline std::uint64_t to_40bit_raw(const std::vector& blob, std::size_t offset = 0) - { - std::uint64_t raw = - static_cast(blob[offset + 0]) | - static_cast(blob[offset + 1]) << 8 | - static_cast(blob[offset + 2]) << 16 | - static_cast(blob[offset + 3]) << 24 | - static_cast(blob[offset + 4]) << 32; - - return raw; - } + T raw = 0; - inline std::uint64_t to_48bit_raw(const std::vector& blob, std::size_t offset = 0) - { - std::uint64_t raw = - static_cast(blob[offset + 0]) | - static_cast(blob[offset + 1]) << 8 | - static_cast(blob[offset + 2]) << 16 | - static_cast(blob[offset + 3]) << 24 | - static_cast(blob[offset + 4]) << 32 | - static_cast(blob[offset + 5]) << 40; - - return raw; - } + for (std::size_t i = 0; i < numBytes; ++i) + { + raw |= static_cast(blob[offset + i]) << (8 * i); + } - inline std::uint64_t to_56bit_raw(const std::vector& blob, std::size_t offset = 0) - { - std::uint64_t raw = - static_cast(blob[offset + 0]) | - static_cast(blob[offset + 1]) << 8 | - static_cast(blob[offset + 2]) << 16 | - static_cast(blob[offset + 3]) << 24 | - static_cast(blob[offset + 4]) << 32 | - static_cast(blob[offset + 5]) << 40 | - static_cast(blob[offset + 6]) << 48; - return raw; } - inline std::uint64_t to_64bit_raw(const std::vector& blob, std::size_t offset = 0) + template inline std::vector from_raw(const T& raw, std::size_t numBytes) { - std::uint64_t raw = - static_cast(blob[offset + 0]) | - static_cast(blob[offset + 1]) << 8 | - static_cast(blob[offset + 2]) << 16 | - static_cast(blob[offset + 3]) << 24 | - static_cast(blob[offset + 4]) << 32 | - static_cast(blob[offset + 5]) << 40 | - static_cast(blob[offset + 6]) << 48 | - static_cast(blob[offset + 7]) << 56; - - return raw; - } + static_assert(std::is_unsigned_v, "from_raw: T must be unsigned"); + if (numBytes > sizeof(T)) + { + throw std::invalid_argument("from_raw: numBytes does not fit T"); + } - inline std::vector from_16bit_raw(const std::uint16_t& raw) - { - return { - static_cast(raw & 0xFF), - static_cast((raw >> 8) & 0xFF) - }; - } + std::vector blob; + blob.reserve(numBytes); - inline std::vector from_32bit_raw(const std::uint32_t& raw) - { - return { - static_cast(raw & 0xFF), - static_cast((raw >> 8) & 0xFF), - static_cast((raw >> 16) & 0xFF), - static_cast((raw >> 24) & 0xFF) - }; - } + for (std::size_t i = 0; i < numBytes; ++i) + { + blob.push_back(static_cast((raw >> (8 * i)) & 0xFF)); + } - inline std::vector from_64bit_raw(const std::uint64_t& raw) - { - return { - static_cast(raw & 0xFF), - static_cast((raw >> 8) & 0xFF), - static_cast((raw >> 16) & 0xFF), - static_cast((raw >> 24) & 0xFF), - static_cast((raw >> 32) & 0xFF), - static_cast((raw >> 40) & 0xFF), - static_cast((raw >> 48) & 0xFF), - static_cast((raw >> 56) & 0xFF) - }; + return blob; } } @@ -230,7 +159,7 @@ namespace sdo { sdo::helpers::check_size(blob, 2, "int16_t"); - std::int16_t value = static_cast(sdo::helpers::to_16bit_raw(blob)); + std::int16_t value = static_cast(sdo::helpers::to_raw(blob, 2)); return value; } @@ -239,7 +168,7 @@ namespace sdo { sdo::helpers::check_size(blob, 4, "int32_t"); - std::int32_t value = static_cast(sdo::helpers::to_32bit_raw(blob)); + std::int32_t value = static_cast(sdo::helpers::to_raw(blob, 4)); return value; } @@ -259,7 +188,7 @@ namespace sdo { sdo::helpers::check_size(blob, 2, "uint16_t"); - return sdo::helpers::to_16bit_raw(blob); + return sdo::helpers::to_raw(blob, 2); } // use for UNSIGNED32, DWORD, BITARR32 @@ -267,7 +196,7 @@ namespace sdo { sdo::helpers::check_size(blob, 4, "uint32_t"); - return sdo::helpers::to_32bit_raw(blob); + return sdo::helpers::to_raw(blob, 4); } // Floating Point @@ -276,7 +205,7 @@ namespace sdo { sdo::helpers::check_size(blob, 4, "float"); - std::uint32_t raw = sdo::helpers::to_32bit_raw(blob); + std::uint32_t raw = sdo::helpers::to_raw(blob, 4); float value; std::memcpy(&value, &raw, sizeof(value)); @@ -288,7 +217,7 @@ namespace sdo { sdo::helpers::check_size(blob, 8, "double"); - std::uint64_t raw = sdo::helpers::to_64bit_raw(blob); + std::uint64_t raw = sdo::helpers::to_raw(blob, 8); double value; std::memcpy(&value, &raw, sizeof(value)); @@ -304,8 +233,8 @@ namespace sdo TimeOfDay value{}; - value.ms_since_midnight = sdo::helpers::to_32bit_raw(blob); - value.d_since_1984_01_01 = sdo::helpers::to_16bit_raw(blob, 4); + value.ms_since_midnight = sdo::helpers::to_raw(blob, 4); + value.d_since_1984_01_01 = sdo::helpers::to_raw(blob, 2, 4); return value; } @@ -317,9 +246,9 @@ namespace sdo TimeDifference value{}; // the upper 4 bits of ms are reserved - value.ms = sdo::helpers::to_32bit_raw(blob) & 0x0FFFFFFF; + value.ms = sdo::helpers::to_raw(blob, 4) & 0x0FFFFFFF; - value.d = sdo::helpers::to_16bit_raw(blob, 4); + value.d = sdo::helpers::to_raw(blob, 2, 4); return value; } @@ -338,7 +267,7 @@ namespace sdo { sdo::helpers::check_size(blob, 3, "Int24"); - std::uint32_t raw = sdo::helpers::to_24bit_raw(blob); + std::uint32_t raw = sdo::helpers::to_raw(blob, 3); if (raw & 0x00800000){ raw |= 0xFF000000; @@ -351,7 +280,7 @@ namespace sdo { sdo::helpers::check_size(blob, 5, "Int40"); - std::uint64_t raw = sdo::helpers::to_40bit_raw(blob); + std::uint64_t raw = sdo::helpers::to_raw(blob, 5); if (raw & 0x0000008000000000ULL) { @@ -365,7 +294,7 @@ namespace sdo { sdo::helpers::check_size(blob, 6, "Int48"); - std::uint64_t raw = sdo::helpers::to_48bit_raw(blob); + std::uint64_t raw = sdo::helpers::to_raw(blob, 6); if (raw & 0x0000800000000000ULL) { @@ -379,7 +308,7 @@ namespace sdo { sdo::helpers::check_size(blob, 7, "Int56"); - std::uint64_t raw = sdo::helpers::to_56bit_raw(blob); + std::uint64_t raw = sdo::helpers::to_raw(blob, 7); if (raw & 0x0080000000000000ULL) { @@ -393,7 +322,7 @@ namespace sdo { sdo::helpers::check_size(blob, 8, "int64_t"); - return static_cast(sdo::helpers::to_64bit_raw(blob)); + return static_cast(sdo::helpers::to_raw(blob, 8)); } // Extended Unsigned Integer @@ -402,35 +331,35 @@ namespace sdo { sdo::helpers::check_size(blob, 3, "UInt24"); - return UInt24{sdo::helpers::to_24bit_raw(blob)}; + return UInt24{sdo::helpers::to_raw(blob, 3)}; } template <> inline UInt40 deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 5, "UInt40"); - return UInt40{sdo::helpers::to_40bit_raw(blob)}; + return UInt40{sdo::helpers::to_raw(blob, 5)}; } template <> inline UInt48 deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 6, "UInt48"); - return UInt48{sdo::helpers::to_48bit_raw(blob)}; + return UInt48{sdo::helpers::to_raw(blob, 6)}; } template <> inline UInt56 deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 7, "UInt56"); - return UInt56{sdo::helpers::to_56bit_raw(blob)}; + return UInt56{sdo::helpers::to_raw(blob, 7)}; } template <> inline std::uint64_t deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 8, "uint64_t"); - return sdo::helpers::to_64bit_raw(blob); + return sdo::helpers::to_raw(blob, 8); } // GUID @@ -473,12 +402,12 @@ namespace sdo template <> inline std::vector serialize(const std::int16_t& value) { - return sdo::helpers::from_16bit_raw(static_cast(value)); + return sdo::helpers::from_raw(static_cast(value), 2); } template <> inline std::vector serialize(const std::int32_t& value) { - return sdo::helpers::from_32bit_raw(static_cast(value)); + return sdo::helpers::from_raw(static_cast(value), 4); } // Unsigned Integer / raw data / bit arrays / bit strings @@ -492,14 +421,14 @@ namespace sdo // use for UNSIGNED16, WORD, BITARR16, BIT9-BIT16 template <> inline std::vector serialize(const std::uint16_t& value) { - return sdo::helpers::from_16bit_raw(value); + return sdo::helpers::from_raw(value, 2); } // use for UNSIGNED32, DWORD, BITARR32 template <> inline std::vector serialize(const std::uint32_t& value) { - return sdo::helpers::from_32bit_raw(value); + return sdo::helpers::from_raw(value, 4); } // Floating Point @@ -509,7 +438,7 @@ namespace sdo std::uint32_t raw; std::memcpy(&raw, &value, sizeof(raw)); - return sdo::helpers::from_32bit_raw(raw); + return sdo::helpers::from_raw(raw, 4); } template <> inline std::vector serialize(const double& value) @@ -517,7 +446,7 @@ namespace sdo std::uint64_t raw; std::memcpy(&raw, &value, sizeof(raw)); - return sdo::helpers::from_64bit_raw(raw); + return sdo::helpers::from_raw(raw, 8); } // Time From a274c2ca5317e9a27aeb48d824e5411ef25a1c8b Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sun, 24 May 2026 13:32:32 +0000 Subject: [PATCH 07/53] sdo_serializer.hpp: add serialization for extended int, extended uint and guid --- .../include/rise_motion/sdo_serializer.hpp | 112 +++++++++++++++++- 1 file changed, 111 insertions(+), 1 deletion(-) 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 index ed8822a..e09ba9e 100644 --- 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 @@ -9,6 +9,7 @@ #include #include #include +#include namespace sdo::helpers @@ -267,7 +268,7 @@ namespace sdo { sdo::helpers::check_size(blob, 3, "Int24"); - std::uint32_t raw = sdo::helpers::to_raw(blob, 3); + std::uint32_t raw = sdo::helpers::to_raw(blob, 3); if (raw & 0x00800000){ raw |= 0xFF000000; @@ -489,4 +490,113 @@ namespace sdo { return value; } + + // Extended Signed Integer + + template <> inline std::vector serialize(const Int24& value) + { + if (value.value < -pow(2, 23) || value.value > pow(2, 23)-1) + { + throw std::out_of_range("Int24 value exceeds 24 bit signed range"); + } + + const auto raw = static_cast(value.value); + + return sdo::helpers::from_raw(raw, 3); + } + + template <> inline std::vector serialize(const Int40& value) + { + if (value.value < -pow(2, 39) || value.value > pow(2, 39)-1) + { + throw std::out_of_range("Int40 value exceeds 40 bit signed range"); + } + + const auto raw = static_cast(value.value); + + return sdo::helpers::from_raw(raw, 5); + } + + template <> inline std::vector serialize(const Int48& value) + { + if (value.value < -pow(2, 47) || value.value > pow(2, 47)-1) + { + throw std::out_of_range("Int48 value exceeds 48 bit signed range"); + } + + const auto raw = static_cast(value.value); + + return sdo::helpers::from_raw(raw, 6); + } + + template <> inline std::vector serialize(const Int56& value) + { + if (value.value < -pow(2, 55) || value.value > pow(2, 55)-1) + { + throw std::out_of_range("Int56 value exceeds 56 bit signed range"); + } + + const auto raw = static_cast(value.value); + + return sdo::helpers::from_raw(raw, 7); + } + + template <> inline std::vector serialize(const int64_t& value) + { + return sdo::helpers::from_raw(static_cast(value), 8); + } + + // Extended unsigned Integer + + template <> inline std::vector serialize(const UInt24& value) + { + if (value.value > pow(2, 24)-1) + { + throw std::out_of_range("UInt24 value exceeds 24 bit unsigned range"); + } + + return sdo::helpers::from_raw(value.value, 3); + } + + template <> inline std::vector serialize(const UInt40& value) + { + if (value.value > pow(2, 40)-1) + { + throw std::out_of_range("UInt40 value exceeds 40 bit unsigned range"); + } + + return sdo::helpers::from_raw(value.value, 5); + } + + template <> inline std::vector serialize(const UInt48& value) + { + if (value.value > pow(2, 48)-1) + { + throw std::out_of_range("UInt48 value exceeds 48 bit unsigned range"); + } + + return sdo::helpers::from_raw(value.value, 6); + } + + template <> inline std::vector serialize(const UInt56& value) + { + if (value.value > pow(2, 56)-1) + { + throw std::out_of_range("UInt56 value exceeds 56 bit unsigned range"); + } + + return sdo::helpers::from_raw(value.value, 7); + } + + template <> inline std::vector serialize(const uint64_t& value) + { + return sdo::helpers::from_raw(value, 8); + } + + // GUID + + template <> inline std::vector serialize(const Guid& value) + { + return std::vector(value.bytes.begin(), value.bytes.end()); + } } From 69e3305b33c675a022a21b96d09ab5bd7a07a1fd Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sun, 24 May 2026 14:41:34 +0000 Subject: [PATCH 08/53] test_sdo_serializer.cpp: add tests for int, uint, float, time, domain and guid --- .../rise_motion/test/test_sdo_serializer.cpp | 332 +++++++++++++++++- 1 file changed, 329 insertions(+), 3 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp index c0b153c..e24d1a5 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp @@ -15,8 +15,8 @@ TEST(SDOSerializationTest, Bool) TEST(SDOSerializationTest, Int8) { std::int8_t value_zero = 0; - std::int8_t value_max = 127; - std::int8_t value_min = -128; + std::int8_t value_max = std::numeric_limits::max(); + std::int8_t value_min = std::numeric_limits::min(); std::vector blob = sdo::serialize(value_zero); auto result_zero = sdo::deserialize(blob); @@ -29,4 +29,330 @@ TEST(SDOSerializationTest, Int8) blob = sdo::serialize(value_min); auto result_min = sdo::deserialize(blob); ASSERT_EQ(result_min, value_min); -} \ No newline at end of file +} + +TEST(SDOSerializationTest, Int16) +{ + std::int16_t value_zero = 0; + std::int16_t value_max = std::numeric_limits::max(); + std::int16_t value_min = std::numeric_limits::min(); + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero, value_zero); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max, value_max); + + blob = sdo::serialize(value_min); + auto result_min = sdo::deserialize(blob); + ASSERT_EQ(result_min, value_min); +} + +TEST(SDOSerializationTest, Int32) +{ + std::int32_t value_zero = 0; + std::int32_t value_max = std::numeric_limits::max(); + std::int32_t value_min = std::numeric_limits::min(); + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero, value_zero); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max, value_max); + + blob = sdo::serialize(value_min); + auto result_min = sdo::deserialize(blob); + ASSERT_EQ(result_min, value_min); +} + +TEST(SDOSerializationTest, uInt8) +{ + std::uint8_t value_zero = 0; + std::uint8_t value_max = std::numeric_limits::max(); + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero, value_zero); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max, value_max); +} + +TEST(SDOSerializationTest, uInt16) +{ + std::uint16_t value_zero = 0; + std::uint16_t value_max = std::numeric_limits::max(); + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero, value_zero); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max, value_max); +} + +TEST(SDOSerializationTest, uInt32) +{ + std::uint32_t value_zero = 0; + std::uint32_t value_max = std::numeric_limits::max(); + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero, value_zero); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max, value_max); +} + +TEST(SDOSerializationTest, Float) +{ + float value_zero = 0.0f; + float value_pos = 123.456f; + float value_neg = -123.456f; + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero, value_zero); + + blob = sdo::serialize(value_pos); + auto result_pos = sdo::deserialize(blob); + ASSERT_EQ(result_pos, value_pos); + + blob = sdo::serialize(value_neg); + auto result_neg = sdo::deserialize(blob); + ASSERT_EQ(result_neg, value_neg); +} + +TEST(SDOSerializationTest, Double) +{ + double value_zero = 0.0; + double value_pos = 123.456; + double value_neg = -123.456; + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero, value_zero); + + blob = sdo::serialize(value_pos); + auto result_pos = sdo::deserialize(blob); + ASSERT_EQ(result_pos, value_pos); + + blob = sdo::serialize(value_neg); + auto result_neg = sdo::deserialize(blob); + ASSERT_EQ(result_neg, value_neg); +} + +TEST(SDOSerializationTest, TimeOfDay) +{ + sdo::TimeOfDay value{12345678, 42}; + + std::vector blob = sdo::serialize(value); + auto result = sdo::deserialize(blob); + ASSERT_EQ(result.ms_since_midnight, value.ms_since_midnight); + ASSERT_EQ(result.d_since_1984_01_01, value.d_since_1984_01_01); +} + +TEST(SDOSerializationTest, TimeDifference) +{ + sdo::TimeDifference value{12345678, 42}; + + std::vector blob = sdo::serialize(value); + auto result = sdo::deserialize(blob); + ASSERT_EQ(result.ms, value.ms); + ASSERT_EQ(result.d, value.d); +} + +TEST(SDOSerializationTest, Domain) +{ + std::vector value = {0, 1, 2, 3}; + + std::vector blob = sdo::serialize>(value); + auto result = sdo::deserialize>(blob); + ASSERT_EQ(result, value); +} + +TEST(SDOSerializationTest, Int24) +{ + sdo::Int24 value_zero{0}; + sdo::Int24 value_max{(std::int64_t{1} << 23) - 1}; + sdo::Int24 value_min{-(std::int64_t{1} << 23)}; + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero.value, value_zero.value); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max.value, value_max.value); + + blob = sdo::serialize(value_min); + auto result_min = sdo::deserialize(blob); + ASSERT_EQ(result_min.value, value_min.value); +} + +TEST(SDOSerializationTest, Int40) +{ + sdo::Int40 value_zero{0}; + sdo::Int40 value_max{(std::int64_t{1} << 39) - 1}; + sdo::Int40 value_min{-(std::int64_t{1} << 39)}; + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero.value, value_zero.value); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max.value, value_max.value); + + blob = sdo::serialize(value_min); + auto result_min = sdo::deserialize(blob); + ASSERT_EQ(result_min.value, value_min.value); +} + +TEST(SDOSerializationTest, Int48) +{ + sdo::Int48 value_zero{0}; + sdo::Int48 value_max{(std::int64_t{1} << 47) - 1}; + sdo::Int48 value_min{-(std::int64_t{1} << 47)}; + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero.value, value_zero.value); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max.value, value_max.value); + + blob = sdo::serialize(value_min); + auto result_min = sdo::deserialize(blob); + ASSERT_EQ(result_min.value, value_min.value); +} + +TEST(SDOSerializationTest, Int56) +{ + sdo::Int56 value_zero{0}; + sdo::Int56 value_max{(std::int64_t{1} << 55) - 1}; + sdo::Int56 value_min{-(std::int64_t{1} << 55)}; + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero.value, value_zero.value); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max.value, value_max.value); + + blob = sdo::serialize(value_min); + auto result_min = sdo::deserialize(blob); + ASSERT_EQ(result_min.value, value_min.value); +} + +TEST(SDOSerializationTest, Int64) +{ + std::int64_t value_zero = 0; + std::int64_t value_max = std::numeric_limits::max(); + std::int64_t value_min = std::numeric_limits::min(); + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero, value_zero); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max, value_max); + + blob = sdo::serialize(value_min); + auto result_min = sdo::deserialize(blob); + ASSERT_EQ(result_min, value_min); +} + +TEST(SDOSerializationTest, uInt24) +{ + sdo::UInt24 value_zero{0}; + sdo::UInt24 value_max{(std::uint64_t{1} << 23) - 1}; + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero.value, value_zero.value); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max.value, value_max.value); +} + +TEST(SDOSerializationTest, uInt40) +{ + sdo::UInt40 value_zero{0}; + sdo::UInt40 value_max{(std::uint64_t{1} << 40) - 1}; + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero.value, value_zero.value); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max.value, value_max.value); +} + +TEST(SDOSerializationTest, uInt48) +{ + sdo::UInt48 value_zero{0}; + sdo::UInt48 value_max{(std::uint64_t{1} << 48) - 1}; + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero.value, value_zero.value); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max.value, value_max.value); +} + +TEST(SDOSerializationTest, uInt56) +{ + sdo::UInt56 value_zero{0}; + sdo::UInt56 value_max{(std::uint64_t{1} << 56) - 1}; + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero.value, value_zero.value); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max.value, value_max.value); +} + +TEST(SDOSerializationTest, uInt64) +{ + std::uint64_t value_zero = 0; + std::uint64_t value_max = std::numeric_limits::max(); + + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero, value_zero); + + blob = sdo::serialize(value_max); + auto result_max = sdo::deserialize(blob); + ASSERT_EQ(result_max, value_max); +} + +TEST(SDOSerializationTest, Guid) +{ + sdo::Guid value{{ + 1, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14, 15, 16 + }}; + + std::vector blob = sdo::serialize(value); + auto result = sdo::deserialize(blob); + ASSERT_EQ(result.bytes, value.bytes); +} From 33dbe3be0ff2da409103fe591ba915ccba4dd700 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sun, 24 May 2026 15:14:47 +0000 Subject: [PATCH 09/53] sdo_serializer.hpp: add [[nodiscard]] to pure converting functions --- .../include/rise_motion/sdo_serializer.hpp | 137 +++++++++--------- 1 file changed, 71 insertions(+), 66 deletions(-) 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 index e09ba9e..74ef296 100644 --- 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 @@ -24,7 +24,7 @@ namespace sdo::helpers } } - template inline T to_raw( + template [[nodiscard]] inline T 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"); @@ -50,7 +50,7 @@ namespace sdo::helpers return raw; } - template inline std::vector from_raw(const T& raw, std::size_t numBytes) + template [[nodiscard]] inline std::vector from_raw(const T& raw, std::size_t numBytes) { static_assert(std::is_unsigned_v, "from_raw: T must be unsigned"); @@ -133,30 +133,32 @@ namespace sdo }; + // ---DESERIALIZATION--- + template T deserialize(const std::vector&) { static_assert(sdo::helpers::always_false, "deserialize: unsupported type"); } - // Boolean + // -Boolean- - template <> inline bool deserialize(const std::vector& blob) + template <> [[nodiscard]] inline bool deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 1, "bool"); return blob[0] != 0; } - // Signed Integer + // -Signed Integer- - template <> inline std::int8_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline std::int8_t deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 1, "int8_t"); return static_cast(blob[0]); } - template <> inline std::int16_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline std::int16_t deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 2, "int16_t"); @@ -165,7 +167,7 @@ namespace sdo return value; } - template <> inline std::int32_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline std::int32_t deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 4, "int32_t"); @@ -174,10 +176,10 @@ namespace sdo return value; } - // Unsigned Integer / raw data / bit arrays / bit strings + // -Unsigned Integer / raw data / bit arrays / bit strings- // use for UNSIGNED8, BYTE, BITARR8, BIT1-BIT8 - template <> inline std::uint8_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline std::uint8_t deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 1, "uint8_t"); @@ -185,7 +187,7 @@ namespace sdo } // use for UNSIGNED16, WORD, BITARR16, BIT9-BIT16 - template <> inline std::uint16_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline std::uint16_t deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 2, "uint16_t"); @@ -193,16 +195,16 @@ namespace sdo } // use for UNSIGNED32, DWORD, BITARR32 - template <> inline std::uint32_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline std::uint32_t deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 4, "uint32_t"); return sdo::helpers::to_raw(blob, 4); } - // Floating Point + // -Floating Point- - template <> inline float deserialize(const std::vector& blob) + template <> [[nodiscard]] inline float deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 4, "float"); @@ -214,7 +216,7 @@ namespace sdo return value; } - template <> inline double deserialize(const std::vector& blob) + template <> [[nodiscard]] inline double deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 8, "double"); @@ -226,9 +228,9 @@ namespace sdo return value; } - // Time + // -Time- - template <> inline TimeOfDay deserialize(const std::vector& blob) + template <> [[nodiscard]] inline TimeOfDay deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 6, "TimeOfDay"); @@ -240,7 +242,7 @@ namespace sdo return value; } - template <> inline TimeDifference deserialize(const std::vector& blob) + template <> [[nodiscard]] inline TimeDifference deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 6, "TimeDifference"); @@ -254,17 +256,18 @@ namespace sdo return value; } - // Domain (returns raw blob as equivalent to the EtherCAT Domain data type) + // -Domain- + // (returns raw blob as equivalent to the EtherCAT Domain data type) - template <> inline std::vector deserialize>( + template <> [[nodiscard]] inline std::vector deserialize>( const std::vector& blob) { return blob; } - // Extended Signed Integer + // -Extended Signed Integer- - template <> inline Int24 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline Int24 deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 3, "Int24"); @@ -277,7 +280,7 @@ namespace sdo return Int24{static_cast(raw)}; } - template <> inline Int40 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline Int40 deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 5, "Int40"); @@ -291,7 +294,7 @@ namespace sdo return Int40{static_cast(raw)}; } - template <> inline Int48 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline Int48 deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 6, "Int48"); @@ -305,7 +308,7 @@ namespace sdo return Int48{static_cast(raw)}; } - template <> inline Int56 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline Int56 deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 7, "Int56"); @@ -319,53 +322,53 @@ namespace sdo return Int56{static_cast(raw)}; } - template <> inline std::int64_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline std::int64_t deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 8, "int64_t"); return static_cast(sdo::helpers::to_raw(blob, 8)); } - // Extended Unsigned Integer + // -Extended Unsigned Integer- - template <> inline UInt24 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline UInt24 deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 3, "UInt24"); return UInt24{sdo::helpers::to_raw(blob, 3)}; } - template <> inline UInt40 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline UInt40 deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 5, "UInt40"); return UInt40{sdo::helpers::to_raw(blob, 5)}; } - template <> inline UInt48 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline UInt48 deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 6, "UInt48"); return UInt48{sdo::helpers::to_raw(blob, 6)}; } - template <> inline UInt56 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline UInt56 deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 7, "UInt56"); return UInt56{sdo::helpers::to_raw(blob, 7)}; } - template <> inline std::uint64_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline std::uint64_t deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 8, "uint64_t"); return sdo::helpers::to_raw(blob, 8); } - // GUID + // -GUID- - template <> inline Guid deserialize(const std::vector& blob) + template <> [[nodiscard]] inline Guid deserialize(const std::vector& blob) { sdo::helpers::check_size(blob, 16, "Guid"); @@ -380,6 +383,7 @@ namespace sdo } + // ---SERIALIZATION--- template std::vector serialize(const T& value) { @@ -387,54 +391,54 @@ namespace sdo return{}; } - // Boolean + // -Boolean- - template <> inline std::vector serialize(const bool& value) + template <> [[nodiscard]] inline std::vector serialize(const bool& value) { return {static_cast(value ? 1 : 0)}; } - // Signed Integer + // -Signed Integer- - template <> inline std::vector serialize(const std::int8_t& value) + template <> [[nodiscard]] inline std::vector serialize(const std::int8_t& value) { return {static_cast(value)}; } - template <> inline std::vector serialize(const std::int16_t& value) + template <> [[nodiscard]] inline std::vector serialize(const std::int16_t& value) { return sdo::helpers::from_raw(static_cast(value), 2); } - template <> inline std::vector serialize(const std::int32_t& value) + template <> [[nodiscard]] inline std::vector serialize(const std::int32_t& value) { return sdo::helpers::from_raw(static_cast(value), 4); } - // Unsigned Integer / raw data / bit arrays / bit strings + // -Unsigned Integer / raw data / bit arrays / bit strings- // use for UNSIGNED8, BYTE, BITARR8, BIT1-BIT8 - template <> inline std::vector serialize(const std::uint8_t& value) + template <> [[nodiscard]] inline std::vector serialize(const std::uint8_t& value) { return { value }; } // use for UNSIGNED16, WORD, BITARR16, BIT9-BIT16 - template <> inline std::vector serialize(const std::uint16_t& value) + template <> [[nodiscard]] inline std::vector serialize(const std::uint16_t& value) { return sdo::helpers::from_raw(value, 2); } // use for UNSIGNED32, DWORD, BITARR32 - template <> inline std::vector serialize(const std::uint32_t& value) + template <> [[nodiscard]] inline std::vector serialize(const std::uint32_t& value) { return sdo::helpers::from_raw(value, 4); } - // Floating Point + // -Floating Point- - template <> inline std::vector serialize(const float& value) + template <> [[nodiscard]] inline std::vector serialize(const float& value) { std::uint32_t raw; std::memcpy(&raw, &value, sizeof(raw)); @@ -442,7 +446,7 @@ namespace sdo return sdo::helpers::from_raw(raw, 4); } - template <> inline std::vector serialize(const double& value) + template <> [[nodiscard]] inline std::vector serialize(const double& value) { std::uint64_t raw; std::memcpy(&raw, &value, sizeof(raw)); @@ -450,9 +454,9 @@ namespace sdo return sdo::helpers::from_raw(raw, 8); } - // Time + // -Time- - template <> inline std::vector serialize(const TimeOfDay& value) + template <> [[nodiscard]] inline std::vector serialize(const TimeOfDay& value) { return { static_cast(value.ms_since_midnight & 0xFF), @@ -465,7 +469,7 @@ namespace sdo }; } - template <> inline std::vector serialize(const TimeDifference& value) + template <> [[nodiscard]] inline std::vector serialize(const TimeDifference& value) { if (value.ms > 0x0FFFFFFF) { @@ -483,17 +487,18 @@ namespace sdo }; } - // Domain (returns raw blob as equivalent to the EtherCAT Domain data type) + // -Domain- + // (returns raw blob as equivalent to the EtherCAT Domain data type) - template <> inline std::vector serialize>( + template <> [[nodiscard]] inline std::vector serialize>( const std::vector& value) { return value; } - // Extended Signed Integer + // -Extended Signed Integer- - template <> inline std::vector serialize(const Int24& value) + template <> [[nodiscard]] inline std::vector serialize(const Int24& value) { if (value.value < -pow(2, 23) || value.value > pow(2, 23)-1) { @@ -505,7 +510,7 @@ namespace sdo return sdo::helpers::from_raw(raw, 3); } - template <> inline std::vector serialize(const Int40& value) + template <> [[nodiscard]] inline std::vector serialize(const Int40& value) { if (value.value < -pow(2, 39) || value.value > pow(2, 39)-1) { @@ -517,7 +522,7 @@ namespace sdo return sdo::helpers::from_raw(raw, 5); } - template <> inline std::vector serialize(const Int48& value) + template <> [[nodiscard]] inline std::vector serialize(const Int48& value) { if (value.value < -pow(2, 47) || value.value > pow(2, 47)-1) { @@ -529,7 +534,7 @@ namespace sdo return sdo::helpers::from_raw(raw, 6); } - template <> inline std::vector serialize(const Int56& value) + template <> [[nodiscard]] inline std::vector serialize(const Int56& value) { if (value.value < -pow(2, 55) || value.value > pow(2, 55)-1) { @@ -541,14 +546,14 @@ namespace sdo return sdo::helpers::from_raw(raw, 7); } - template <> inline std::vector serialize(const int64_t& value) + template <> [[nodiscard]] inline std::vector serialize(const int64_t& value) { return sdo::helpers::from_raw(static_cast(value), 8); } - // Extended unsigned Integer + // -Extended unsigned Integer- - template <> inline std::vector serialize(const UInt24& value) + template <> [[nodiscard]] inline std::vector serialize(const UInt24& value) { if (value.value > pow(2, 24)-1) { @@ -558,7 +563,7 @@ namespace sdo return sdo::helpers::from_raw(value.value, 3); } - template <> inline std::vector serialize(const UInt40& value) + template <> [[nodiscard]] inline std::vector serialize(const UInt40& value) { if (value.value > pow(2, 40)-1) { @@ -568,7 +573,7 @@ namespace sdo return sdo::helpers::from_raw(value.value, 5); } - template <> inline std::vector serialize(const UInt48& value) + template <> [[nodiscard]] inline std::vector serialize(const UInt48& value) { if (value.value > pow(2, 48)-1) { @@ -578,7 +583,7 @@ namespace sdo return sdo::helpers::from_raw(value.value, 6); } - template <> inline std::vector serialize(const UInt56& value) + template <> [[nodiscard]] inline std::vector serialize(const UInt56& value) { if (value.value > pow(2, 56)-1) { @@ -588,14 +593,14 @@ namespace sdo return sdo::helpers::from_raw(value.value, 7); } - template <> inline std::vector serialize(const uint64_t& value) + template <> [[nodiscard]] inline std::vector serialize(const uint64_t& value) { return sdo::helpers::from_raw(value, 8); } - // GUID + // -GUID- - template <> inline std::vector serialize(const Guid& value) + template <> [[nodiscard]] inline std::vector serialize(const Guid& value) { return std::vector(value.bytes.begin(), value.bytes.end()); } From 9a551e79cc5abd69291a45be734771664dc33a51 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Mon, 25 May 2026 20:28:01 +0000 Subject: [PATCH 10/53] add aliases for EtherCAT and driver data types and enable implicit conversion from std types to custom types --- .../include/rise_motion/sdo_serializer.hpp | 187 ++++++++++++++++-- 1 file changed, 169 insertions(+), 18 deletions(-) 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 index 74ef296..f299b43 100644 --- 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 @@ -19,8 +19,8 @@ namespace sdo::helpers inline void check_size(const std::vector& blob, std::size_t expectedSize, const char* type) { if (blob.size() != expectedSize){ - throw std::invalid_argument(std::string("deserialize<") + type + - ">: expected " + std::to_string(expectedSize) + " bytes, got " + std::to_string(blob.size())); + throw std::invalid_argument(std::string("deserialize<") + type + ">: expected " + + std::to_string(expectedSize) + " bytes, got " + std::to_string(blob.size())); } } @@ -90,49 +90,208 @@ namespace sdo struct Int24 { std::int32_t value; + + Int24(std::int32_t v) : value(v) + { + if (v < -pow(2, 23) || v > pow(2, 23)-1){ + throw std::out_of_range("Can not convert int32_t value exceeding 24 bit signed range into Int24"); + } + } + + operator std::int32_t() const + { + return value; + } }; struct Int40 { std::int64_t value; + + Int40(std::int64_t v) : value(v) + { + if (v < -pow(2, 39) || v > pow(2, 39)-1){ + throw std::out_of_range("Can not convert int64_t value exceeding 40 bit signed range into Int40"); + } + } + + operator std::int64_t() const + { + return value; + } }; struct Int48 { std::int64_t value; + + Int48(std::int64_t v) : value(v) + { + if (v < -pow(2, 47) || v > pow(2, 47)-1){ + throw std::out_of_range("Can not convert int64_t value exceeding 48 bit signed range into Int48"); + } + } + + operator std::int64_t() const + { + return value; + } }; struct Int56 { std::int64_t value; + + Int56(std::int64_t v) : value(v) + { + if (v < -pow(2, 55) || v > pow(2, 55)-1){ + throw std::out_of_range("Can not convert int64_t value exceeding 56 bit signed range into Int56"); + } + } + + operator std::int64_t() const + { + return value; + } }; struct UInt24 { std::uint32_t value; + + UInt24(std::uint32_t v) : value(v) + { + if (v > pow(2, 24)-1){ + throw std::out_of_range("Can not convert uint32_t value exceeding 24 bit unsigned range into UInt24"); + } + } + + operator std::uint32_t() const + { + return value; + } }; struct UInt40 { std::uint64_t value; + + UInt40(std::uint64_t v) : value(v) + { + if (v > pow(2, 40)-1){ + throw std::out_of_range("Can not convert uint64_t value exceeding 40 bit unsigned range into UInt40"); + } + } + + operator std::uint64_t() const + { + return value; + } }; struct UInt48 { std::uint64_t value; + + UInt48(std::uint64_t v) : value(v) + { + if (v > pow(2, 48)-1){ + throw std::out_of_range("Can not convert uint64_t value exceeding 48 bit unsigned range into UInt48"); + } + } + + operator std::uint64_t() const + { + return value; + } }; struct UInt56 { std::uint64_t value; + + UInt56(std::uint64_t v) : value(v) + { + if (v > pow(2, 56)-1){ + throw std::out_of_range("Can not convert uint64_t value exceeding 56 bit unsigned range into UInt56"); + } + } + + 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; + } }; + // 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 REAL32 = float; + using REAL64 = double; + + using TIME_OF_DAY = sdo::TimeOfDay; + using TIME_DIFFERENCE = sdo::TimeDifference; + + using GUID = sdo::Guid; + using DOMAIN = std::vector; + + // ---DESERIALIZATION--- template T deserialize(const std::vector&) @@ -500,8 +659,7 @@ namespace sdo template <> [[nodiscard]] inline std::vector serialize(const Int24& value) { - if (value.value < -pow(2, 23) || value.value > pow(2, 23)-1) - { + if (value.value < -pow(2, 23) || value.value > pow(2, 23)-1){ throw std::out_of_range("Int24 value exceeds 24 bit signed range"); } @@ -512,8 +670,7 @@ namespace sdo template <> [[nodiscard]] inline std::vector serialize(const Int40& value) { - if (value.value < -pow(2, 39) || value.value > pow(2, 39)-1) - { + if (value.value < -pow(2, 39) || value.value > pow(2, 39)-1){ throw std::out_of_range("Int40 value exceeds 40 bit signed range"); } @@ -524,8 +681,7 @@ namespace sdo template <> [[nodiscard]] inline std::vector serialize(const Int48& value) { - if (value.value < -pow(2, 47) || value.value > pow(2, 47)-1) - { + if (value.value < -pow(2, 47) || value.value > pow(2, 47)-1){ throw std::out_of_range("Int48 value exceeds 48 bit signed range"); } @@ -536,8 +692,7 @@ namespace sdo template <> [[nodiscard]] inline std::vector serialize(const Int56& value) { - if (value.value < -pow(2, 55) || value.value > pow(2, 55)-1) - { + if (value.value < -pow(2, 55) || value.value > pow(2, 55)-1){ throw std::out_of_range("Int56 value exceeds 56 bit signed range"); } @@ -555,8 +710,7 @@ namespace sdo template <> [[nodiscard]] inline std::vector serialize(const UInt24& value) { - if (value.value > pow(2, 24)-1) - { + if (value.value > pow(2, 24)-1){ throw std::out_of_range("UInt24 value exceeds 24 bit unsigned range"); } @@ -565,8 +719,7 @@ namespace sdo template <> [[nodiscard]] inline std::vector serialize(const UInt40& value) { - if (value.value > pow(2, 40)-1) - { + if (value.value > pow(2, 40)-1){ throw std::out_of_range("UInt40 value exceeds 40 bit unsigned range"); } @@ -575,8 +728,7 @@ namespace sdo template <> [[nodiscard]] inline std::vector serialize(const UInt48& value) { - if (value.value > pow(2, 48)-1) - { + if (value.value > pow(2, 48)-1){ throw std::out_of_range("UInt48 value exceeds 48 bit unsigned range"); } @@ -585,8 +737,7 @@ namespace sdo template <> [[nodiscard]] inline std::vector serialize(const UInt56& value) { - if (value.value > pow(2, 56)-1) - { + if (value.value > pow(2, 56)-1){ throw std::out_of_range("UInt56 value exceeds 56 bit unsigned range"); } From 49ec36156640e88d324499de5630390313b93a06 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Mon, 25 May 2026 22:19:48 +0000 Subject: [PATCH 11/53] sdo_serializer.hpp: add STRING data type --- .../include/rise_motion/sdo_serializer.hpp | 192 ++++++++++++------ 1 file changed, 129 insertions(+), 63 deletions(-) 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 index f299b43..73d8af2 100644 --- 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 @@ -11,66 +11,6 @@ #include #include - -namespace sdo::helpers -{ - template inline constexpr bool always_false = false; - - inline void check_size(const std::vector& blob, std::size_t expectedSize, const char* type) - { - if (blob.size() != expectedSize){ - throw std::invalid_argument(std::string("deserialize<") + type + ">: expected " + - std::to_string(expectedSize) + " bytes, got " + std::to_string(blob.size())); - } - } - - template [[nodiscard]] inline T 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)) - { - throw std::invalid_argument("to_raw: numBytes does not fit T"); - } - - if (offset + numBytes > blob.size()) - { - throw std::out_of_range("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 raw; - } - - template [[nodiscard]] inline std::vector 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)) - { - throw std::invalid_argument("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 blob; - } -} - namespace sdo { // as equivalent of the EtherCAT TIME_OF_DAY data type @@ -236,6 +176,25 @@ namespace sdo } }; + template struct STRING + { + std::string value; + + STRING() = default; + + STRING(std::string v) : value(std::move(v)) + { + if (value.size() > T){ + throw std::out_of_range("STRING: string is too long to be coverted to STRING of size T"); + } + }; + + operator std::string() const + { + return value; + } + }; + // IEC 61131-3 data types using BOOL = bool; @@ -290,13 +249,105 @@ namespace sdo using GUID = sdo::Guid; using DOMAIN = std::vector; +} + + +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; + }; + + inline void check_size(const std::vector& blob, std::size_t expectedSize, const char* type) + { + if (blob.size() != expectedSize){ + throw std::invalid_argument(std::string("deserialize<") + type + ">: expected " + + std::to_string(expectedSize) + " bytes, got " + std::to_string(blob.size())); + } + } + template [[nodiscard]] inline T 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)) + { + throw std::invalid_argument("to_raw: numBytes does not fit T"); + } + + if (offset + numBytes > blob.size()) + { + throw std::out_of_range("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 raw; + } + + template [[nodiscard]] inline std::vector 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)) + { + throw std::invalid_argument("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 blob; + } +} + +namespace sdo +{ // ---DESERIALIZATION--- - template T deserialize(const std::vector&) + template T deserialize(const std::vector& blob) { - static_assert(sdo::helpers::always_false, "deserialize: unsupported type"); + if constexpr (sdo::helpers::is_string_type>>::value){ + + constexpr std::size_t size = sdo::helpers::string_size::size; + + if (blob.size() > size){ + throw std::invalid_argument("deserialize>: blob is larger than the expected size T"); + } + + T string{}; + + string.value.assign(blob.begin(), blob.end()); + + // strip padding zeros + while (!string.value.empty() && string.value.back() == '\0'){ + string.value.pop_back(); + } + + return string; + } + else{ + static_assert(sdo::helpers::always_false, "deserialize: unsupported type"); + } } // -Boolean- @@ -546,7 +597,22 @@ namespace sdo template std::vector serialize(const T& value) { - static_assert(sdo::helpers::always_false, "serialize: unsupported type"); + if constexpr (sdo::helpers::is_string_type>>::value){ + + constexpr std::size_t size = sdo::helpers::string_size::size; + + if (value.value.size() > size){ + throw std::out_of_range("serialize>: string is longer than size T"); + } + + std::vector blob(value.value.begin(), value.value.end()); + blob.resize(size, '\0'); + + return blob; + } + else{ + static_assert(sdo::helpers::always_false, "serialize: unsupported type"); + } return{}; } From 145345e3377c48fb658ca9a9b5896000a170e258 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Mon, 25 May 2026 22:21:25 +0000 Subject: [PATCH 12/53] test_sdo_serializer.cpp: add test for STRING(50) type --- .../rise_motion/test/test_sdo_serializer.cpp | 35 +++++++++++++++---- 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp index e24d1a5..44cb457 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp @@ -74,8 +74,8 @@ TEST(SDOSerializationTest, uInt8) std::uint8_t value_zero = 0; std::uint8_t value_max = std::numeric_limits::max(); - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); ASSERT_EQ(result_zero, value_zero); blob = sdo::serialize(value_max); @@ -180,13 +180,13 @@ TEST(SDOSerializationTest, Domain) TEST(SDOSerializationTest, Int24) { - sdo::Int24 value_zero{0}; + std::int32_t value_zero = 0; sdo::Int24 value_max{(std::int64_t{1} << 23) - 1}; sdo::Int24 value_min{-(std::int64_t{1} << 23)}; - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); + std::vector blob = sdo::serialize(value_zero); + auto result_zero = sdo::deserialize(blob); + ASSERT_EQ(result_zero, value_zero); blob = sdo::serialize(value_max); auto result_max = sdo::deserialize(blob); @@ -356,3 +356,26 @@ TEST(SDOSerializationTest, Guid) auto result = sdo::deserialize(blob); ASSERT_EQ(result.bytes, value.bytes); } + +TEST(SDOSerializationTest, String50) +{ + std::string string = "RISE"; + + std::vector blob = sdo::serialize>(string); + + ASSERT_EQ(blob.size(), 50); + + ASSERT_EQ(blob[0], static_cast('R')); + ASSERT_EQ(blob[1], static_cast('I')); + ASSERT_EQ(blob[2], static_cast('S')); + ASSERT_EQ(blob[3], static_cast('E')); + + for (std::size_t i = string.size(); i < blob.size(); ++i) + { + ASSERT_EQ(blob[i], 0); + } + + std::string result = sdo::deserialize>(blob); + + ASSERT_EQ(result, string); +} From a94c05e203bc516830d9ff37af137e1859b29386 Mon Sep 17 00:00:00 2001 From: Anton Date: Tue, 26 May 2026 17:47:26 +0200 Subject: [PATCH 13/53] infrastructure for python utility library --- rise_motion_dev_ws/src/rise_motion/CMakeLists.txt | 8 ++++++++ rise_motion_dev_ws/src/rise_motion/package.xml | 3 +++ .../src/rise_motion/rise_motion/__init__.py | 0 3 files changed, 11 insertions(+) create mode 100644 rise_motion_dev_ws/src/rise_motion/rise_motion/__init__.py diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index 3f49c4f..2cf1b17 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -9,6 +9,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rise_motion_messages REQUIRED) +find_package(ament_cmake_python REQUIRED) include_directories(include) @@ -54,9 +55,13 @@ install(TARGETS testing_node DESTINATION lib/${PROJECT_NAME} ) + +ament_python_install_package(${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest 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) @@ -70,6 +75,9 @@ if(BUILD_TESTING) ament_add_gtest(test_sdo_serializer test/test_sdo_serializer.cpp ) + ament_add_pytest_test(test_sdo_serializer + test/test_sdo_serializer.py + ) if(TARGET test_sdo_serializer) target_link_libraries(test_sdo_serializer sdo_serializer_lib) endif() diff --git a/rise_motion_dev_ws/src/rise_motion/package.xml b/rise_motion_dev_ws/src/rise_motion/package.xml index 9c5ac6c..e7034d0 100644 --- a/rise_motion_dev_ws/src/rise_motion/package.xml +++ b/rise_motion_dev_ws/src/rise_motion/package.xml @@ -8,6 +8,9 @@ TODO: License declaration ament_cmake + + ament_cmake_python + ament_cmake_pytest rclcpp rise_motion_messages diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/__init__.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/__init__.py new file mode 100644 index 0000000..e69de29 From d759642a7c26dfdc9ff0774fcd3a3892fe32f04e Mon Sep 17 00:00:00 2001 From: Anton Date: Tue, 26 May 2026 23:27:53 +0200 Subject: [PATCH 14/53] start implementing marshalling for bit strings --- .../rise_motion/rise_motion/sdo_serializer.py | 211 ++++++++++++++++++ .../rise_motion/test/test_sdo_serializer.py | 10 + 2 files changed, 221 insertions(+) create mode 100644 rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py create mode 100644 rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py new file mode 100644 index 0000000..a20ea35 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -0,0 +1,211 @@ +from __future__ import annotations +from dataclasses import dataclass +from typing import Dict + + +# --------------------------------------------------------------------------- +# Record type +# --------------------------------------------------------------------------- + +@dataclass(frozen=True) +class TypeInfo: + 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 (keyed by object dictionary index) +# --------------------------------------------------------------------------- + +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_uint8", "deserialize_uint8"), + 0x001F: TypeInfo(0x001F, "WORD", "WORD", 16, "serialize_uint16", "deserialize_uint16"), + 0x0020: TypeInfo(0x0020, "DWORD", "DWORD", 32, "serialize_uint32", "deserialize_uint32"), + + # Time types (48-bit, special structure) + 0x000C: TypeInfo(0x000C, "TIME_OF_DAY", "TIME_OF_DAY", 48, "serialize_time48", "deserialize_time48"), + 0x000D: TypeInfo(0x000D, "TIME_DIFFERENCE","TIME_DIFFERENCE", 48, "serialize_time48", "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_bitarr8", "deserialize_bitarr8"), + 0x002E: TypeInfo(0x002E, "BITARR16", "BITARR16", 16, "serialize_bitarr16", "deserialize_bitarr16"), + 0x002F: TypeInfo(0x002F, "BITARR32", "BITARR32", 32, "serialize_bitarr32", "deserialize_bitarr32"), + + # Signed integers + 0x0002: TypeInfo(0x0002, "INTEGER8", "SINT", 8, "serialize_int8", "deserialize_int8"), + 0x0003: TypeInfo(0x0003, "INTEGER16", "INT", 16, "serialize_int16", "deserialize_int16"), + 0x0010: TypeInfo(0x0010, "INTEGER24", "INT24", 24, "serialize_intn", "deserialize_intn"), + 0x0004: TypeInfo(0x0004, "INTEGER32", "DINT", 32, "serialize_int32", "deserialize_int32"), + 0x0012: TypeInfo(0x0012, "INTEGER40", "INT40", 40, "serialize_intn", "deserialize_intn"), + 0x0013: TypeInfo(0x0013, "INTEGER48", "INT48", 48, "serialize_intn", "deserialize_intn"), + 0x0014: TypeInfo(0x0014, "INTEGER56", "INT56", 56, "serialize_intn", "deserialize_intn"), + 0x0015: TypeInfo(0x0015, "INTEGER64", "LINT", 64, "serialize_int64", "deserialize_int64"), + + # Unsigned integers + 0x0005: TypeInfo(0x0005, "UNSIGNED8", "USINT", 8, "serialize_uint8", "deserialize_uint8"), + 0x0006: TypeInfo(0x0006, "UNSIGNED16", "UINT", 16, "serialize_uint16", "deserialize_uint16"), + 0x0016: TypeInfo(0x0016, "UNSIGNED24", "UINT24", 24, "serialize_uintn", "deserialize_uintn"), # ← comma was missing in source + 0x0007: TypeInfo(0x0007, "UNSIGNED32", "UDINT", 32, "serialize_uint32", "deserialize_uint32"), + 0x0018: TypeInfo(0x0018, "UNSIGNED40", "UINT40", 40, "serialize_uintn", "deserialize_uintn"), + 0x0019: TypeInfo(0x0019, "UNSIGNED48", "UINT48", 48, "serialize_uintn", "deserialize_uintn"), + 0x001A: TypeInfo(0x001A, "UNSIGNED56", "UINT56", 56, "serialize_uintn", "deserialize_uintn"), + 0x001B: TypeInfo(0x001B, "UNSIGNED64", "ULINT", 64, "serialize_uint64", "deserialize_uint64"), + + # Floating point + 0x0008: TypeInfo(0x0008, "REAL32", "REAL", 32, "serialize_float32", "deserialize_float32"), + 0x0011: TypeInfo(0x0011, "REAL64", "LREAL", 64, "serialize_float64", "deserialize_float64"), + + # GUID + 0x001D: TypeInfo(0x001D, "GUID", "GUID", 128, "serialize_guid", "deserialize_guid"), +} + + +# --------------------------------------------------------------------------- +# 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:#06x}") 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, +) -> bytes: + object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) + # using the specific function for this data type which is stored in the almighty dict + return globals()[object_info.serialize_fn](value, object_info.bit_size) + +def deserialize( + serialized_value: bytes, + *, + index: int | None = None, + name: str | None = None, + base_data_type: str | None = None, +): + if type(serialized_value) is not bytes: + raise TypeError("serialized_value should be bytes object") + object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) + # using the specific function for this data type which is stored in the almighty dict + return globals()[object_info.deserialize_fn](serialized_value, object_info.bit_size) + + +# --------------------------------------------------------------------------- +# Specific functions (called by generic functions) +# --------------------------------------------------------------------------- + +# TODO: could add support for other ways to pass bits than strings +def serialize_bitn(val: str, bit_s: int) -> bytes: + """ + Serialize a bit-string into a bytes object. + """ + if not (1 <= len(val) <= 16): + raise ValueError("val length must be between 1 and 16 bits") + + if any(c not in "01" for c in val): + raise ValueError("val must contain only '0' and '1'") + + if bit_s != len(val): + raise ValueError("bit_s must be equal to val length") + + # TODO: test if left-pad with zeroes lines up with how SOEM inserts or retreives the string + # Left-pad with zeros to requested bit width + padded = val.zfill(bit_s) + + # Convert to integer + n = int(padded, 2) + + # Number of bytes required + byte_len = (bit_s + 7) // 8 + + return n.to_bytes(byte_len, byteorder="big") + +def deserialize_bitn(ser_val, bit_s): + """ + Deserialize a bytes object into a bit-string. + """ + if not (1 <= len(ser_val)*8 <= 16): + raise ValueError("val length must be between 1 and 16 bits") + + if (bit_s + 7) // 8 != len(ser_val): + raise ValueError( + "number of bytes needed to contain bit_s must be equal to ser_val length") + + return ''.join(format(byte, '08b') for byte in ser_val) \ No newline at end of file diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py new file mode 100644 index 0000000..9f52eeb --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -0,0 +1,10 @@ +import random +from rise_motion.sdo_serializer import * + +def test_SDO_serialization_bit1(): + value = random.choice(('0', '1')) + assert value == deserialize(serialize(value, base_data_type="BIT1"), base_data_type="BIT1") + +def test_SDO_serialization_bit2(): + value = format(random.randint(0, 3), "b") + assert value == deserialize(serialize(value, base_data_type="BIT1"), base_data_type="BIT1") \ No newline at end of file From 40bf0ca79706a9e13e069ad5a18aede917cad0ae Mon Sep 17 00:00:00 2001 From: Anton Date: Tue, 26 May 2026 23:41:47 +0200 Subject: [PATCH 15/53] fix test_sdo_serializer name overlap --- rise_motion_dev_ws/src/rise_motion/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index 2cf1b17..aaa258d 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -75,7 +75,7 @@ if(BUILD_TESTING) ament_add_gtest(test_sdo_serializer test/test_sdo_serializer.cpp ) - ament_add_pytest_test(test_sdo_serializer + ament_add_pytest_test(test_sdo_serializer_python test/test_sdo_serializer.py ) if(TARGET test_sdo_serializer) From c61e2ff759113bb40e84dea3aa3c2a0a81871878 Mon Sep 17 00:00:00 2001 From: Anton Date: Wed, 27 May 2026 21:52:50 +0200 Subject: [PATCH 16/53] BIT1 - BIT16 (de)serialization pass pytest --- .../rise_motion/rise_motion/sdo_serializer.py | 32 ++++----- .../rise_motion/test/test_sdo_serializer.py | 67 +++++++++++++++++-- 2 files changed, 72 insertions(+), 27 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py index a20ea35..cd68eed 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -172,40 +172,32 @@ def deserialize( # --------------------------------------------------------------------------- # TODO: could add support for other ways to pass bits than strings -def serialize_bitn(val: str, bit_s: int) -> bytes: +def serialize_bitn(val, bit_s: int) -> bytes: """ Serialize a bit-string into a bytes object. """ - if not (1 <= len(val) <= 16): - raise ValueError("val length must be between 1 and 16 bits") - - if any(c not in "01" for c in val): - raise ValueError("val must contain only '0' and '1'") - - if bit_s != len(val): - raise ValueError("bit_s must be equal to val length") - - # TODO: test if left-pad with zeroes lines up with how SOEM inserts or retreives the string - # Left-pad with zeros to requested bit width - padded = val.zfill(bit_s) - - # Convert to integer - n = int(padded, 2) + if type(val) is str: + vali = int(val, 2) + elif type(val) is int: + vali = val + else: + raise TypeError(f"val length must be either int or str, not {type(val)}") # Number of bytes required byte_len = (bit_s + 7) // 8 - return n.to_bytes(byte_len, byteorder="big") + return vali.to_bytes(byte_len, byteorder="little") def deserialize_bitn(ser_val, bit_s): """ Deserialize a bytes object into a bit-string. """ - if not (1 <= len(ser_val)*8 <= 16): - raise ValueError("val length must be between 1 and 16 bits") if (bit_s + 7) // 8 != len(ser_val): raise ValueError( "number of bytes needed to contain bit_s must be equal to ser_val length") - return ''.join(format(byte, '08b') for byte in ser_val) \ No newline at end of file + value = int.from_bytes(ser_val, byteorder='little') + bit_string = bin(value)[2:] + bit_string = bit_string.zfill(bit_s) + return bit_string \ No newline at end of file diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py index 9f52eeb..6f43955 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -1,10 +1,63 @@ +"""Tests for sdo_serializer.py module.""" import random -from rise_motion.sdo_serializer import * +import pytest +from rise_motion.sdo_serializer import serialize, deserialize -def test_SDO_serialization_bit1(): - value = random.choice(('0', '1')) - assert value == deserialize(serialize(value, base_data_type="BIT1"), base_data_type="BIT1") +# --------------------------------------------------------------------------- +# Bit strings BIT1 - BIT16 tests +# --------------------------------------------------------------------------- -def test_SDO_serialization_bit2(): - value = format(random.randint(0, 3), "b") - assert value == deserialize(serialize(value, base_data_type="BIT1"), base_data_type="BIT1") \ No newline at end of file +# --- round-trip tests --- + +@pytest.mark.parametrize("bit_width", range(1, 17)) +def test_roundtrip_random(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(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(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 bytes) --- + +@pytest.mark.parametrize("value, dtype, expected_bytes", [ + ("0", "BIT1", b"\x00"), + ("1", "BIT1", b"\x01"), + ("11", "BIT2", b"\x03"), + ("10", "BIT2", b"\x02"), + ("1111111111111111", "BIT16", b"\xff\xff"), + ("0000000000000000", "BIT16", b"\x00\x00"), + ("1000000000000000", "BIT16", b"\x00\x80"), # MSB only +]) +def test_serialize_known_values(value: str, dtype: str, expected_bytes: bytes): + assert expected_bytes == serialize(value, base_data_type=dtype) + + +# --- Explicit deserialization checks (known bytes -> known output) --- + +@pytest.mark.parametrize("raw_bytes, dtype, expected_value", [ + (b"\x00", "BIT1", "0"), + (b"\x01", "BIT1", "1"), + (b"\x03", "BIT2", "11"), + (b"\x02", "BIT2", "10"), + (b"\xff\xff", "BIT16", "1111111111111111"), + (b"\x00\x00", "BIT16", "0000000000000000"), +]) +def test_deserialize_known_values(raw_bytes: bytes, dtype: str, expected_value: str): + assert expected_value == deserialize(raw_bytes, base_data_type=dtype) \ No newline at end of file From d79ba5175bfa2e0452c9f83f83ef95b57418c179 Mon Sep 17 00:00:00 2001 From: Anton Date: Wed, 27 May 2026 22:16:30 +0200 Subject: [PATCH 17/53] BITARR8,16,32 (de)serialization pass pytest --- .../rise_motion/rise_motion/sdo_serializer.py | 9 ++- .../rise_motion/test/test_sdo_serializer.py | 59 +++++++++++++++++++ 2 files changed, 63 insertions(+), 5 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py index cd68eed..af43c06 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -33,7 +33,7 @@ class TypeInfo: 0x000C: TypeInfo(0x000C, "TIME_OF_DAY", "TIME_OF_DAY", 48, "serialize_time48", "deserialize_time48"), 0x000D: TypeInfo(0x000D, "TIME_DIFFERENCE","TIME_DIFFERENCE", 48, "serialize_time48", "deserialize_time48"), - # Bit strings BIT1 – BIT16 + # 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"), @@ -52,9 +52,9 @@ class TypeInfo: 0x003F: TypeInfo(0x003F, "BIT16", "BIT16", 16, "serialize_bitn", "deserialize_bitn"), # Bit arrays - 0x002D: TypeInfo(0x002D, "BITARR8", "BITARR8", 8, "serialize_bitarr8", "deserialize_bitarr8"), - 0x002E: TypeInfo(0x002E, "BITARR16", "BITARR16", 16, "serialize_bitarr16", "deserialize_bitarr16"), - 0x002F: TypeInfo(0x002F, "BITARR32", "BITARR32", 32, "serialize_bitarr32", "deserialize_bitarr32"), + 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_int8", "deserialize_int8"), @@ -171,7 +171,6 @@ def deserialize( # Specific functions (called by generic functions) # --------------------------------------------------------------------------- -# TODO: could add support for other ways to pass bits than strings def serialize_bitn(val, bit_s: int) -> bytes: """ Serialize a bit-string into a bytes object. diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py index 6f43955..f1355a3 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -59,5 +59,64 @@ def test_serialize_known_values(value: str, dtype: str, expected_bytes: bytes): (b"\xff\xff", "BIT16", "1111111111111111"), (b"\x00\x00", "BIT16", "0000000000000000"), ]) +def test_deserialize_known_values(raw_bytes: bytes, dtype: str, expected_value: str): + assert expected_value == deserialize(raw_bytes, base_data_type=dtype) + + +# --------------------------------------------------------------------------- +# Bit arrays BITARR8, BITARR16, BITARR32 tests +# --------------------------------------------------------------------------- + +# --- round-trip tests --- + +@pytest.mark.parametrize("bit_width", [8, 16, 32]) +def test_roundtrip_random(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"BITARR{bit_width}" + assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype) + + +@pytest.mark.parametrize("bit_width", [8, 16, 32]) +def test_roundtrip_min(bit_width: int): + """Zero survives a round-trip for every bit width.""" + value = "0".zfill(bit_width) + dtype = f"BITARR{bit_width}" + assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype) + + +@pytest.mark.parametrize("bit_width", [8, 16, 32]) +def test_roundtrip_max(bit_width: int): + """All-ones value survives a round-trip for every bit width.""" + value = "1" * bit_width + dtype = f"BITARR{bit_width}" + assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype) + + +# --- Explicit serialization checks (known input -> known bytes) --- + +@pytest.mark.parametrize("value, dtype, expected_bytes", [ + ("10101100", "BITARR8", b"\xac"), + ("01010101", "BITARR8", b"\x55"), + ("1100101001110001", "BITARR16", b"\x71\xca"), + ("0011110000101110", "BITARR16", b"\x2e\x3c"), + ("10010000111100001010101001101101", "BITARR32", b"\x6d\xaa\xf0\x90"), + ("01101111000101011000001111110000", "BITARR32", b"\xf0\x83\x15\x6f"), +]) +def test_serialize_known_values(value: str, dtype: str, expected_bytes: bytes): + assert expected_bytes == serialize(value, base_data_type=dtype) + + +# --- Explicit deserialization checks (known bytes -> known output) --- + +@pytest.mark.parametrize("expected_value, dtype, raw_bytes", [ + ("10101100", "BITARR8", b"\xac"), + ("01010101", "BITARR8", b"\x55"), + ("1100101001110001", "BITARR16", b"\x71\xca"), + ("0011110000101110", "BITARR16", b"\x2e\x3c"), + ("10010000111100001010101001101101", "BITARR32", b"\x6d\xaa\xf0\x90"), + ("01101111000101011000001111110000", "BITARR32", b"\xf0\x83\x15\x6f"), +]) def test_deserialize_known_values(raw_bytes: bytes, dtype: str, expected_value: str): assert expected_value == deserialize(raw_bytes, base_data_type=dtype) \ No newline at end of file From 26178b80edc6ce23d22aafc6227c9d843eb1e285 Mon Sep 17 00:00:00 2001 From: Anton Date: Wed, 27 May 2026 23:48:05 +0200 Subject: [PATCH 18/53] signed ints (de)serialization pass pytest --- .../rise_motion/rise_motion/sdo_serializer.py | 60 ++++++++------ .../rise_motion/test/test_sdo_serializer.py | 78 ++++++++++++++++++- 2 files changed, 115 insertions(+), 23 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py index af43c06..d973bb5 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -57,24 +57,24 @@ class TypeInfo: 0x002F: TypeInfo(0x002F, "BITARR32", "BITARR32", 32, "serialize_bitn", "deserialize_bitn"), # Signed integers - 0x0002: TypeInfo(0x0002, "INTEGER8", "SINT", 8, "serialize_int8", "deserialize_int8"), - 0x0003: TypeInfo(0x0003, "INTEGER16", "INT", 16, "serialize_int16", "deserialize_int16"), - 0x0010: TypeInfo(0x0010, "INTEGER24", "INT24", 24, "serialize_intn", "deserialize_intn"), - 0x0004: TypeInfo(0x0004, "INTEGER32", "DINT", 32, "serialize_int32", "deserialize_int32"), - 0x0012: TypeInfo(0x0012, "INTEGER40", "INT40", 40, "serialize_intn", "deserialize_intn"), - 0x0013: TypeInfo(0x0013, "INTEGER48", "INT48", 48, "serialize_intn", "deserialize_intn"), - 0x0014: TypeInfo(0x0014, "INTEGER56", "INT56", 56, "serialize_intn", "deserialize_intn"), - 0x0015: TypeInfo(0x0015, "INTEGER64", "LINT", 64, "serialize_int64", "deserialize_int64"), + 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_uint8", "deserialize_uint8"), - 0x0006: TypeInfo(0x0006, "UNSIGNED16", "UINT", 16, "serialize_uint16", "deserialize_uint16"), - 0x0016: TypeInfo(0x0016, "UNSIGNED24", "UINT24", 24, "serialize_uintn", "deserialize_uintn"), # ← comma was missing in source - 0x0007: TypeInfo(0x0007, "UNSIGNED32", "UDINT", 32, "serialize_uint32", "deserialize_uint32"), - 0x0018: TypeInfo(0x0018, "UNSIGNED40", "UINT40", 40, "serialize_uintn", "deserialize_uintn"), - 0x0019: TypeInfo(0x0019, "UNSIGNED48", "UINT48", 48, "serialize_uintn", "deserialize_uintn"), - 0x001A: TypeInfo(0x001A, "UNSIGNED56", "UINT56", 56, "serialize_uintn", "deserialize_uintn"), - 0x001B: TypeInfo(0x001B, "UNSIGNED64", "ULINT", 64, "serialize_uint64", "deserialize_uint64"), + 0x0005: TypeInfo(0x0005, "UNSIGNED8", "USINT", 8, "serialize_int", "deserialize_int"), + 0x0006: TypeInfo(0x0006, "UNSIGNED16", "UINT", 16, "serialize_int", "deserialize_int"), + 0x0016: TypeInfo(0x0016, "UNSIGNED24", "UINT24", 24, "serialize_int", "deserialize_int"), + 0x0007: TypeInfo(0x0007, "UNSIGNED32", "UDINT", 32, "serialize_int", "deserialize_int"), + 0x0018: TypeInfo(0x0018, "UNSIGNED40", "UINT40", 40, "serialize_int", "deserialize_int"), + 0x0019: TypeInfo(0x0019, "UNSIGNED48", "UINT48", 48, "serialize_int", "deserialize_int"), + 0x001A: TypeInfo(0x001A, "UNSIGNED56", "UINT56", 56, "serialize_int", "deserialize_int"), + 0x001B: TypeInfo(0x001B, "UNSIGNED64", "ULINT", 64, "serialize_int", "deserialize_int"), # Floating point 0x0008: TypeInfo(0x0008, "REAL32", "REAL", 32, "serialize_float32", "deserialize_float32"), @@ -163,6 +163,9 @@ def deserialize( if type(serialized_value) is not bytes: raise TypeError("serialized_value should be bytes object") object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) + if (object_info.bit_size + 7) // 8 != len(serialized_value): + raise ValueError( + "number of bytes needed to contain object_info.bit_size must be equal to serialized_value length") # using the specific function for this data type which is stored in the almighty dict return globals()[object_info.deserialize_fn](serialized_value, object_info.bit_size) @@ -187,16 +190,29 @@ def serialize_bitn(val, bit_s: int) -> bytes: return vali.to_bytes(byte_len, byteorder="little") -def deserialize_bitn(ser_val, bit_s): +def deserialize_bitn(ser_val: bytes, bit_s): """ Deserialize a bytes object into a bit-string. """ - if (bit_s + 7) // 8 != len(ser_val): - raise ValueError( - "number of bytes needed to contain bit_s must be equal to ser_val length") - value = int.from_bytes(ser_val, byteorder='little') bit_string = bin(value)[2:] bit_string = bit_string.zfill(bit_s) - return bit_string \ No newline at end of file + return bit_string + +def serialize_int(val: int, bit_s: int) -> bytes: + """ + Serialize an int into a bytes object. + """ + + # Number of bytes required + byte_len = (bit_s + 7) // 8 + + return val.to_bytes(byte_len, byteorder="little", signed=True) + +def deserialize_int(ser_val: bytes, bit_s): + """ + Deserialize a bytes object into an int. + """ + + return int.from_bytes(ser_val, byteorder='little', signed=True) \ No newline at end of file diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py index f1355a3..a0c2f4e 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -119,4 +119,80 @@ def test_serialize_known_values(value: str, dtype: str, expected_bytes: bytes): ("01101111000101011000001111110000", "BITARR32", b"\xf0\x83\x15\x6f"), ]) def test_deserialize_known_values(raw_bytes: bytes, dtype: str, expected_value: str): - assert expected_value == deserialize(raw_bytes, base_data_type=dtype) \ No newline at end of file + assert expected_value == deserialize(raw_bytes, base_data_type=dtype) + + +# --------------------------------------------------------------------------- +# Signed integers 8, 16, 24, 32, 40, 48, 56, 64 tests +# --------------------------------------------------------------------------- +# These tests could be overkill for such a simple function, +# because I wrote them using the tests for the datatypes above as a template. +# Maybe they will help should the serialization for signed ints get more elaborate. + +# --- round-trip tests --- + +@pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64]) +def test_roundtrip_random(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_min(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(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_max(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 bytes) --- + +@pytest.mark.parametrize("value, dtype, expected_bytes", [ + (-(1 << 3), "INTEGER8", b"\xf8"), + ((1 << 12), "INTEGER16", b"\x00\x10"), + (-(1 << 22), "INTEGER24", b"\x00\x00\xc0"), + ((1 << 2), "INTEGER32", b"\x04\x00\x00\x00"), + (-(1 << 20), "INTEGER40", b"\x00\x00\xf0\xff\xff"), + ((1 << 40), "INTEGER48", b"\x00\x00\x00\x00\x00\x01"), + (-(1 << 40), "INTEGER56", b"\x00\x00\x00\x00\x00\xff\xff"), + ((1 << 62), "INTEGER64", b"\x00\x00\x00\x00\x00\x00\x00\x40"), +]) +def test_serialize_known_values(value: str, dtype: str, expected_bytes: bytes): + assert expected_bytes == serialize(value, name=dtype) + + +# --- Explicit deserialization checks (known bytes -> known output) --- + +@pytest.mark.parametrize("expected_value, dtype, raw_bytes", [ + (-(1 << 3), "INTEGER8", b"\xf8"), + ((1 << 12), "INTEGER16", b"\x00\x10"), + (-(1 << 22), "INTEGER24", b"\x00\x00\xc0"), + ((1 << 2), "INTEGER32", b"\x04\x00\x00\x00"), + (-(1 << 20), "INTEGER40", b"\x00\x00\xf0\xff\xff"), + ((1 << 40), "INTEGER48", b"\x00\x00\x00\x00\x00\x01"), + (-(1 << 40), "INTEGER56", b"\x00\x00\x00\x00\x00\xff\xff"), + ((1 << 62), "INTEGER64", b"\x00\x00\x00\x00\x00\x00\x00\x40"), +]) +def test_deserialize_known_values(raw_bytes: bytes, dtype: str, expected_value: str): + assert expected_value == deserialize(raw_bytes, name=dtype) \ No newline at end of file From b7fd544135da08bca2ab0086c10328bdab5d5e87 Mon Sep 17 00:00:00 2001 From: Anton Date: Thu, 28 May 2026 00:08:04 +0200 Subject: [PATCH 19/53] uints (de)serialization pass pytest --- .../rise_motion/rise_motion/sdo_serializer.py | 35 ++++++++++++++----- .../rise_motion/test/test_sdo_serializer.py | 34 +++++++++++++++++- 2 files changed, 59 insertions(+), 10 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py index d973bb5..3030ec6 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -67,14 +67,14 @@ class TypeInfo: 0x0015: TypeInfo(0x0015, "INTEGER64", "LINT", 64, "serialize_int", "deserialize_int"), # Unsigned integers - 0x0005: TypeInfo(0x0005, "UNSIGNED8", "USINT", 8, "serialize_int", "deserialize_int"), - 0x0006: TypeInfo(0x0006, "UNSIGNED16", "UINT", 16, "serialize_int", "deserialize_int"), - 0x0016: TypeInfo(0x0016, "UNSIGNED24", "UINT24", 24, "serialize_int", "deserialize_int"), - 0x0007: TypeInfo(0x0007, "UNSIGNED32", "UDINT", 32, "serialize_int", "deserialize_int"), - 0x0018: TypeInfo(0x0018, "UNSIGNED40", "UINT40", 40, "serialize_int", "deserialize_int"), - 0x0019: TypeInfo(0x0019, "UNSIGNED48", "UINT48", 48, "serialize_int", "deserialize_int"), - 0x001A: TypeInfo(0x001A, "UNSIGNED56", "UINT56", 56, "serialize_int", "deserialize_int"), - 0x001B: TypeInfo(0x001B, "UNSIGNED64", "ULINT", 64, "serialize_int", "deserialize_int"), + 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_float32", "deserialize_float32"), @@ -215,4 +215,21 @@ def deserialize_int(ser_val: bytes, bit_s): Deserialize a bytes object into an int. """ - return int.from_bytes(ser_val, byteorder='little', signed=True) \ No newline at end of file + return int.from_bytes(ser_val, byteorder='little', signed=True) + +def serialize_uint(val: int, bit_s: int) -> bytes: + """ + Serialize a uint into a bytes object. + """ + + # Number of bytes required + byte_len = (bit_s + 7) // 8 + + return val.to_bytes(byte_len, byteorder="little") + +def deserialize_uint(ser_val: bytes, bit_s): + """ + Deserialize a bytes object into a uint. + """ + + return int.from_bytes(ser_val, byteorder='little') \ No newline at end of file diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py index a0c2f4e..37fcb05 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -195,4 +195,36 @@ def test_serialize_known_values(value: str, dtype: str, expected_bytes: bytes): ((1 << 62), "INTEGER64", b"\x00\x00\x00\x00\x00\x00\x00\x40"), ]) def test_deserialize_known_values(raw_bytes: bytes, dtype: str, expected_value: str): - assert expected_value == deserialize(raw_bytes, name=dtype) \ No newline at end of file + assert expected_value == deserialize(raw_bytes, 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(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(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(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) \ No newline at end of file From fa0d13b480f8fd70923b967f34051dcef382bb1c Mon Sep 17 00:00:00 2001 From: Anton Date: Thu, 28 May 2026 12:48:00 +0200 Subject: [PATCH 20/53] bool and generic words (de)serialization pass pytest --- .../rise_motion/rise_motion/sdo_serializer.py | 33 +++++- .../rise_motion/test/test_sdo_serializer.py | 111 +++++++++++++----- 2 files changed, 113 insertions(+), 31 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py index 3030ec6..70053aa 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -25,9 +25,9 @@ class TypeInfo: # Boolean / generic word types 0x0001: TypeInfo(0x0001, "BOOLEAN", "BOOL", 1, "serialize_bool", "deserialize_bool"), - 0x001E: TypeInfo(0x001E, "BYTE", "BYTE", 8, "serialize_uint8", "deserialize_uint8"), - 0x001F: TypeInfo(0x001F, "WORD", "WORD", 16, "serialize_uint16", "deserialize_uint16"), - 0x0020: TypeInfo(0x0020, "DWORD", "DWORD", 32, "serialize_uint32", "deserialize_uint32"), + 0x001E: TypeInfo(0x001E, "BYTE", "BYTE", 8, "serialize_byte", "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_time48", "deserialize_time48"), @@ -232,4 +232,29 @@ def deserialize_uint(ser_val: bytes, bit_s): Deserialize a bytes object into a uint. """ - return int.from_bytes(ser_val, byteorder='little') \ No newline at end of file + return int.from_bytes(ser_val, byteorder='little') + +def serialize_bool(val, bit_s: int) -> bytes: + """ + Serialize a bool into a bytes object. + """ + if type(val) is bool: + val = int(val) + + return serialize_bitn(val, bit_s) + +def deserialize_bool(ser_val: bytes, bit_s): + """ + Deserialize a bytes object into a bool. + """ + + return bool(int.from_bytes(ser_val, byteorder='little')) + +def serialize_byte(val, bit_s: int) -> bytes: + """ + Serialize a byte into a bytes object. + """ + if type(val) is bytes: + return val + + return serialize_bitn(val, bit_s) diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py index 37fcb05..ef5d431 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -10,7 +10,7 @@ # --- round-trip tests --- @pytest.mark.parametrize("bit_width", range(1, 17)) -def test_roundtrip_random(bit_width: int): +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") @@ -19,7 +19,7 @@ def test_roundtrip_random(bit_width: int): @pytest.mark.parametrize("bit_width", range(1, 17)) -def test_roundtrip_min(bit_width: int): +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}" @@ -27,7 +27,7 @@ def test_roundtrip_min(bit_width: int): @pytest.mark.parametrize("bit_width", range(1, 17)) -def test_roundtrip_max(bit_width: int): +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}" @@ -45,7 +45,7 @@ def test_roundtrip_max(bit_width: int): ("0000000000000000", "BIT16", b"\x00\x00"), ("1000000000000000", "BIT16", b"\x00\x80"), # MSB only ]) -def test_serialize_known_values(value: str, dtype: str, expected_bytes: bytes): +def test_serialize_known_values_bitstrings(value: str, dtype: str, expected_bytes: bytes): assert expected_bytes == serialize(value, base_data_type=dtype) @@ -59,38 +59,41 @@ def test_serialize_known_values(value: str, dtype: str, expected_bytes: bytes): (b"\xff\xff", "BIT16", "1111111111111111"), (b"\x00\x00", "BIT16", "0000000000000000"), ]) -def test_deserialize_known_values(raw_bytes: bytes, dtype: str, expected_value: str): +def test_deserialize_known_values_bitstrings(raw_bytes: bytes, dtype: str, expected_value: str): assert expected_value == deserialize(raw_bytes, base_data_type=dtype) # --------------------------------------------------------------------------- -# Bit arrays BITARR8, BITARR16, BITARR32 tests +# BITARR8, BITARR16, BITARR32, BYTE, WORD, DWORD tests # --------------------------------------------------------------------------- # --- round-trip tests --- -@pytest.mark.parametrize("bit_width", [8, 16, 32]) -def test_roundtrip_random(bit_width: int): +@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") - dtype = f"BITARR{bit_width}" assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype) -@pytest.mark.parametrize("bit_width", [8, 16, 32]) -def test_roundtrip_min(bit_width: int): +@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) - dtype = f"BITARR{bit_width}" assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype) -@pytest.mark.parametrize("bit_width", [8, 16, 32]) -def test_roundtrip_max(bit_width: int): +@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 - dtype = f"BITARR{bit_width}" assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype) @@ -103,8 +106,14 @@ def test_roundtrip_max(bit_width: int): ("0011110000101110", "BITARR16", b"\x2e\x3c"), ("10010000111100001010101001101101", "BITARR32", b"\x6d\xaa\xf0\x90"), ("01101111000101011000001111110000", "BITARR32", b"\xf0\x83\x15\x6f"), + ("10101100", "BYTE", b"\xac"), + ("01010101", "BYTE", b"\x55"), + ("1100101001110001", "WORD", b"\x71\xca"), + ("0011110000101110", "WORD", b"\x2e\x3c"), + ("10010000111100001010101001101101", "DWORD", b"\x6d\xaa\xf0\x90"), + ("01101111000101011000001111110000", "DWORD", b"\xf0\x83\x15\x6f"), ]) -def test_serialize_known_values(value: str, dtype: str, expected_bytes: bytes): +def test_serialize_known_values_bitarr_word(value: str, dtype: str, expected_bytes: bytes): assert expected_bytes == serialize(value, base_data_type=dtype) @@ -117,10 +126,22 @@ def test_serialize_known_values(value: str, dtype: str, expected_bytes: bytes): ("0011110000101110", "BITARR16", b"\x2e\x3c"), ("10010000111100001010101001101101", "BITARR32", b"\x6d\xaa\xf0\x90"), ("01101111000101011000001111110000", "BITARR32", b"\xf0\x83\x15\x6f"), + ("10101100", "BYTE", b"\xac"), + ("01010101", "BYTE", b"\x55"), + ("1100101001110001", "WORD", b"\x71\xca"), + ("0011110000101110", "WORD", b"\x2e\x3c"), + ("10010000111100001010101001101101", "DWORD", b"\x6d\xaa\xf0\x90"), + ("01101111000101011000001111110000", "DWORD", b"\xf0\x83\x15\x6f"), ]) -def test_deserialize_known_values(raw_bytes: bytes, dtype: str, expected_value: str): +def test_deserialize_known_values_bitarr_word(raw_bytes: bytes, dtype: str, expected_value: str): assert expected_value == deserialize(raw_bytes, base_data_type=dtype) +# --- bytes passthrough test for serializing BYTE --- + +def test_serializing_byte_passthrough(): + """Serializing a bytes object should return the same bytes object.""" + value = serialize(random.randint(0, (1 << 8)-1), name="UNSIGNED8") + assert value == serialize(value, name="BYTE") # --------------------------------------------------------------------------- # Signed integers 8, 16, 24, 32, 40, 48, 56, 64 tests @@ -132,7 +153,7 @@ def test_deserialize_known_values(raw_bytes: bytes, dtype: str, expected_value: # --- round-trip tests --- @pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64]) -def test_roundtrip_random(bit_width: int): +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)) @@ -141,7 +162,7 @@ def test_roundtrip_random(bit_width: int): @pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64]) -def test_roundtrip_min(bit_width: int): +def test_roundtrip_min_sint(bit_width: int): """Zero survives a round-trip for every integer size.""" value = 0 dtype = f"INTEGER{bit_width}" @@ -149,7 +170,7 @@ def test_roundtrip_min(bit_width: int): @pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64]) -def test_roundtrip_max(bit_width: int): +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 @@ -158,7 +179,7 @@ def test_roundtrip_max(bit_width: int): @pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64]) -def test_roundtrip_max(bit_width: int): +def test_roundtrip_max_sint(bit_width: int): """min-value survives a round-trip for every integer size.""" max_val = 1 << (bit_width-1) value = -max_val @@ -178,7 +199,7 @@ def test_roundtrip_max(bit_width: int): (-(1 << 40), "INTEGER56", b"\x00\x00\x00\x00\x00\xff\xff"), ((1 << 62), "INTEGER64", b"\x00\x00\x00\x00\x00\x00\x00\x40"), ]) -def test_serialize_known_values(value: str, dtype: str, expected_bytes: bytes): +def test_serialize_known_values_sint(value: str, dtype: str, expected_bytes: bytes): assert expected_bytes == serialize(value, name=dtype) @@ -194,7 +215,7 @@ def test_serialize_known_values(value: str, dtype: str, expected_bytes: bytes): (-(1 << 40), "INTEGER56", b"\x00\x00\x00\x00\x00\xff\xff"), ((1 << 62), "INTEGER64", b"\x00\x00\x00\x00\x00\x00\x00\x40"), ]) -def test_deserialize_known_values(raw_bytes: bytes, dtype: str, expected_value: str): +def test_deserialize_known_values_sint(raw_bytes: bytes, dtype: str, expected_value: str): assert expected_value == deserialize(raw_bytes, name=dtype) @@ -205,7 +226,7 @@ def test_deserialize_known_values(raw_bytes: bytes, dtype: str, expected_value: # --- round-trip tests --- @pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64]) -def test_roundtrip_random(bit_width: int): +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) @@ -214,7 +235,7 @@ def test_roundtrip_random(bit_width: int): @pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64]) -def test_roundtrip_min(bit_width: int): +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}" @@ -222,9 +243,45 @@ def test_roundtrip_min(bit_width: int): @pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64]) -def test_roundtrip_max(bit_width: int): +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) \ No newline at end of file + 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 bytes) --- + +@pytest.mark.parametrize("value, dtype, expected_bytes", [ + (False, "BOOL", b"\x00"), + (True, "BOOL", b"\x01"), + (0, "BOOL", b"\x00"), + (1, "BOOL", b"\x01"), +]) +def test_serialize_known_values_bool(value: str, dtype: str, expected_bytes: bytes): + assert expected_bytes == serialize(value, base_data_type=dtype) + + +# --- Explicit deserialization checks (known bytes -> known output) --- + +@pytest.mark.parametrize("expected_value, dtype, raw_bytes", [ + (False, "BOOL", b"\x00"), + (True, "BOOL", b"\x01"), + (0, "BOOL", b"\x00"), + (1, "BOOL", b"\x01"), +]) +def test_deserialize_known_values_bool(raw_bytes: bytes, dtype: str, expected_value: str): + assert expected_value == deserialize(raw_bytes, base_data_type=dtype) \ No newline at end of file From 412a621f2a95f76844808fedacd9e177c4509e2d Mon Sep 17 00:00:00 2001 From: Anton Date: Thu, 28 May 2026 13:52:48 +0200 Subject: [PATCH 21/53] adjust to correct mapping of uint8[] and add float serialization --- .../rise_motion/rise_motion/sdo_serializer.py | 114 ++++---- .../rise_motion/test/test_sdo_serializer.py | 256 ++++++++++-------- 2 files changed, 197 insertions(+), 173 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py index 70053aa..1264ec2 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -1,7 +1,7 @@ from __future__ import annotations from dataclasses import dataclass -from typing import Dict - +from typing import Dict, List +import struct # --------------------------------------------------------------------------- # Record type @@ -25,15 +25,15 @@ class TypeInfo: # Boolean / generic word types 0x0001: TypeInfo(0x0001, "BOOLEAN", "BOOL", 1, "serialize_bool", "deserialize_bool"), - 0x001E: TypeInfo(0x001E, "BYTE", "BYTE", 8, "serialize_byte", "deserialize_bitn"), + 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) + # Time types (48-bit, special structure) 0x000C: TypeInfo(0x000C, "TIME_OF_DAY", "TIME_OF_DAY", 48, "serialize_time48", "deserialize_time48"), 0x000D: TypeInfo(0x000D, "TIME_DIFFERENCE","TIME_DIFFERENCE", 48, "serialize_time48", "deserialize_time48"), - # Bit strings BIT1 - BIT16 + # 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"), @@ -51,12 +51,12 @@ class TypeInfo: 0x003E: TypeInfo(0x003E, "BIT15", "BIT15", 15, "serialize_bitn", "deserialize_bitn"), 0x003F: TypeInfo(0x003F, "BIT16", "BIT16", 16, "serialize_bitn", "deserialize_bitn"), - # Bit arrays + # 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 + # 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"), @@ -66,7 +66,7 @@ class TypeInfo: 0x0014: TypeInfo(0x0014, "INTEGER56", "INT56", 56, "serialize_int", "deserialize_int"), 0x0015: TypeInfo(0x0015, "INTEGER64", "LINT", 64, "serialize_int", "deserialize_int"), - # Unsigned integers + # 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"), @@ -76,11 +76,11 @@ class TypeInfo: 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_float32", "deserialize_float32"), - 0x0011: TypeInfo(0x0011, "REAL64", "LREAL", 64, "serialize_float64", "deserialize_float64"), + # Floating point + 0x0008: TypeInfo(0x0008, "REAL32", "REAL", 32, "serialize_float", "deserialize_float"), + 0x0011: TypeInfo(0x0011, "REAL64", "LREAL", 64, "serialize_float", "deserialize_float"), - # GUID + # GUID 0x001D: TypeInfo(0x001D, "GUID", "GUID", 128, "serialize_guid", "deserialize_guid"), } @@ -139,7 +139,7 @@ def get_type_info( # --------------------------------------------------------------------------- -# Generic functions +# Generic functions # --------------------------------------------------------------------------- def serialize( @@ -148,25 +148,23 @@ def serialize( index: int | None = None, name: str | None = None, base_data_type: str | None = None, -) -> bytes: +) -> List[int]: object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) - # using the specific function for this data type which is stored in the almighty dict return globals()[object_info.serialize_fn](value, object_info.bit_size) def deserialize( - serialized_value: bytes, + serialized_value: List[int], *, index: int | None = None, name: str | None = None, base_data_type: str | None = None, ): - if type(serialized_value) is not bytes: - raise TypeError("serialized_value should be bytes object") + if not isinstance(serialized_value, list) or not all(isinstance(b, int) for b in serialized_value): + raise TypeError("serialized_value must be a list[int]") object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) if (object_info.bit_size + 7) // 8 != len(serialized_value): raise ValueError( "number of bytes needed to contain object_info.bit_size must be equal to serialized_value length") - # using the specific function for this data type which is stored in the almighty dict return globals()[object_info.deserialize_fn](serialized_value, object_info.bit_size) @@ -174,87 +172,75 @@ def deserialize( # Specific functions (called by generic functions) # --------------------------------------------------------------------------- -def serialize_bitn(val, bit_s: int) -> bytes: +def serialize_bitn(val, bit_s: int) -> List[int]: """ - Serialize a bit-string into a bytes object. + Serialize a bit-string into a list[int]. """ if type(val) is str: vali = int(val, 2) elif type(val) is int: vali = val else: - raise TypeError(f"val length must be either int or str, not {type(val)}") + raise TypeError(f"val must be either int or str, not {type(val)}") - # Number of bytes required byte_len = (bit_s + 7) // 8 + return list(vali.to_bytes(byte_len, byteorder="little")) - return vali.to_bytes(byte_len, byteorder="little") - -def deserialize_bitn(ser_val: bytes, bit_s): +def deserialize_bitn(ser_val: List[int], bit_s: int) -> str: """ - Deserialize a bytes object into a bit-string. + Deserialize a list[int] into a bit-string. """ + value = int.from_bytes(bytes(ser_val), byteorder='little') + return bin(value)[2:].zfill(bit_s) - value = int.from_bytes(ser_val, byteorder='little') - bit_string = bin(value)[2:] - bit_string = bit_string.zfill(bit_s) - return bit_string - -def serialize_int(val: int, bit_s: int) -> bytes: +def serialize_int(val: int, bit_s: int) -> List[int]: """ - Serialize an int into a bytes object. + Serialize a signed int into a list[int]. """ - - # Number of bytes required byte_len = (bit_s + 7) // 8 + return list(val.to_bytes(byte_len, byteorder="little", signed=True)) - return val.to_bytes(byte_len, byteorder="little", signed=True) - -def deserialize_int(ser_val: bytes, bit_s): +def deserialize_int(ser_val: List[int], bit_s: int) -> int: """ - Deserialize a bytes object into an int. + Deserialize a list[int] into a signed int. """ - - return int.from_bytes(ser_val, byteorder='little', signed=True) + return int.from_bytes(bytes(ser_val), byteorder='little', signed=True) -def serialize_uint(val: int, bit_s: int) -> bytes: +def serialize_uint(val: int, bit_s: int) -> List[int]: """ - Serialize a uint into a bytes object. + Serialize an unsigned int into a list[int]. """ - - # Number of bytes required byte_len = (bit_s + 7) // 8 + return list(val.to_bytes(byte_len, byteorder="little")) - return val.to_bytes(byte_len, byteorder="little") - -def deserialize_uint(ser_val: bytes, bit_s): +def deserialize_uint(ser_val: List[int], bit_s: int) -> int: """ - Deserialize a bytes object into a uint. + Deserialize a list[int] into an unsigned int. """ - - return int.from_bytes(ser_val, byteorder='little') + return int.from_bytes(bytes(ser_val), byteorder='little') -def serialize_bool(val, bit_s: int) -> bytes: +def serialize_bool(val, bit_s: int) -> List[int]: """ - Serialize a bool into a bytes object. + Serialize a bool into a list[int]. """ if type(val) is bool: val = int(val) - return serialize_bitn(val, bit_s) -def deserialize_bool(ser_val: bytes, bit_s): +def deserialize_bool(ser_val: List[int], bit_s: int) -> bool: """ - Deserialize a bytes object into a bool. + Deserialize a list[int] into a bool. """ - - return bool(int.from_bytes(ser_val, byteorder='little')) + return bool(int.from_bytes(bytes(ser_val), byteorder='little')) -def serialize_byte(val, bit_s: int) -> bytes: +def serialize_float(val, bit_s: int) -> List[int]: """ - Serialize a byte into a bytes object. + Serialize a float into a list[int]. """ - if type(val) is bytes: - return val + return list(struct.pack(f"<{'f' if bit_s == 32 else 'd'}", val)) - return serialize_bitn(val, bit_s) +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] \ No newline at end of file diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py index ef5d431..ea5e12c 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -2,6 +2,8 @@ import random import pytest from rise_motion.sdo_serializer import serialize, deserialize +from math import isnan +from typing import List # --------------------------------------------------------------------------- # Bit strings BIT1 - BIT16 tests @@ -34,33 +36,33 @@ def test_roundtrip_max_bitstrings(bit_width: int): assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype) -# --- Explicit serialization checks (known input -> known bytes) --- +# --- Explicit serialization checks (known input -> known list[int]) --- -@pytest.mark.parametrize("value, dtype, expected_bytes", [ - ("0", "BIT1", b"\x00"), - ("1", "BIT1", b"\x01"), - ("11", "BIT2", b"\x03"), - ("10", "BIT2", b"\x02"), - ("1111111111111111", "BIT16", b"\xff\xff"), - ("0000000000000000", "BIT16", b"\x00\x00"), - ("1000000000000000", "BIT16", b"\x00\x80"), # MSB only +@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_bytes: bytes): - assert expected_bytes == serialize(value, base_data_type=dtype) +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 bytes -> known output) --- +# --- Explicit deserialization checks (known list[int] -> known output) --- -@pytest.mark.parametrize("raw_bytes, dtype, expected_value", [ - (b"\x00", "BIT1", "0"), - (b"\x01", "BIT1", "1"), - (b"\x03", "BIT2", "11"), - (b"\x02", "BIT2", "10"), - (b"\xff\xff", "BIT16", "1111111111111111"), - (b"\x00\x00", "BIT16", "0000000000000000"), +@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(raw_bytes: bytes, dtype: str, expected_value: str): - assert expected_value == deserialize(raw_bytes, base_data_type=dtype) +def test_deserialize_known_values_bitstrings(serialized: List[int], dtype: str, expected_value: str): + assert expected_value == deserialize(serialized, base_data_type=dtype) # --------------------------------------------------------------------------- @@ -97,58 +99,49 @@ def test_roundtrip_max_bitarr_word(bit_width: int, dtype: str): assert value == deserialize(serialize(value, base_data_type=dtype), base_data_type=dtype) -# --- Explicit serialization checks (known input -> known bytes) --- - -@pytest.mark.parametrize("value, dtype, expected_bytes", [ - ("10101100", "BITARR8", b"\xac"), - ("01010101", "BITARR8", b"\x55"), - ("1100101001110001", "BITARR16", b"\x71\xca"), - ("0011110000101110", "BITARR16", b"\x2e\x3c"), - ("10010000111100001010101001101101", "BITARR32", b"\x6d\xaa\xf0\x90"), - ("01101111000101011000001111110000", "BITARR32", b"\xf0\x83\x15\x6f"), - ("10101100", "BYTE", b"\xac"), - ("01010101", "BYTE", b"\x55"), - ("1100101001110001", "WORD", b"\x71\xca"), - ("0011110000101110", "WORD", b"\x2e\x3c"), - ("10010000111100001010101001101101", "DWORD", b"\x6d\xaa\xf0\x90"), - ("01101111000101011000001111110000", "DWORD", b"\xf0\x83\x15\x6f"), +# --- 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_bytes: bytes): - assert expected_bytes == serialize(value, base_data_type=dtype) - - -# --- Explicit deserialization checks (known bytes -> known output) --- - -@pytest.mark.parametrize("expected_value, dtype, raw_bytes", [ - ("10101100", "BITARR8", b"\xac"), - ("01010101", "BITARR8", b"\x55"), - ("1100101001110001", "BITARR16", b"\x71\xca"), - ("0011110000101110", "BITARR16", b"\x2e\x3c"), - ("10010000111100001010101001101101", "BITARR32", b"\x6d\xaa\xf0\x90"), - ("01101111000101011000001111110000", "BITARR32", b"\xf0\x83\x15\x6f"), - ("10101100", "BYTE", b"\xac"), - ("01010101", "BYTE", b"\x55"), - ("1100101001110001", "WORD", b"\x71\xca"), - ("0011110000101110", "WORD", b"\x2e\x3c"), - ("10010000111100001010101001101101", "DWORD", b"\x6d\xaa\xf0\x90"), - ("01101111000101011000001111110000", "DWORD", b"\xf0\x83\x15\x6f"), +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(raw_bytes: bytes, dtype: str, expected_value: str): - assert expected_value == deserialize(raw_bytes, base_data_type=dtype) +def test_deserialize_known_values_bitarr_word(serialized: List[int], dtype: str, expected_value: str): + assert expected_value == deserialize(serialized, base_data_type=dtype) -# --- bytes passthrough test for serializing BYTE --- - -def test_serializing_byte_passthrough(): - """Serializing a bytes object should return the same bytes object.""" - value = serialize(random.randint(0, (1 << 8)-1), name="UNSIGNED8") - assert value == serialize(value, name="BYTE") # --------------------------------------------------------------------------- # Signed integers 8, 16, 24, 32, 40, 48, 56, 64 tests # --------------------------------------------------------------------------- -# These tests could be overkill for such a simple function, -# because I wrote them using the tests for the datatypes above as a template. -# Maybe they will help should the serialization for signed ints get more elaborate. # --- round-trip tests --- @@ -162,7 +155,7 @@ def test_roundtrip_random_sint(bit_width: int): @pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64]) -def test_roundtrip_min_sint(bit_width: int): +def test_roundtrip_zero_sint(bit_width: int): """Zero survives a round-trip for every integer size.""" value = 0 dtype = f"INTEGER{bit_width}" @@ -179,7 +172,7 @@ def test_roundtrip_max_sint(bit_width: int): @pytest.mark.parametrize("bit_width", [8, 16, 24, 32, 40, 48, 56, 64]) -def test_roundtrip_max_sint(bit_width: int): +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 @@ -187,36 +180,36 @@ def test_roundtrip_max_sint(bit_width: int): assert value == deserialize(serialize(value, name=dtype), name=dtype) -# --- Explicit serialization checks (known input -> known bytes) --- +# --- Explicit serialization checks (known input -> known list[int]) --- -@pytest.mark.parametrize("value, dtype, expected_bytes", [ - (-(1 << 3), "INTEGER8", b"\xf8"), - ((1 << 12), "INTEGER16", b"\x00\x10"), - (-(1 << 22), "INTEGER24", b"\x00\x00\xc0"), - ((1 << 2), "INTEGER32", b"\x04\x00\x00\x00"), - (-(1 << 20), "INTEGER40", b"\x00\x00\xf0\xff\xff"), - ((1 << 40), "INTEGER48", b"\x00\x00\x00\x00\x00\x01"), - (-(1 << 40), "INTEGER56", b"\x00\x00\x00\x00\x00\xff\xff"), - ((1 << 62), "INTEGER64", b"\x00\x00\x00\x00\x00\x00\x00\x40"), +@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: str, dtype: str, expected_bytes: bytes): - assert expected_bytes == serialize(value, name=dtype) +def test_serialize_known_values_sint(value: int, dtype: str, expected: List[int]): + assert expected == serialize(value, name=dtype) -# --- Explicit deserialization checks (known bytes -> known output) --- +# --- Explicit deserialization checks (known list[int] -> known output) --- -@pytest.mark.parametrize("expected_value, dtype, raw_bytes", [ - (-(1 << 3), "INTEGER8", b"\xf8"), - ((1 << 12), "INTEGER16", b"\x00\x10"), - (-(1 << 22), "INTEGER24", b"\x00\x00\xc0"), - ((1 << 2), "INTEGER32", b"\x04\x00\x00\x00"), - (-(1 << 20), "INTEGER40", b"\x00\x00\xf0\xff\xff"), - ((1 << 40), "INTEGER48", b"\x00\x00\x00\x00\x00\x01"), - (-(1 << 40), "INTEGER56", b"\x00\x00\x00\x00\x00\xff\xff"), - ((1 << 62), "INTEGER64", b"\x00\x00\x00\x00\x00\x00\x00\x40"), +@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(raw_bytes: bytes, dtype: str, expected_value: str): - assert expected_value == deserialize(raw_bytes, name=dtype) +def test_deserialize_known_values_sint(serialized: List[int], dtype: str, expected_value: int): + assert expected_value == deserialize(serialized, name=dtype) # --------------------------------------------------------------------------- @@ -250,7 +243,7 @@ def test_roundtrip_max_uint(bit_width: int): dtype = f"UNSIGNED{bit_width}" assert value == deserialize(serialize(value, name=dtype), name=dtype) - + # --------------------------------------------------------------------------- # Bool tests # --------------------------------------------------------------------------- @@ -263,25 +256,70 @@ def test_roundtrip_random_bool(): assert value == deserialize(serialize(value, base_data_type="BOOL"), base_data_type="BOOL") -# --- Explicit serialization checks (known input -> known bytes) --- +# --- Explicit serialization checks (known input -> known list[int]) --- -@pytest.mark.parametrize("value, dtype, expected_bytes", [ - (False, "BOOL", b"\x00"), - (True, "BOOL", b"\x01"), - (0, "BOOL", b"\x00"), - (1, "BOOL", b"\x01"), +@pytest.mark.parametrize("value, dtype, expected", [ + (False, "BOOL", [0x00]), + (True, "BOOL", [0x01]), + (0, "BOOL", [0x00]), + (1, "BOOL", [0x01]), ]) -def test_serialize_known_values_bool(value: str, dtype: str, expected_bytes: bytes): - assert expected_bytes == serialize(value, base_data_type=dtype) +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) -# --- Explicit deserialization checks (known bytes -> known output) --- -@pytest.mark.parametrize("expected_value, dtype, raw_bytes", [ - (False, "BOOL", b"\x00"), - (True, "BOOL", b"\x01"), - (0, "BOOL", b"\x00"), - (1, "BOOL", b"\x01"), +@pytest.mark.parametrize(("dtype", "value"), [ + ("REAL32", float("inf")), + ("REAL32", float("-inf")), + ("REAL64", float("inf")), + ("REAL64", float("-inf")), ]) -def test_deserialize_known_values_bool(raw_bytes: bytes, dtype: str, expected_value: str): - assert expected_value == deserialize(raw_bytes, base_data_type=dtype) \ No newline at end of file +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) \ No newline at end of file From 6ea6cf44ffa0dcb1924bc47664d17f8ff4b04c6d Mon Sep 17 00:00:00 2001 From: Anton Date: Thu, 28 May 2026 19:07:59 +0200 Subject: [PATCH 22/53] GUID and time48 (de)serialization pass pytest --- .../rise_motion/rise_motion/sdo_serializer.py | 75 ++++++++++++---- .../rise_motion/test/test_sdo_serializer.py | 86 ++++++++++++++++--- 2 files changed, 130 insertions(+), 31 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py index 1264ec2..1b0722e 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -1,7 +1,6 @@ -from __future__ import annotations from dataclasses import dataclass -from typing import Dict, List import struct +import uuid # --------------------------------------------------------------------------- # Record type @@ -21,7 +20,7 @@ class TypeInfo: # Master table (keyed by object dictionary index) # --------------------------------------------------------------------------- -BASE_DATA_TYPES: Dict[int, TypeInfo] = { +BASE_DATA_TYPES: dict[int, TypeInfo] = { # Boolean / generic word types 0x0001: TypeInfo(0x0001, "BOOLEAN", "BOOL", 1, "serialize_bool", "deserialize_bool"), @@ -89,11 +88,11 @@ class TypeInfo: # Secondary lookup dicts (built once at import time) # --------------------------------------------------------------------------- -BY_NAME: Dict[str, TypeInfo] = { +BY_NAME: dict[str, TypeInfo] = { info.name: info for info in BASE_DATA_TYPES.values() } -BY_BASE_DATA_TYPE: Dict[str, TypeInfo] = { +BY_BASE_DATA_TYPE: dict[str, TypeInfo] = { info.base_data_type: info for info in BASE_DATA_TYPES.values() } @@ -148,19 +147,20 @@ def serialize( index: int | None = None, name: str | None = None, base_data_type: str | None = None, -) -> List[int]: +) -> list[int]: object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) return globals()[object_info.serialize_fn](value, object_info.bit_size) def deserialize( - serialized_value: List[int], + serialized_value: list[int], *, index: int | None = None, name: str | None = None, base_data_type: str | None = None, ): if not isinstance(serialized_value, list) or not all(isinstance(b, int) for b in serialized_value): - raise TypeError("serialized_value must be a list[int]") + error_msg = f"serialized_value must be a list[int] not {type(serialized_value)}" + raise TypeError(error_msg) object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) if (object_info.bit_size + 7) // 8 != len(serialized_value): raise ValueError( @@ -172,7 +172,7 @@ def deserialize( # Specific functions (called by generic functions) # --------------------------------------------------------------------------- -def serialize_bitn(val, bit_s: int) -> List[int]: +def serialize_bitn(val, bit_s: int) -> list[int]: """ Serialize a bit-string into a list[int]. """ @@ -186,40 +186,40 @@ def serialize_bitn(val, bit_s: int) -> List[int]: byte_len = (bit_s + 7) // 8 return list(vali.to_bytes(byte_len, byteorder="little")) -def deserialize_bitn(ser_val: List[int], bit_s: int) -> str: +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]: +def serialize_int(val: int, bit_s: int) -> list[int]: """ Serialize a signed int into a list[int]. """ 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: +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]: +def serialize_uint(val: int, bit_s: int) -> list[int]: """ Serialize an unsigned int into a list[int]. """ 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: +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]: +def serialize_bool(val, bit_s: int) -> list[int]: """ Serialize a bool into a list[int]. """ @@ -227,20 +227,57 @@ def serialize_bool(val, bit_s: int) -> List[int]: val = int(val) return serialize_bitn(val, bit_s) -def deserialize_bool(ser_val: List[int], bit_s: int) -> bool: +def deserialize_bool(ser_val: list[int], bit_s: int) -> bool: """ Deserialize a list[int] into a bool. """ return bool(int.from_bytes(bytes(ser_val), byteorder='little')) -def serialize_float(val, bit_s: int) -> List[int]: +def serialize_float(val, bit_s: int) -> list[int]: """ Serialize a float into a list[int]. """ return list(struct.pack(f"<{'f' if bit_s == 32 else 'd'}", val)) -def deserialize_float(ser_val: List[int], bit_s: int) -> float: +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] \ No newline at end of file + return struct.unpack(f"<{'f' if bit_s == 32 else 'd'}", bytes(ser_val))[0] + +def serialize_time48(val, bit_s: int) -> list[int]: + """ + Takes either an int or str of bits describing the whole time48 struct + or a tuple containing ms and days + """ + if not isinstance(val, (str, int)): + ms = format(val[0], '028b') # UNSIGNED28 ms + void = "0000" # VOID4 reserved + days = format(val[1], '016b') # UNSIGNED16 days + val = ms+void+days + + return serialize_bitn(val, bit_s) + +def deserialize_time48(ser_val: list[int], bit_s: int) -> tuple[int]: + """ + Takes list[int] and returns a tuple containing ms and days + """ + bits = deserialize_bitn(ser_val, bit_s) + return (int(bits[0:28], base=2), int(bits[32:48], base=2)) + + +def serialize_guid(val, bit_s: int) -> list[int]: + """ + Serialize a UUID into a list[int]. + Accepts a uuid.UUID object or a GUID string e.g. "550e8400-e29b-41d4-a716-446655440000". + """ + if isinstance(val, str): + val = uuid.UUID(val) + return list(val.bytes_le) + + +def deserialize_guid(ser_val: list[int], bit_s: int) -> uuid.UUID: + """ + Deserialize a list[int] into a uuid.UUID. + """ + return uuid.UUID(bytes_le=bytes(ser_val)) \ No newline at end of file diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py index ea5e12c..f15fd5b 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -1,9 +1,9 @@ """Tests for sdo_serializer.py module.""" -import random import pytest -from rise_motion.sdo_serializer import serialize, deserialize +import random from math import isnan -from typing import List +import uuid +from rise_motion.sdo_serializer import serialize, deserialize # --------------------------------------------------------------------------- # Bit strings BIT1 - BIT16 tests @@ -47,7 +47,7 @@ def test_roundtrip_max_bitstrings(bit_width: int): ("0000000000000000", "BIT16", [0x00, 0x00]), ("1000000000000000", "BIT16", [0x00, 0x80]), # MSB only ]) -def test_serialize_known_values_bitstrings(value: str, dtype: str, expected: List[int]): +def test_serialize_known_values_bitstrings(value: str, dtype: str, expected: list[int]): assert expected == serialize(value, base_data_type=dtype) @@ -61,7 +61,7 @@ def test_serialize_known_values_bitstrings(value: str, dtype: str, expected: Lis ([0xff, 0xff], "BIT16", "1111111111111111"), ([0x00, 0x00], "BIT16", "0000000000000000"), ]) -def test_deserialize_known_values_bitstrings(serialized: List[int], dtype: str, expected_value: str): +def test_deserialize_known_values_bitstrings(serialized: list[int], dtype: str, expected_value: str): assert expected_value == deserialize(serialized, base_data_type=dtype) @@ -115,7 +115,7 @@ def test_roundtrip_max_bitarr_word(bit_width: int, dtype: str): ("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]): +def test_serialize_known_values_bitarr_word(value: str, dtype: str, expected: list[int]): assert expected == serialize(value, base_data_type=dtype) @@ -135,7 +135,7 @@ def test_serialize_known_values_bitarr_word(value: str, dtype: str, expected: Li ([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): +def test_deserialize_known_values_bitarr_word(serialized: list[int], dtype: str, expected_value: str): assert expected_value == deserialize(serialized, base_data_type=dtype) @@ -192,7 +192,7 @@ def test_roundtrip_min_sint(bit_width: int): (-(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]): +def test_serialize_known_values_sint(value: int, dtype: str, expected: list[int]): assert expected == serialize(value, name=dtype) @@ -208,7 +208,7 @@ def test_serialize_known_values_sint(value: int, dtype: str, expected: List[int] ([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): +def test_deserialize_known_values_sint(serialized: list[int], dtype: str, expected_value: int): assert expected_value == deserialize(serialized, name=dtype) @@ -264,7 +264,7 @@ def test_roundtrip_random_bool(): (0, "BOOL", [0x00]), (1, "BOOL", [0x01]), ]) -def test_serialize_known_values_bool(value, dtype: str, expected: List[int]): +def test_serialize_known_values_bool(value, dtype: str, expected: list[int]): assert expected == serialize(value, base_data_type=dtype) @@ -276,7 +276,7 @@ def test_serialize_known_values_bool(value, dtype: str, expected: List[int]): ([0x00], "BOOL", 0), ([0x01], "BOOL", 1), ]) -def test_deserialize_known_values_bool(serialized: List[int], dtype: str, expected_value): +def test_deserialize_known_values_bool(serialized: list[int], dtype: str, expected_value): assert expected_value == deserialize(serialized, base_data_type=dtype) @@ -322,4 +322,66 @@ 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) \ No newline at end of file + 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", [0x88, 0x13, 0x80, 0x38, 0x01, 0x00]), + ((1000, 1000), "TIME_DIFFERENCE", [0xE8, 0x03, 0x80, 0x3E, 0x00, 0x00]), +]) +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", [ + ([0x88, 0x13, 0x80, 0x38, 0x01, 0x00], "TIME_OF_DAY", (5000, 5000)), + ([0xE8, 0x03, 0x80, 0x3E, 0x00, 0x00], "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) + + +# --------------------------------------------------------------------------- +# TIME_OF_DAY, TIME_DIFFERENCE tests +# --------------------------------------------------------------------------- + +# --- round-trip test --- + +def test_roundtrip_random_guid(): + """Serializing then deserializing a random valid value returns the original.""" + value = uuid.uuid4() + assert value == deserialize(serialize(value, base_data_type="GUID"), base_data_type="GUID") \ No newline at end of file From 0d715f4dbc1efaae8179f13a5da990ae20497031 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Fri, 29 May 2026 13:44:53 +0000 Subject: [PATCH 23/53] testing_node.cpp: add provisional sdo request --- .../src/rise_motion/src/testing_node.cpp | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp index d7c51e7..05f20bc 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp @@ -2,11 +2,14 @@ #include #include #include +#include #include #include #include #include #include +#include +#include class TestNode : public rclcpp::Node { @@ -113,6 +116,25 @@ int main(int argc, char **argv) { while (!node->request_enable_ethercat()) { } + + rclcpp::Client::SharedPtr + sdo_client; + sdo_client = node->create_client( + "sdo_read"); + auto request = std::make_shared(); + request->device_id = 1; + request->index = 0x1008; + request->subindex = 0; + request->value_type = 0; + auto result = sdo_client->async_send_request(request); + // wait for result + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sdo_value: %s", (std::string(sdo::deserialize>(result.get()->value))).std::string::c_str()); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service sdo_read"); + } rclcpp::spin(node); rclcpp::shutdown(); return 0; From 9a9e08c26560592726552eaac9c1d0874fa0db28 Mon Sep 17 00:00:00 2001 From: Anton Date: Sat, 30 May 2026 00:39:24 +0200 Subject: [PATCH 24/53] string (de)serialization passes pytest --- .../rise_motion/rise_motion/sdo_serializer.py | 93 ++++++++++-- .../rise_motion/test/test_sdo_serializer.py | 134 +++++++++++++++++- 2 files changed, 217 insertions(+), 10 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py index 1b0722e..759f386 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -81,6 +81,25 @@ class TypeInfo: # GUID 0x001D: TypeInfo(0x001D, "GUID", "GUID", 128, "serialize_guid", "deserialize_guid"), + + # - 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) } @@ -149,7 +168,16 @@ def serialize( base_data_type: str | None = None, ) -> list[int]: object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) - return globals()[object_info.serialize_fn](value, object_info.bit_size) + + # serialize a base data type + if object_info.name[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 def deserialize( serialized_value: list[int], @@ -162,11 +190,20 @@ def deserialize( error_msg = f"serialized_value must be a list[int] not {type(serialized_value)}" raise TypeError(error_msg) object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) - if (object_info.bit_size + 7) // 8 != len(serialized_value): - raise ValueError( - "number of bytes needed to contain object_info.bit_size must be equal to serialized_value length") - return globals()[object_info.deserialize_fn](serialized_value, object_info.bit_size) - + byte_len = (object_info.bit_size + 7) // 8 + + # deserialize a serialized base data type + if object_info.name[0:5] != "ARRAY": + if byte_len != len(serialized_value) and object_info.name[-6:] != "STRING": + raise ValueError( + f"number of bytes needed to contain object_info.bit_size must be equal to serialized_value length") + return globals()[object_info.deserialize_fn](serialized_value, object_info.bit_size) + # deserialize a serialized base data type list (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 # --------------------------------------------------------------------------- # Specific functions (called by generic functions) @@ -265,7 +302,6 @@ def deserialize_time48(ser_val: list[int], bit_s: int) -> tuple[int]: bits = deserialize_bitn(ser_val, bit_s) return (int(bits[0:28], base=2), int(bits[32:48], base=2)) - def serialize_guid(val, bit_s: int) -> list[int]: """ Serialize a UUID into a list[int]. @@ -275,9 +311,48 @@ def serialize_guid(val, bit_s: int) -> list[int]: val = uuid.UUID(val) return list(val.bytes_le) - def deserialize_guid(ser_val: list[int], bit_s: int) -> uuid.UUID: """ Deserialize a list[int] into a uuid.UUID. """ - return uuid.UUID(bytes_le=bytes(ser_val)) \ No newline at end of file + return uuid.UUID(bytes_le=bytes(ser_val)) + +def serialize_visible_string(val: str, bit_s: int) -> list[int]: + return list(val.encode("ASCII")) + +def deserialize_visible_string(ser_val: list[int], bit_s: int) -> str: + return bytes(ser_val).decode("ASCII") + +def serialize_unicode_string(val: str, bit_s: int) -> list[int]: + return list(val.encode("utf_16_le")) + +def deserialize_unicode_string(ser_val: list[int], bit_s: int) -> str: + return bytes(ser_val).decode("utf_16_le") + +# def serialize_bitn_field(val: list, bit_s: int) -> list[int]: +# object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) +# return globals()[object_info.serialize_fn](value, object_info.bit_size) +# out = [] +# for bitn in val: +# out.extend(serialize_bitn(bitn, bit_s)) +# return out + +# def deserialize_bitn_field(ser_val: list[int], bit_s: int) -> str: +# byte_len = (bit_s + 7) // 8 +# out = "" +# for i in range(0, len, ser_val, byte_len): +# out+deserialize_bitn(list[i:i+byte_len], bit_s) +# return out + +# def serialize_uint_field(val: list, bit_s: int) -> list[int]: +# out = [] +# for bitn in val: +# out.extend(serialize_bitn(bitn, bit_s)) +# return out + +# def deserialize_uint_field(ser_val: list[int], bit_s: int) -> str: +# byte_len = (bit_s + 7) // 8 +# out = "" +# for i in range(0, len, ser_val, byte_len): +# out+deserialize_bitn(list[i:i+byte_len], bit_s) +# return out diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py index f15fd5b..773ecf3 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -384,4 +384,136 @@ def test_deserialize_known_values_time48(serialized: list[int], dtype: str, expe def test_roundtrip_random_guid(): """Serializing then deserializing a random valid value returns the original.""" value = uuid.uuid4() - assert value == deserialize(serialize(value, base_data_type="GUID"), base_data_type="GUID") \ No newline at end of file + 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"), +]) +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", [ + "café", # non-ASCII (é is > 0x7F, invalid for VISIBLE_STRING) + "日本語", # CJK characters + "😀", # emoji +]) +def test_serialize_visible_string_rejects_non_ascii(value: str): + """Characters outside the visible ASCII range should raise.""" + with pytest.raises((ValueError, UnicodeEncodeError)): + 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.""" + with pytest.raises((ValueError, UnicodeDecodeError)): + deserialize([0x41], name="UNICODE_STRING") + + From 3af01930ace6cd6b8efeb6fa509d5299e35cb4ee Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Fri, 29 May 2026 23:57:14 +0000 Subject: [PATCH 25/53] rework handling of runtime errors --- .../include/rise_motion/result.hpp | 30 + .../include/rise_motion/sdo_serializer.hpp | 534 +++++++++++------- .../rise_motion/test/test_sdo_serializer.cpp | 380 ++++++++----- 3 files changed, 617 insertions(+), 327 deletions(-) create mode 100644 rise_motion_dev_ws/src/rise_motion/include/rise_motion/result.hpp 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 index 73d8af2..eaae677 100644 --- 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 @@ -10,14 +10,31 @@ #include #include #include +#include "result.hpp" namespace sdo { + enum class ErrorCode + { + None, + InvalidSize, + OutOfRange + }; + + 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 @@ -25,18 +42,18 @@ namespace sdo { 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) - { - if (v < -pow(2, 23) || v > pow(2, 23)-1){ - throw std::out_of_range("Can not convert int32_t value exceeding 24 bit signed range into Int24"); - } - } + {} operator std::int32_t() const { @@ -48,12 +65,10 @@ namespace sdo { std::int64_t value; + Int40() = default; + Int40(std::int64_t v) : value(v) - { - if (v < -pow(2, 39) || v > pow(2, 39)-1){ - throw std::out_of_range("Can not convert int64_t value exceeding 40 bit signed range into Int40"); - } - } + {} operator std::int64_t() const { @@ -65,12 +80,10 @@ namespace sdo { std::int64_t value; + Int48() = default; + Int48(std::int64_t v) : value(v) - { - if (v < -pow(2, 47) || v > pow(2, 47)-1){ - throw std::out_of_range("Can not convert int64_t value exceeding 48 bit signed range into Int48"); - } - } + {} operator std::int64_t() const { @@ -82,12 +95,10 @@ namespace sdo { std::int64_t value; + Int56() = default; + Int56(std::int64_t v) : value(v) - { - if (v < -pow(2, 55) || v > pow(2, 55)-1){ - throw std::out_of_range("Can not convert int64_t value exceeding 56 bit signed range into Int56"); - } - } + {} operator std::int64_t() const { @@ -99,12 +110,10 @@ namespace sdo { std::uint32_t value; + UInt24() = default; + UInt24(std::uint32_t v) : value(v) - { - if (v > pow(2, 24)-1){ - throw std::out_of_range("Can not convert uint32_t value exceeding 24 bit unsigned range into UInt24"); - } - } + {} operator std::uint32_t() const { @@ -116,12 +125,10 @@ namespace sdo { std::uint64_t value; + UInt40() = default; + UInt40(std::uint64_t v) : value(v) - { - if (v > pow(2, 40)-1){ - throw std::out_of_range("Can not convert uint64_t value exceeding 40 bit unsigned range into UInt40"); - } - } + {} operator std::uint64_t() const { @@ -133,12 +140,10 @@ namespace sdo { std::uint64_t value; + UInt48() = default; + UInt48(std::uint64_t v) : value(v) - { - if (v > pow(2, 48)-1){ - throw std::out_of_range("Can not convert uint64_t value exceeding 48 bit unsigned range into UInt48"); - } - } + {} operator std::uint64_t() const { @@ -150,12 +155,10 @@ namespace sdo { std::uint64_t value; + UInt56() = default; + UInt56(std::uint64_t v) : value(v) - { - if (v > pow(2, 56)-1){ - throw std::out_of_range("Can not convert uint64_t value exceeding 56 bit unsigned range into UInt56"); - } - } + {} operator std::uint64_t() const { @@ -183,11 +186,7 @@ namespace sdo STRING() = default; STRING(std::string v) : value(std::move(v)) - { - if (value.size() > T){ - throw std::out_of_range("STRING: string is too long to be coverted to STRING of size T"); - } - }; + {} operator std::string() const { @@ -196,6 +195,9 @@ namespace sdo }; + template using DeserializeResult = rise::Result; + using SerializeResult = rise::Result, sdo::Error>; + // IEC 61131-3 data types using BOOL = bool; @@ -265,15 +267,20 @@ namespace sdo::helpers static constexpr std::size_t size = T; }; - inline void check_size(const std::vector& blob, std::size_t expectedSize, const char* type) + inline std::optional check_size( + const std::vector& blob, std::size_t expectedSize, const char* type) { if (blob.size() != expectedSize){ - throw std::invalid_argument(std::string("deserialize<") + type + ">: expected " + - std::to_string(expectedSize) + " bytes, got " + std::to_string(blob.size())); + 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 T to_raw( + 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"); @@ -281,12 +288,14 @@ namespace sdo::helpers if (numBytes > sizeof(T)) { - throw std::invalid_argument("to_raw: numBytes does not fit T"); + return DeserializeResult::err({ + ErrorCode::InvalidSize, "to_raw: numBytes does not fit T"}); } if (offset + numBytes > blob.size()) { - throw std::out_of_range("to_raw: blob has less bytes than numBytes"); + return DeserializeResult::err({ + ErrorCode::InvalidSize, "to_raw: blob has less bytes than numBytes"}); } T raw = 0; @@ -296,16 +305,17 @@ namespace sdo::helpers raw |= static_cast(blob[offset + i]) << (8 * i); } - return raw; + return DeserializeResult::ok(raw); } - template [[nodiscard]] inline std::vector from_raw(const T& raw, std::size_t numBytes) + 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)) { - throw std::invalid_argument("from_raw: numBytes does not fit T"); + SerializeResult::err({ErrorCode::InvalidSize, "from_raw: numBytes does not fit T"}); } std::vector blob; @@ -316,7 +326,7 @@ namespace sdo::helpers blob.push_back(static_cast((raw >> (8 * i)) & 0xFF)); } - return blob; + return SerializeResult::ok(blob); } } @@ -324,14 +334,17 @@ namespace sdo { // ---DESERIALIZATION--- - template T deserialize(const std::vector& blob) + template [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { if constexpr (sdo::helpers::is_string_type>>::value){ constexpr std::size_t size = sdo::helpers::string_size::size; if (blob.size() > size){ - throw std::invalid_argument("deserialize>: blob is larger than the expected size T"); + return DeserializeResult::err({ + ErrorCode::InvalidSize, + "deserialize>: blob is larger than the expected size T"}); } T string{}; @@ -343,7 +356,7 @@ namespace sdo string.value.pop_back(); } - return string; + return DeserializeResult::ok(string); } else{ static_assert(sdo::helpers::always_false, "deserialize: unsupported type"); @@ -352,235 +365,371 @@ namespace sdo // -Boolean- - template <> [[nodiscard]] inline bool deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 1, "bool"); + if (auto error = sdo::helpers::check_size(blob, 1, "bool")){ + return sdo::DeserializeResult::err(*error); + } - return blob[0] != 0; + return DeserializeResult::ok(blob[0] != 0); } // -Signed Integer- - template <> [[nodiscard]] inline std::int8_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 1, "int8_t"); + if (auto error = sdo::helpers::check_size(blob, 1, "int8_t")){ + return sdo::DeserializeResult::err(*error); + } - return static_cast(blob[0]); + return DeserializeResult::ok(static_cast(blob[0])); } - template <> [[nodiscard]] inline std::int16_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 2, "int16_t"); + 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); + } - std::int16_t value = static_cast(sdo::helpers::to_raw(blob, 2)); + auto result = static_cast(raw.value); - return value; + return DeserializeResult::ok(result); } - template <> [[nodiscard]] inline std::int32_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 4, "int32_t"); + 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); + } - std::int32_t value = static_cast(sdo::helpers::to_raw(blob, 4)); + auto result = static_cast(raw.value); - return value; + return DeserializeResult::ok(result); } // -Unsigned Integer / raw data / bit arrays / bit strings- // use for UNSIGNED8, BYTE, BITARR8, BIT1-BIT8 - template <> [[nodiscard]] inline std::uint8_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 1, "uint8_t"); + if (auto error = sdo::helpers::check_size(blob, 1, "uint8_t")){ + return sdo::DeserializeResult::err(*error); + } - return static_cast(blob[0]); + return DeserializeResult::ok(static_cast(blob[0])); } // use for UNSIGNED16, WORD, BITARR16, BIT9-BIT16 - template <> [[nodiscard]] inline std::uint16_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 2, "uint16_t"); + 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 std::uint32_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 4, "uint32_t"); + 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 float deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 4, "float"); + if (auto error = sdo::helpers::check_size(blob, 4, "float")){ + return sdo::DeserializeResult::err(*error); + } - std::uint32_t raw = sdo::helpers::to_raw(blob, 4); + auto raw = sdo::helpers::to_raw(blob, 4); + if(!raw){ + return DeserializeResult::err(raw.error); + } float value; - std::memcpy(&value, &raw, sizeof(value)); + std::memcpy(&value, &raw.value, sizeof(value)); - return value; + return DeserializeResult::ok(value); } - template <> [[nodiscard]] inline double deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 8, "double"); + if (auto error = sdo::helpers::check_size(blob, 8, "double")){ + return sdo::DeserializeResult::err(*error); + } - std::uint64_t raw = sdo::helpers::to_raw(blob, 8); + auto raw = sdo::helpers::to_raw(blob, 8); + if(!raw){ + return DeserializeResult::err(raw.error); + } double value; - std::memcpy(&value, &raw, sizeof(value)); + std::memcpy(&value, &raw.value, sizeof(value)); - return value; + return DeserializeResult::ok(value); } // -Time- - template <> [[nodiscard]] inline TimeOfDay deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 6, "TimeOfDay"); + if (auto error = sdo::helpers::check_size(blob, 6, "TimeOfDay")){ + return sdo::DeserializeResult::err(*error); + } TimeOfDay value{}; - value.ms_since_midnight = sdo::helpers::to_raw(blob, 4); - value.d_since_1984_01_01 = sdo::helpers::to_raw(blob, 2, 4); + 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; - return 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 TimeDifference deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 6, "TimeDifference"); + 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 = sdo::helpers::to_raw(blob, 4) & 0x0FFFFFFF; + value.ms = ms_raw.value & 0x0FFFFFFF; - value.d = sdo::helpers::to_raw(blob, 2, 4); + 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 value; + return DeserializeResult::ok(value); } // -Domain- // (returns raw blob as equivalent to the EtherCAT Domain data type) - template <> [[nodiscard]] inline std::vector deserialize>( + template <> [[nodiscard]] inline DeserializeResult> deserialize>( const std::vector& blob) { - return blob; + return DeserializeResult>::ok(blob); } // -Extended Signed Integer- - template <> [[nodiscard]] inline Int24 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 3, "Int24"); + if (auto error = sdo::helpers::check_size(blob, 3, "Int24")){ + return sdo::DeserializeResult::err(*error); + } - std::uint32_t raw = sdo::helpers::to_raw(blob, 3); + auto raw = sdo::helpers::to_raw(blob, 3); + if(!raw){ + return DeserializeResult::err(raw.error); + } - if (raw & 0x00800000){ - raw |= 0xFF000000; + if (raw.value & 0x00800000){ + raw.value |= 0xFF000000; } - return Int24{static_cast(raw)}; + return DeserializeResult::ok(Int24{static_cast(raw.value)}); } - template <> [[nodiscard]] inline Int40 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 5, "Int40"); + if (auto error = sdo::helpers::check_size(blob, 5, "Int40")){ + return sdo::DeserializeResult::err(*error); + } - std::uint64_t raw = sdo::helpers::to_raw(blob, 5); + auto raw = sdo::helpers::to_raw(blob, 5); + if(!raw){ + return DeserializeResult::err(raw.error); + } - if (raw & 0x0000008000000000ULL) + if (raw.value & 0x0000008000000000ULL) { - raw |= 0xFFFFFF0000000000ULL; + raw.value |= 0xFFFFFF0000000000ULL; } - return Int40{static_cast(raw)}; + return DeserializeResult::ok(Int40{static_cast(raw.value)}); } - template <> [[nodiscard]] inline Int48 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 6, "Int48"); + if (auto error = sdo::helpers::check_size(blob, 6, "Int48")){ + return sdo::DeserializeResult::err(*error); + } - std::uint64_t raw = sdo::helpers::to_raw(blob, 6); + auto raw = sdo::helpers::to_raw(blob, 6); + if(!raw){ + return DeserializeResult::err(raw.error); + } - if (raw & 0x0000800000000000ULL) + if (raw.value & 0x0000800000000000ULL) { - raw |= 0xFFFF000000000000ULL; + raw.value |= 0xFFFF000000000000ULL; } - return Int48{static_cast(raw)}; + return DeserializeResult::ok(Int48{static_cast(raw.value)}); } - template <> [[nodiscard]] inline Int56 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 7, "Int56"); + if (auto error = sdo::helpers::check_size(blob, 7, "Int56")){ + return sdo::DeserializeResult::err(*error); + } - std::uint64_t raw = sdo::helpers::to_raw(blob, 7); + auto raw = sdo::helpers::to_raw(blob, 7); + if(!raw){ + return DeserializeResult::err(raw.error); + } - if (raw & 0x0080000000000000ULL) + if (raw.value & 0x0080000000000000ULL) { - raw |= 0xFF00000000000000ULL; + raw.value |= 0xFF00000000000000ULL; } - return Int56{static_cast(raw)}; + return DeserializeResult::ok(Int56{static_cast(raw.value)}); } - template <> [[nodiscard]] inline std::int64_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 8, "int64_t"); + 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 static_cast(sdo::helpers::to_raw(blob, 8)); + return DeserializeResult::ok(static_cast(raw.value)); } // -Extended Unsigned Integer- - template <> [[nodiscard]] inline UInt24 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 3, "UInt24"); + if (auto error = sdo::helpers::check_size(blob, 3, "UInt24")){ + return sdo::DeserializeResult::err(*error); + } - return UInt24{sdo::helpers::to_raw(blob, 3)}; + auto raw = sdo::helpers::to_raw(blob, 3); + if(!raw){ + return DeserializeResult::err(raw.error); + } + + return DeserializeResult::ok(UInt24{raw.value}); } - template <> [[nodiscard]] inline UInt40 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 5, "UInt40"); + if (auto error = sdo::helpers::check_size(blob, 5, "UInt40")){ + return sdo::DeserializeResult::err(*error); + } - return UInt40{sdo::helpers::to_raw(blob, 5)}; + auto raw = sdo::helpers::to_raw(blob, 5); + if(!raw){ + return DeserializeResult::err(raw.error); + } + + return DeserializeResult::ok(UInt40{raw.value}); } - template <> [[nodiscard]] inline UInt48 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 6, "UInt48"); + if (auto error = sdo::helpers::check_size(blob, 6, "UInt48")){ + return sdo::DeserializeResult::err(*error); + } - return UInt48{sdo::helpers::to_raw(blob, 6)}; + auto raw = sdo::helpers::to_raw(blob, 6); + if(!raw){ + return DeserializeResult::err(raw.error); + } + + return DeserializeResult::ok(UInt48{raw.value}); } - template <> [[nodiscard]] inline UInt56 deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 7, "UInt56"); + if (auto error = sdo::helpers::check_size(blob, 7, "UInt56")){ + return sdo::DeserializeResult::err(*error); + } + + auto raw = sdo::helpers::to_raw(blob, 7); + if(!raw){ + return DeserializeResult::err(raw.error); + } - return UInt56{sdo::helpers::to_raw(blob, 7)}; + return DeserializeResult::ok(UInt56{raw.value}); } - template <> [[nodiscard]] inline std::uint64_t deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 8, "uint64_t"); + if (auto error = sdo::helpers::check_size(blob, 8, "uint64_t")){ + return sdo::DeserializeResult::err(*error); + } return sdo::helpers::to_raw(blob, 8); } // -GUID- - template <> [[nodiscard]] inline Guid deserialize(const std::vector& blob) + template <> [[nodiscard]] inline DeserializeResult deserialize( + const std::vector& blob) { - sdo::helpers::check_size(blob, 16, "Guid"); + if (auto error = sdo::helpers::check_size(blob, 16, "Guid")){ + return sdo::DeserializeResult::err(*error); + } Guid value{}; @@ -589,26 +738,27 @@ namespace sdo value.bytes[i] = blob[i]; } - return value; + return DeserializeResult::ok(value); } // ---SERIALIZATION--- - template std::vector serialize(const T& value) + template SerializeResult serialize(const T& value) { if constexpr (sdo::helpers::is_string_type>>::value){ constexpr std::size_t size = sdo::helpers::string_size::size; if (value.value.size() > size){ - throw std::out_of_range("serialize>: string is longer than size T"); + return SerializeResult::err({ + ErrorCode::InvalidSize, "serialize>: string is longer than size T"}); } std::vector blob(value.value.begin(), value.value.end()); blob.resize(size, '\0'); - return blob; + return SerializeResult::ok(blob); } else{ static_assert(sdo::helpers::always_false, "serialize: unsupported type"); @@ -618,24 +768,24 @@ namespace sdo // -Boolean- - template <> [[nodiscard]] inline std::vector serialize(const bool& value) + template <> [[nodiscard]] inline SerializeResult serialize(const bool& value) { - return {static_cast(value ? 1 : 0)}; + return SerializeResult::ok({static_cast(value ? 1 : 0)}); } // -Signed Integer- - template <> [[nodiscard]] inline std::vector serialize(const std::int8_t& value) + template <> [[nodiscard]] inline SerializeResult serialize(const std::int8_t& value) { - return {static_cast(value)}; + return SerializeResult::ok({static_cast(value)}); } - template <> [[nodiscard]] inline std::vector serialize(const std::int16_t& value) + template <> [[nodiscard]] inline SerializeResult serialize(const std::int16_t& value) { return sdo::helpers::from_raw(static_cast(value), 2); } - template <> [[nodiscard]] inline std::vector serialize(const std::int32_t& value) + template <> [[nodiscard]] inline SerializeResult serialize(const std::int32_t& value) { return sdo::helpers::from_raw(static_cast(value), 4); } @@ -643,27 +793,27 @@ namespace sdo // -Unsigned Integer / raw data / bit arrays / bit strings- // use for UNSIGNED8, BYTE, BITARR8, BIT1-BIT8 - template <> [[nodiscard]] inline std::vector serialize(const std::uint8_t& value) + template <> [[nodiscard]] inline SerializeResult serialize(const std::uint8_t& value) { - return { value }; + return SerializeResult::ok({value}); } // use for UNSIGNED16, WORD, BITARR16, BIT9-BIT16 - template <> [[nodiscard]] inline std::vector serialize(const std::uint16_t& value) + template <> [[nodiscard]] inline SerializeResult serialize(const std::uint16_t& value) { return sdo::helpers::from_raw(value, 2); } // use for UNSIGNED32, DWORD, BITARR32 - template <> [[nodiscard]] inline std::vector serialize(const std::uint32_t& value) + template <> [[nodiscard]] inline SerializeResult serialize(const std::uint32_t& value) { return sdo::helpers::from_raw(value, 4); } // -Floating Point- - template <> [[nodiscard]] inline std::vector serialize(const float& value) + template <> [[nodiscard]] inline SerializeResult serialize(const float& value) { std::uint32_t raw; std::memcpy(&raw, &value, sizeof(raw)); @@ -671,7 +821,7 @@ namespace sdo return sdo::helpers::from_raw(raw, 4); } - template <> [[nodiscard]] inline std::vector serialize(const double& value) + template <> [[nodiscard]] inline SerializeResult serialize(const double& value) { std::uint64_t raw; std::memcpy(&raw, &value, sizeof(raw)); @@ -681,9 +831,9 @@ namespace sdo // -Time- - template <> [[nodiscard]] inline std::vector serialize(const TimeOfDay& value) + template <> [[nodiscard]] inline SerializeResult serialize(const TimeOfDay& value) { - return { + return SerializeResult::ok({ static_cast(value.ms_since_midnight & 0xFF), static_cast((value.ms_since_midnight >> 8) & 0xFF), static_cast((value.ms_since_midnight >> 16) & 0xFF), @@ -691,17 +841,19 @@ namespace sdo static_cast(value.d_since_1984_01_01 & 0xFF), static_cast((value.d_since_1984_01_01 >> 8) & 0xFF) - }; + }); } - template <> [[nodiscard]] inline std::vector serialize(const TimeDifference& value) + template <> [[nodiscard]] inline SerializeResult serialize(const TimeDifference& value) { if (value.ms > 0x0FFFFFFF) { - throw std::out_of_range("serialize: TimeDifference.ms exceeds 28 bit range"); + SerializeResult::err({ + ErrorCode::OutOfRange, + "serialize: TimeDifference.ms exceeds 28 bit range"}); } - return { + return SerializeResult::ok({ static_cast(value.ms & 0xFF), static_cast((value.ms >> 8) & 0xFF), static_cast((value.ms >> 16) & 0xFF), @@ -709,24 +861,24 @@ namespace sdo static_cast(value.d & 0xFF), static_cast((value.d >> 8) & 0xFF) - }; + }); } // -Domain- // (returns raw blob as equivalent to the EtherCAT Domain data type) - template <> [[nodiscard]] inline std::vector serialize>( + template <> [[nodiscard]] inline SerializeResult serialize>( const std::vector& value) { - return value; + return SerializeResult::ok(value); } // -Extended Signed Integer- - template <> [[nodiscard]] inline std::vector serialize(const Int24& value) + template <> [[nodiscard]] inline SerializeResult serialize(const Int24& value) { if (value.value < -pow(2, 23) || value.value > pow(2, 23)-1){ - throw std::out_of_range("Int24 value exceeds 24 bit signed range"); + SerializeResult::err({ErrorCode::OutOfRange, "Int24 value exceeds 24 bit signed range"}); } const auto raw = static_cast(value.value); @@ -734,10 +886,10 @@ namespace sdo return sdo::helpers::from_raw(raw, 3); } - template <> [[nodiscard]] inline std::vector serialize(const Int40& value) + template <> [[nodiscard]] inline SerializeResult serialize(const Int40& value) { if (value.value < -pow(2, 39) || value.value > pow(2, 39)-1){ - throw std::out_of_range("Int40 value exceeds 40 bit signed range"); + SerializeResult::err({ErrorCode::OutOfRange, "Int40 value exceeds 40 bit signed range"}); } const auto raw = static_cast(value.value); @@ -745,10 +897,10 @@ namespace sdo return sdo::helpers::from_raw(raw, 5); } - template <> [[nodiscard]] inline std::vector serialize(const Int48& value) + template <> [[nodiscard]] inline SerializeResult serialize(const Int48& value) { if (value.value < -pow(2, 47) || value.value > pow(2, 47)-1){ - throw std::out_of_range("Int48 value exceeds 48 bit signed range"); + SerializeResult::err({ErrorCode::OutOfRange, "Int48 value exceeds 48 bit signed range"}); } const auto raw = static_cast(value.value); @@ -756,10 +908,10 @@ namespace sdo return sdo::helpers::from_raw(raw, 6); } - template <> [[nodiscard]] inline std::vector serialize(const Int56& value) + template <> [[nodiscard]] inline SerializeResult serialize(const Int56& value) { if (value.value < -pow(2, 55) || value.value > pow(2, 55)-1){ - throw std::out_of_range("Int56 value exceeds 56 bit signed range"); + SerializeResult::err({ErrorCode::OutOfRange, "Int56 value exceeds 56 bit signed range"}); } const auto raw = static_cast(value.value); @@ -767,58 +919,62 @@ namespace sdo return sdo::helpers::from_raw(raw, 7); } - template <> [[nodiscard]] inline std::vector serialize(const int64_t& value) + template <> [[nodiscard]] inline SerializeResult serialize(const int64_t& value) { return sdo::helpers::from_raw(static_cast(value), 8); } // -Extended unsigned Integer- - template <> [[nodiscard]] inline std::vector serialize(const UInt24& value) + template <> [[nodiscard]] inline SerializeResult serialize(const UInt24& value) { if (value.value > pow(2, 24)-1){ - throw std::out_of_range("UInt24 value exceeds 24 bit unsigned range"); + SerializeResult::err({ + ErrorCode::OutOfRange, "UInt24 value exceeds 24 bit unsigned range"}); } return sdo::helpers::from_raw(value.value, 3); } - template <> [[nodiscard]] inline std::vector serialize(const UInt40& value) + template <> [[nodiscard]] inline SerializeResult serialize(const UInt40& value) { if (value.value > pow(2, 40)-1){ - throw std::out_of_range("UInt40 value exceeds 40 bit unsigned range"); + SerializeResult::err({ + ErrorCode::OutOfRange, "UInt40 value exceeds 40 bit unsigned range"}); } return sdo::helpers::from_raw(value.value, 5); } - template <> [[nodiscard]] inline std::vector serialize(const UInt48& value) + template <> [[nodiscard]] inline SerializeResult serialize(const UInt48& value) { if (value.value > pow(2, 48)-1){ - throw std::out_of_range("UInt48 value exceeds 48 bit unsigned range"); + SerializeResult::err({ + ErrorCode::OutOfRange, "UInt48 value exceeds 48 bit unsigned range"}); } return sdo::helpers::from_raw(value.value, 6); } - template <> [[nodiscard]] inline std::vector serialize(const UInt56& value) + template <> [[nodiscard]] inline SerializeResult serialize(const UInt56& value) { if (value.value > pow(2, 56)-1){ - throw std::out_of_range("UInt56 value exceeds 56 bit unsigned range"); + SerializeResult::err({ + ErrorCode::OutOfRange, "UInt56 value exceeds 56 bit unsigned range"}); } return sdo::helpers::from_raw(value.value, 7); } - template <> [[nodiscard]] inline std::vector serialize(const uint64_t& value) + template <> [[nodiscard]] inline SerializeResult serialize(const uint64_t& value) { return sdo::helpers::from_raw(value, 8); } // -GUID- - template <> [[nodiscard]] inline std::vector serialize(const Guid& value) + template <> [[nodiscard]] inline SerializeResult serialize(const Guid& value) { - return std::vector(value.bytes.begin(), value.bytes.end()); + return SerializeResult::ok(std::vector(value.bytes.begin(), value.bytes.end())); } } diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp index 44cb457..de80c7f 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp @@ -6,10 +6,12 @@ TEST(SDOSerializationTest, Bool) { bool value = true; - std::vector blob = sdo::serialize(value); - auto result = sdo::deserialize(blob); + auto blob = sdo::serialize(value); + ASSERT_TRUE(blob); + auto result = sdo::deserialize(blob.value); + ASSERT_TRUE(result); - ASSERT_EQ(result, value); + ASSERT_EQ(result.value, value); } TEST(SDOSerializationTest, Int8) @@ -18,17 +20,23 @@ TEST(SDOSerializationTest, Int8) std::int8_t value_max = std::numeric_limits::max(); std::int8_t value_min = std::numeric_limits::min(); - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value, value_zero); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value, value_max); blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min, value_min); + ASSERT_TRUE(blob); + auto result_min = sdo::deserialize(blob.value); + ASSERT_TRUE(result_min); + ASSERT_EQ(result_min.value, value_min); } TEST(SDOSerializationTest, Int16) @@ -37,17 +45,23 @@ TEST(SDOSerializationTest, Int16) std::int16_t value_max = std::numeric_limits::max(); std::int16_t value_min = std::numeric_limits::min(); - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value, value_zero); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value, value_max); blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min, value_min); + ASSERT_TRUE(blob); + auto result_min = sdo::deserialize(blob.value); + ASSERT_TRUE(result_min); + ASSERT_EQ(result_min.value, value_min); } TEST(SDOSerializationTest, Int32) @@ -56,17 +70,23 @@ TEST(SDOSerializationTest, Int32) std::int32_t value_max = std::numeric_limits::max(); std::int32_t value_min = std::numeric_limits::min(); - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value, value_zero); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value, value_max); blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min, value_min); + ASSERT_TRUE(blob); + auto result_min = sdo::deserialize(blob.value); + ASSERT_TRUE(result_min); + ASSERT_EQ(result_min.value, value_min); } TEST(SDOSerializationTest, uInt8) @@ -74,13 +94,17 @@ TEST(SDOSerializationTest, uInt8) std::uint8_t value_zero = 0; std::uint8_t value_max = std::numeric_limits::max(); - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value, value_zero); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value, value_max); } TEST(SDOSerializationTest, uInt16) @@ -88,13 +112,17 @@ TEST(SDOSerializationTest, uInt16) std::uint16_t value_zero = 0; std::uint16_t value_max = std::numeric_limits::max(); - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value, value_zero); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value, value_max); } TEST(SDOSerializationTest, uInt32) @@ -102,13 +130,17 @@ TEST(SDOSerializationTest, uInt32) std::uint32_t value_zero = 0; std::uint32_t value_max = std::numeric_limits::max(); - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value, value_zero); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value, value_max); } TEST(SDOSerializationTest, Float) @@ -117,17 +149,23 @@ TEST(SDOSerializationTest, Float) float value_pos = 123.456f; float value_neg = -123.456f; - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value, value_zero); blob = sdo::serialize(value_pos); - auto result_pos = sdo::deserialize(blob); - ASSERT_EQ(result_pos, value_pos); + ASSERT_TRUE(blob); + auto result_pos = sdo::deserialize(blob.value); + ASSERT_TRUE(result_pos); + ASSERT_EQ(result_pos.value, value_pos); blob = sdo::serialize(value_neg); - auto result_neg = sdo::deserialize(blob); - ASSERT_EQ(result_neg, value_neg); + ASSERT_TRUE(blob); + auto result_neg = sdo::deserialize(blob.value); + ASSERT_TRUE(result_neg); + ASSERT_EQ(result_neg.value, value_neg); } TEST(SDOSerializationTest, Double) @@ -136,46 +174,58 @@ TEST(SDOSerializationTest, Double) double value_pos = 123.456; double value_neg = -123.456; - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value, value_zero); blob = sdo::serialize(value_pos); - auto result_pos = sdo::deserialize(blob); - ASSERT_EQ(result_pos, value_pos); + ASSERT_TRUE(blob); + auto result_pos = sdo::deserialize(blob.value); + ASSERT_TRUE(result_pos); + ASSERT_EQ(result_pos.value, value_pos); blob = sdo::serialize(value_neg); - auto result_neg = sdo::deserialize(blob); - ASSERT_EQ(result_neg, value_neg); + ASSERT_TRUE(blob); + auto result_neg = sdo::deserialize(blob.value); + ASSERT_TRUE(result_neg); + ASSERT_EQ(result_neg.value, value_neg); } TEST(SDOSerializationTest, TimeOfDay) { sdo::TimeOfDay value{12345678, 42}; - std::vector blob = sdo::serialize(value); - auto result = sdo::deserialize(blob); - ASSERT_EQ(result.ms_since_midnight, value.ms_since_midnight); - ASSERT_EQ(result.d_since_1984_01_01, value.d_since_1984_01_01); + auto blob = sdo::serialize(value); + ASSERT_TRUE(blob); + auto result = sdo::deserialize(blob.value); + ASSERT_TRUE(result); + ASSERT_EQ(result.value.ms_since_midnight, value.ms_since_midnight); + ASSERT_EQ(result.value.d_since_1984_01_01, value.d_since_1984_01_01); } TEST(SDOSerializationTest, TimeDifference) { sdo::TimeDifference value{12345678, 42}; - std::vector blob = sdo::serialize(value); - auto result = sdo::deserialize(blob); - ASSERT_EQ(result.ms, value.ms); - ASSERT_EQ(result.d, value.d); + auto blob = sdo::serialize(value); + ASSERT_TRUE(blob); + auto result = sdo::deserialize(blob.value); + ASSERT_TRUE(result); + ASSERT_EQ(result.value.ms, value.ms); + ASSERT_EQ(result.value.d, value.d); } TEST(SDOSerializationTest, Domain) { std::vector value = {0, 1, 2, 3}; - std::vector blob = sdo::serialize>(value); - auto result = sdo::deserialize>(blob); - ASSERT_EQ(result, value); + auto blob = sdo::serialize>(value); + ASSERT_TRUE(blob); + auto result = sdo::deserialize>(blob.value); + ASSERT_TRUE(result); + ASSERT_EQ(result.value, value); } TEST(SDOSerializationTest, Int24) @@ -184,17 +234,23 @@ TEST(SDOSerializationTest, Int24) sdo::Int24 value_max{(std::int64_t{1} << 23) - 1}; sdo::Int24 value_min{-(std::int64_t{1} << 23)}; - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value, value_zero); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value.value, value_max.value); blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min.value, value_min.value); + ASSERT_TRUE(blob); + auto result_min = sdo::deserialize(blob.value); + ASSERT_TRUE(result_min); + ASSERT_EQ(result_min.value.value, value_min.value); } TEST(SDOSerializationTest, Int40) @@ -203,17 +259,23 @@ TEST(SDOSerializationTest, Int40) sdo::Int40 value_max{(std::int64_t{1} << 39) - 1}; sdo::Int40 value_min{-(std::int64_t{1} << 39)}; - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value.value, value_zero.value); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value.value, value_max.value); blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min.value, value_min.value); + ASSERT_TRUE(blob); + auto result_min = sdo::deserialize(blob.value); + ASSERT_TRUE(result_min); + ASSERT_EQ(result_min.value.value, value_min.value); } TEST(SDOSerializationTest, Int48) @@ -222,17 +284,23 @@ TEST(SDOSerializationTest, Int48) sdo::Int48 value_max{(std::int64_t{1} << 47) - 1}; sdo::Int48 value_min{-(std::int64_t{1} << 47)}; - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value.value, value_zero.value); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value.value, value_max.value); blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min.value, value_min.value); + ASSERT_TRUE(blob); + auto result_min = sdo::deserialize(blob.value); + ASSERT_TRUE(result_min); + ASSERT_EQ(result_min.value.value, value_min.value); } TEST(SDOSerializationTest, Int56) @@ -241,17 +309,23 @@ TEST(SDOSerializationTest, Int56) sdo::Int56 value_max{(std::int64_t{1} << 55) - 1}; sdo::Int56 value_min{-(std::int64_t{1} << 55)}; - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value.value, value_zero.value); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value.value, value_max.value); blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min.value, value_min.value); + ASSERT_TRUE(blob); + auto result_min = sdo::deserialize(blob.value); + ASSERT_TRUE(result_min); + ASSERT_EQ(result_min.value.value, value_min.value); } TEST(SDOSerializationTest, Int64) @@ -260,17 +334,23 @@ TEST(SDOSerializationTest, Int64) std::int64_t value_max = std::numeric_limits::max(); std::int64_t value_min = std::numeric_limits::min(); - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value, value_zero); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value, value_max); blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min, value_min); + ASSERT_TRUE(blob); + auto result_min = sdo::deserialize(blob.value); + ASSERT_TRUE(result_min); + ASSERT_EQ(result_min.value, value_min); } TEST(SDOSerializationTest, uInt24) @@ -278,13 +358,17 @@ TEST(SDOSerializationTest, uInt24) sdo::UInt24 value_zero{0}; sdo::UInt24 value_max{(std::uint64_t{1} << 23) - 1}; - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value.value, value_zero.value); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value.value, value_max.value); } TEST(SDOSerializationTest, uInt40) @@ -292,13 +376,17 @@ TEST(SDOSerializationTest, uInt40) sdo::UInt40 value_zero{0}; sdo::UInt40 value_max{(std::uint64_t{1} << 40) - 1}; - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value.value, value_zero.value); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value.value, value_max.value); } TEST(SDOSerializationTest, uInt48) @@ -306,13 +394,17 @@ TEST(SDOSerializationTest, uInt48) sdo::UInt48 value_zero{0}; sdo::UInt48 value_max{(std::uint64_t{1} << 48) - 1}; - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value.value, value_zero.value); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value.value, value_max.value); } TEST(SDOSerializationTest, uInt56) @@ -320,13 +412,17 @@ TEST(SDOSerializationTest, uInt56) sdo::UInt56 value_zero{0}; sdo::UInt56 value_max{(std::uint64_t{1} << 56) - 1}; - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value.value, value_zero.value); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value.value, value_max.value); } TEST(SDOSerializationTest, uInt64) @@ -334,13 +430,17 @@ TEST(SDOSerializationTest, uInt64) std::uint64_t value_zero = 0; std::uint64_t value_max = std::numeric_limits::max(); - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); + auto blob = sdo::serialize(value_zero); + ASSERT_TRUE(blob); + auto result_zero = sdo::deserialize(blob.value); + ASSERT_TRUE(result_zero); + ASSERT_EQ(result_zero.value, value_zero); blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); + ASSERT_TRUE(blob); + auto result_max = sdo::deserialize(blob.value); + ASSERT_TRUE(result_max); + ASSERT_EQ(result_max.value, value_max); } TEST(SDOSerializationTest, Guid) @@ -352,30 +452,34 @@ TEST(SDOSerializationTest, Guid) 13, 14, 15, 16 }}; - std::vector blob = sdo::serialize(value); - auto result = sdo::deserialize(blob); - ASSERT_EQ(result.bytes, value.bytes); + auto blob = sdo::serialize(value); + ASSERT_TRUE(blob); + auto result = sdo::deserialize(blob.value); + ASSERT_TRUE(result); + ASSERT_EQ(result.value.bytes, value.bytes); } TEST(SDOSerializationTest, String50) { - std::string string = "RISE"; + std::string str = "RISE"; - std::vector blob = sdo::serialize>(string); + auto blob = sdo::serialize>(str); + ASSERT_TRUE(blob); - ASSERT_EQ(blob.size(), 50); + ASSERT_EQ(blob.value.size(), 50); - ASSERT_EQ(blob[0], static_cast('R')); - ASSERT_EQ(blob[1], static_cast('I')); - ASSERT_EQ(blob[2], static_cast('S')); - ASSERT_EQ(blob[3], static_cast('E')); + ASSERT_EQ(blob.value[0], static_cast('R')); + ASSERT_EQ(blob.value[1], static_cast('I')); + ASSERT_EQ(blob.value[2], static_cast('S')); + ASSERT_EQ(blob.value[3], static_cast('E')); - for (std::size_t i = string.size(); i < blob.size(); ++i) + for (std::size_t i = str.size(); i < blob.value.size(); ++i) { - ASSERT_EQ(blob[i], 0); + ASSERT_EQ(blob.value[i], 0); } - std::string result = sdo::deserialize>(blob); + auto result = sdo::deserialize>(blob.value); + ASSERT_TRUE(result); - ASSERT_EQ(result, string); + ASSERT_EQ(std::string(result.value), str); } From 0156313a3e99cc14396eb5487640276254cb1588 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sat, 30 May 2026 02:29:27 +0000 Subject: [PATCH 26/53] add support for BIT, BITARR, WSTRING and ARRAY data types --- .../include/rise_motion/sdo_serializer.hpp | 309 +++++++++++++++++- .../rise_motion/test/test_sdo_serializer.cpp | 77 +++++ 2 files changed, 376 insertions(+), 10 deletions(-) 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 index eaae677..5b50aa0 100644 --- 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 @@ -18,7 +18,8 @@ namespace sdo { None, InvalidSize, - OutOfRange + OutOfRange, + UnsupportedType }; struct Error @@ -194,6 +195,36 @@ namespace sdo } }; + 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>; @@ -243,6 +274,27 @@ namespace sdo 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; @@ -251,6 +303,19 @@ namespace sdo using GUID = sdo::Guid; using DOMAIN = std::vector; + + template using VISIBLE_STRING = STRING; + template using UNICODE_STRING = WSTRING; + template using OKTET_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; } @@ -267,6 +332,32 @@ namespace sdo::helpers 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) { @@ -337,9 +428,105 @@ namespace sdo template [[nodiscard]] inline DeserializeResult deserialize( const std::vector& blob) { - if constexpr (sdo::helpers::is_string_type>>::value){ - - constexpr std::size_t size = sdo::helpers::string_size::size; + 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({ @@ -347,16 +534,16 @@ namespace sdo "deserialize>: blob is larger than the expected size T"}); } - T string{}; + T str{}; - string.value.assign(blob.begin(), blob.end()); + str.value.assign(blob.begin(), blob.end()); // strip padding zeros - while (!string.value.empty() && string.value.back() == '\0'){ - string.value.pop_back(); + while (!str.value.empty() && str.value.back() == '\0'){ + str.value.pop_back(); } - return DeserializeResult::ok(string); + return DeserializeResult::ok(str); } else{ static_assert(sdo::helpers::always_false, "deserialize: unsupported type"); @@ -746,7 +933,109 @@ namespace sdo template SerializeResult serialize(const T& value) { - if constexpr (sdo::helpers::is_string_type>>::value){ + 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 (value.value.size() > numElements){ + return SerializeResult::err({ + ErrorCode::OutOfRange, + "serialize>: array is longer than size N" + }); + } + + std::vector blob; + blob.reserve(byteSize); + + for (const auto& element : value.value){ + if constexpr (elementSize == 1){ + const auto raw = static_cast(element); + + blob.push_back(raw); + } + else if constexpr (elementSize == 2){ + const auto raw = static_cast(element); + + blob.push_back(static_cast(raw & 0xFF)); + blob.push_back(static_cast((raw >> 8) & 0xFF)); + } + else if constexpr (elementSize == 4){ + std::uint32_t raw; + + if constexpr (std::is_same_v){ + std::memcpy(&raw, &element, sizeof(raw)); + } + else{ + raw = static_cast(element); + } + + blob.push_back(static_cast(raw & 0xFF)); + blob.push_back(static_cast((raw >> 8) & 0xFF)); + blob.push_back(static_cast((raw >> 16) & 0xFF)); + blob.push_back(static_cast((raw >> 24) & 0xFF)); + } + else if constexpr (elementSize == 8){ + std::uint64_t raw; + + if constexpr (std::is_same_v){ + std::memcpy(&raw, &element, sizeof(raw)); + } + else{ + raw = static_cast(element); + } + + blob.push_back(static_cast(raw & 0xFF)); + blob.push_back(static_cast((raw >> 8) & 0xFF)); + blob.push_back(static_cast((raw >> 16) & 0xFF)); + blob.push_back(static_cast((raw >> 24) & 0xFF)); + blob.push_back(static_cast((raw >> 32) & 0xFF)); + blob.push_back(static_cast((raw >> 40) & 0xFF)); + blob.push_back(static_cast((raw >> 48) & 0xFF)); + blob.push_back(static_cast((raw >> 56) & 0xFF)); + } + else{ + return SerializeResult::err({ + ErrorCode::UnsupportedType, + "serialize>: unsupported array type"}); + } + } + + blob.resize(byteSize, 0); + + return SerializeResult::ok(std::move(blob)); + } + else if constexpr (sdo::helpers::is_wstring_type::value) + { + constexpr std::size_t size = sdo::helpers::wstring_size::size; + + if (value.value.size() > size) + { + return SerializeResult::err({ + ErrorCode::InvalidSize, "serialize>: string is longer than size T"}); + } + + std::vector blob; + blob.reserve(size * 2); + + for (char16_t c : value.value) + { + std::uint16_t raw = static_cast(c); + + blob.push_back(static_cast(raw & 0xFF)); + blob.push_back(static_cast((raw >> 8) & 0xFF)); + } + + blob.resize(size * 2, '\0'); + + return SerializeResult::ok(std::move(blob)); + } + else if constexpr (sdo::helpers::is_string_type::value) + { constexpr std::size_t size = sdo::helpers::string_size::size; diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp index de80c7f..922d238 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp @@ -483,3 +483,80 @@ TEST(SDOSerializationTest, String50) ASSERT_EQ(std::string(result.value), str); } + +TEST(SDOSerializationTest, WString50) +{ + std::u16string str = u"RISE"; + + auto blob = sdo::serialize>(str); + ASSERT_TRUE(blob); + + ASSERT_EQ(blob.value.size(), 100); + + ASSERT_EQ(blob.value[0], static_cast('R')); + ASSERT_EQ(blob.value[1], 0); + ASSERT_EQ(blob.value[2], static_cast('I')); + ASSERT_EQ(blob.value[3], 0); + ASSERT_EQ(blob.value[4], static_cast('S')); + ASSERT_EQ(blob.value[5], 0); + ASSERT_EQ(blob.value[6], static_cast('E')); + ASSERT_EQ(blob.value[7], 0); + + for (std::size_t i = str.size() * 2; i < blob.value.size(); ++i) + { + ASSERT_EQ(blob.value[i], 0); + } + + auto result = sdo::deserialize>(blob.value); + ASSERT_TRUE(result); + + ASSERT_EQ(std::u16string(result.value), str); +} + +TEST(SDOSerializationTest, ArrayUInt16) +{ + sdo::ARRAY_OF_BITARR16<5> value{{1, 2, 3, 4, 5}}; + + auto blob = sdo::serialize>(value); + ASSERT_TRUE(blob); + + ASSERT_EQ(blob.value.size(), 10); + + ASSERT_EQ(blob.value[0], 1); + ASSERT_EQ(blob.value[1], 0); + ASSERT_EQ(blob.value[2], 2); + ASSERT_EQ(blob.value[3], 0); + ASSERT_EQ(blob.value[4], 3); + ASSERT_EQ(blob.value[5], 0); + ASSERT_EQ(blob.value[6], 4); + ASSERT_EQ(blob.value[7], 0); + ASSERT_EQ(blob.value[8], 5); + ASSERT_EQ(blob.value[9], 0); + + auto result = sdo::deserialize>(blob.value); + ASSERT_TRUE(result); + + ASSERT_EQ(result.value.value, value.value); +} + +TEST(SDOSerializationTest, ArrayInt16) +{ + sdo::ARRAY_OF_INT<3> value{{0, 123, -1}}; + + auto blob = sdo::serialize>(value); + ASSERT_TRUE(blob); + + ASSERT_EQ(blob.value.size(), 6); + + ASSERT_EQ(blob.value[0], 0); + ASSERT_EQ(blob.value[1], 0); + ASSERT_EQ(blob.value[2], 123); + ASSERT_EQ(blob.value[3], 0); + ASSERT_EQ(blob.value[4], 0xFF); + ASSERT_EQ(blob.value[5], 0xFF); + + auto result = sdo::deserialize>(blob.value); + ASSERT_TRUE(result); + + ASSERT_EQ(result.value.value, value.value); +} From 81503995e67021f29badfc838f201d4e4dd12a47 Mon Sep 17 00:00:00 2001 From: Anton Date: Sat, 30 May 2026 13:41:10 +0200 Subject: [PATCH 27/53] make bool encoding fit specification rules --- .../rise_motion/rise_motion/sdo_serializer.py | 80 ++-- .../rise_motion/test/test_sdo_serializer.py | 385 +++++++++++++++++- 2 files changed, 422 insertions(+), 43 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py index 759f386..63a4350 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -17,7 +17,7 @@ class TypeInfo: # --------------------------------------------------------------------------- -# Master table (keyed by object dictionary index) +# Master table # --------------------------------------------------------------------------- BASE_DATA_TYPES: dict[int, TypeInfo] = { @@ -139,7 +139,7 @@ def get_type_info( try: return BASE_DATA_TYPES[index] except KeyError: - raise KeyError(f"No EtherCAT type with index {index:#06x}") from None + raise KeyError(f"No EtherCAT type with index {index:#04x}") from None if name is not None: try: @@ -170,7 +170,7 @@ def serialize( object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) # serialize a base data type - if object_info.name[0:5] != "ARRAY": + 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: @@ -186,22 +186,35 @@ def deserialize( name: str | None = None, base_data_type: str | None = None, ): + """ + + """ if not isinstance(serialized_value, list) or not all(isinstance(b, int) for b in serialized_value): error_msg = f"serialized_value must be a list[int] not {type(serialized_value)}" raise TypeError(error_msg) 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" - # deserialize a serialized base data type - if object_info.name[0:5] != "ARRAY": - if byte_len != len(serialized_value) and object_info.name[-6:] != "STRING": + # 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 list (imagine this: base_data_type[]) + # deserialize a serialized base data type array (imagine this: base_data_type[]) else: out = [] - for i in range(0, len, serialized_value, byte_len): + 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 @@ -218,7 +231,7 @@ def serialize_bitn(val, bit_s: int) -> list[int]: elif type(val) is int: vali = val else: - raise TypeError(f"val must be either int or str, not {type(val)}") + raise TypeError(f"val must be either int or str, not {type(val)} {val}") byte_len = (bit_s + 7) // 8 return list(vali.to_bytes(byte_len, byteorder="little")) @@ -260,15 +273,16 @@ def serialize_bool(val, bit_s: int) -> list[int]: """ Serialize a bool into a list[int]. """ - if type(val) is bool: - val = int(val) - return serialize_bitn(val, bit_s) + 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 bool(int.from_bytes(bytes(ser_val), byteorder='little')) + return 0 != ser_val[0] def serialize_float(val, bit_s: int) -> list[int]: """ @@ -318,41 +332,25 @@ def deserialize_guid(ser_val: list[int], bit_s: int) -> uuid.UUID: return uuid.UUID(bytes_le=bytes(ser_val)) def serialize_visible_string(val: str, bit_s: int) -> list[int]: + """ + Serialize an ASCII encoded string into a list[int]. + """ return list(val.encode("ASCII")) def deserialize_visible_string(ser_val: list[int], bit_s: int) -> str: + """ + Deerialize a list[int] into an ASCII encoded string. + """ return bytes(ser_val).decode("ASCII") def serialize_unicode_string(val: str, bit_s: int) -> list[int]: + """ + Serialize a utf_16_le encoded string into a list[int]. + """ 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") - -# def serialize_bitn_field(val: list, bit_s: int) -> list[int]: -# object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) -# return globals()[object_info.serialize_fn](value, object_info.bit_size) -# out = [] -# for bitn in val: -# out.extend(serialize_bitn(bitn, bit_s)) -# return out - -# def deserialize_bitn_field(ser_val: list[int], bit_s: int) -> str: -# byte_len = (bit_s + 7) // 8 -# out = "" -# for i in range(0, len, ser_val, byte_len): -# out+deserialize_bitn(list[i:i+byte_len], bit_s) -# return out - -# def serialize_uint_field(val: list, bit_s: int) -> list[int]: -# out = [] -# for bitn in val: -# out.extend(serialize_bitn(bitn, bit_s)) -# return out - -# def deserialize_uint_field(ser_val: list[int], bit_s: int) -> str: -# byte_len = (bit_s + 7) // 8 -# out = "" -# for i in range(0, len, ser_val, byte_len): -# out+deserialize_bitn(list[i:i+byte_len], bit_s) -# return out diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py index 773ecf3..54ddd23 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -260,9 +260,9 @@ def test_roundtrip_random_bool(): @pytest.mark.parametrize("value, dtype, expected", [ (False, "BOOL", [0x00]), - (True, "BOOL", [0x01]), + (True, "BOOL", [0xff]), (0, "BOOL", [0x00]), - (1, "BOOL", [0x01]), + (1, "BOOL", [0xff]), ]) def test_serialize_known_values_bool(value, dtype: str, expected: list[int]): assert expected == serialize(value, base_data_type=dtype) @@ -517,3 +517,384 @@ def test_deserialize_unicode_string_rejects_odd_length(): 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 From d25a51d0923d45fd8f862e754fa7f8bd16c3f31c Mon Sep 17 00:00:00 2001 From: Anton Date: Sat, 30 May 2026 14:54:03 +0200 Subject: [PATCH 28/53] make GUID fit encoding rules --- .../rise_motion/rise_motion/sdo_serializer.py | 19 ++----------------- .../rise_motion/test/test_sdo_serializer.py | 5 +++-- 2 files changed, 5 insertions(+), 19 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py index 63a4350..c46f9a2 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -79,8 +79,8 @@ class TypeInfo: 0x0008: TypeInfo(0x0008, "REAL32", "REAL", 32, "serialize_float", "deserialize_float"), 0x0011: TypeInfo(0x0011, "REAL64", "LREAL", 64, "serialize_float", "deserialize_float"), - # GUID - 0x001D: TypeInfo(0x001D, "GUID", "GUID", 128, "serialize_guid", "deserialize_guid"), + # 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 @@ -316,21 +316,6 @@ def deserialize_time48(ser_val: list[int], bit_s: int) -> tuple[int]: bits = deserialize_bitn(ser_val, bit_s) return (int(bits[0:28], base=2), int(bits[32:48], base=2)) -def serialize_guid(val, bit_s: int) -> list[int]: - """ - Serialize a UUID into a list[int]. - Accepts a uuid.UUID object or a GUID string e.g. "550e8400-e29b-41d4-a716-446655440000". - """ - if isinstance(val, str): - val = uuid.UUID(val) - return list(val.bytes_le) - -def deserialize_guid(ser_val: list[int], bit_s: int) -> uuid.UUID: - """ - Deserialize a list[int] into a uuid.UUID. - """ - return uuid.UUID(bytes_le=bytes(ser_val)) - def serialize_visible_string(val: str, bit_s: int) -> list[int]: """ Serialize an ASCII encoded string into a list[int]. diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py index 54ddd23..80a068e 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -376,14 +376,15 @@ def test_deserialize_known_values_time48(serialized: list[int], dtype: str, expe # --------------------------------------------------------------------------- -# TIME_OF_DAY, TIME_DIFFERENCE tests +# GUID tests # --------------------------------------------------------------------------- # --- round-trip test --- def test_roundtrip_random_guid(): """Serializing then deserializing a random valid value returns the original.""" - value = uuid.uuid4() + 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") From 3411ebbb0c1735b08ff776f6446cc552ebae690b Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sat, 30 May 2026 13:22:03 +0000 Subject: [PATCH 29/53] testing_node.cpp: move sdo_read request into a separate function --- .../src/rise_motion/src/testing_node.cpp | 54 +++++++++++++------ 1 file changed, 37 insertions(+), 17 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp index 05f20bc..86f26b4 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp @@ -76,6 +76,35 @@ class TestNode : public rclcpp::Node { return result->status_enable; } + template auto sdo_read( + uint16_t device_id, uint16_t index, uint8_t subindex, uint8_t value_type = 0) + { + auto client = this->create_client("sdo_read"); + + while (!client->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + return sdo::Result::err({"Interrupted while waiting for sdo_read service"}); + } + } + + auto request = std::make_shared(); + + request->device_id = device_id; + request->index = index; + request->subindex = subindex; + request->value_type = value_type; + + auto future = client->async_send_request(request); + + if (rclcpp::spin_until_future_complete(this, future) != rclcpp::FutureReturnCode::SUCCESS){ + client->remove_pending_request(future); + return sdo::Result::err({"Failed to call sdo_read service"}); + } + + return sdo::deserialize(future.get()->value); + } + + private: std::vector motor_pos; void print_motor_positions(std::vector const &motor_pos, @@ -117,24 +146,15 @@ int main(int argc, char **argv) { while (!node->request_enable_ethercat()) { } - rclcpp::Client::SharedPtr - sdo_client; - sdo_client = node->create_client( - "sdo_read"); - auto request = std::make_shared(); - request->device_id = 1; - request->index = 0x1008; - request->subindex = 0; - request->value_type = 0; - auto result = sdo_client->async_send_request(request); - // wait for result - if (rclcpp::spin_until_future_complete(node, result) == - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sdo_value: %s", (std::string(sdo::deserialize>(result.get()->value))).std::string::c_str()); - } else { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service sdo_read"); + auto result = node->sdo_read>(1, 0x1008, 0, 0); + + if(!result){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), result.error); } + else{ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sdo_value: %s", (std::string(result.value.std::string::c_str()))); + } + rclcpp::spin(node); rclcpp::shutdown(); return 0; From cf6e16115fe15b0a9620c779c028ffc9bda137ed Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sat, 30 May 2026 13:29:11 +0000 Subject: [PATCH 30/53] sdo_serializer.hpp: align bool serialization with ETG spec --- .../src/rise_motion/include/rise_motion/sdo_serializer.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 5b50aa0..9f35fb4 100644 --- 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 @@ -1059,7 +1059,7 @@ namespace sdo template <> [[nodiscard]] inline SerializeResult serialize(const bool& value) { - return SerializeResult::ok({static_cast(value ? 1 : 0)}); + return SerializeResult::ok({static_cast(value ? 0xFF : 0)}); } // -Signed Integer- From 71a2fce3d468c17f6a63a8ed49021e4f382d94da Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sat, 30 May 2026 15:57:58 +0000 Subject: [PATCH 31/53] fix integration of sdo_serializer.hpp --- .../include/rise_motion/sdo_serializer.hpp | 4 +++- .../src/rise_motion/src/testing_node.cpp | 13 ++++++++----- 2 files changed, 11 insertions(+), 6 deletions(-) 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 index 9f35fb4..4174520 100644 --- 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 @@ -19,7 +19,9 @@ namespace sdo None, InvalidSize, OutOfRange, - UnsupportedType + UnsupportedType, + CallFailed, + ServiceUnavailable }; struct Error diff --git a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp index 86f26b4..9f9ffeb 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp @@ -83,7 +83,8 @@ class TestNode : public rclcpp::Node { while (!client->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { - return sdo::Result::err({"Interrupted while waiting for sdo_read service"}); + return rise::Result::err({ + sdo::ErrorCode::ServiceUnavailable, "Interrupted while waiting for sdo_read service"}); } } @@ -96,9 +97,11 @@ class TestNode : public rclcpp::Node { auto future = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(this, future) != rclcpp::FutureReturnCode::SUCCESS){ + if (rclcpp::spin_until_future_complete( + this->get_node_base_interface(), future) != rclcpp::FutureReturnCode::SUCCESS){ client->remove_pending_request(future); - return sdo::Result::err({"Failed to call sdo_read service"}); + return rise::Result::err({ + sdo::ErrorCode::CallFailed, "Failed to call sdo_read service"}); } return sdo::deserialize(future.get()->value); @@ -149,10 +152,10 @@ int main(int argc, char **argv) { auto result = node->sdo_read>(1, 0x1008, 0, 0); if(!result){ - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), result.error); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), result.error.message.std::string::c_str()); } else{ - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sdo_value: %s", (std::string(result.value.std::string::c_str()))); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sdo_value: %s", (std::string(result.value).std::string::c_str())); } rclcpp::spin(node); From cf102f800d116bc7bdbf991c61d0a7ce90b99899 Mon Sep 17 00:00:00 2001 From: Anton Date: Sat, 30 May 2026 19:44:07 +0200 Subject: [PATCH 32/53] make time fit encoding rules --- .../rise_motion/rise_motion/sdo_serializer.py | 26 ++++++++++--------- .../rise_motion/test/test_sdo_serializer.py | 8 +++--- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py index c46f9a2..f3ccbb3 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -29,8 +29,8 @@ class TypeInfo: 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_time48", "deserialize_time48"), - 0x000D: TypeInfo(0x000D, "TIME_DIFFERENCE","TIME_DIFFERENCE", 48, "serialize_time48", "deserialize_time48"), + 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"), @@ -296,25 +296,27 @@ def deserialize_float(ser_val: list[int], bit_s: int) -> float: """ return struct.unpack(f"<{'f' if bit_s == 32 else 'd'}", bytes(ser_val))[0] -def serialize_time48(val, bit_s: int) -> list[int]: +def serialize_time_of_day(val, bit_s: int) -> list[int]: """ Takes either an int or str of bits describing the whole time48 struct or a tuple containing ms and days """ - if not isinstance(val, (str, int)): - ms = format(val[0], '028b') # UNSIGNED28 ms - void = "0000" # VOID4 reserved - days = format(val[1], '016b') # UNSIGNED16 days - val = ms+void+days - - return serialize_bitn(val, bit_s) + if val[0] > (1<<28)-1: + raise OverflowError("the 4 most significant bits of number of milliseconds 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]: + """ + Takes either an int or str of bits describing the whole time48 struct + or a tuple containing ms and days + """ + 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]: """ Takes list[int] and returns a tuple containing ms and days """ - bits = deserialize_bitn(ser_val, bit_s) - return (int(bits[0:28], base=2), int(bits[32:48], base=2)) + 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]: """ diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py index 80a068e..49350c2 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -358,8 +358,8 @@ def test_roundtrip_max_time48(dtype: str): # --- Explicit serialization checks (known input -> known list[int]) --- @pytest.mark.parametrize("value, dtype, expected", [ - ((5000, 5000), "TIME_OF_DAY", [0x88, 0x13, 0x80, 0x38, 0x01, 0x00]), - ((1000, 1000), "TIME_DIFFERENCE", [0xE8, 0x03, 0x80, 0x3E, 0x00, 0x00]), + ((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) @@ -368,8 +368,8 @@ def test_serialize_known_values_time48(value: int, dtype: str, expected: list[in # --- Explicit deserialization checks (known list[int] -> known output) --- @pytest.mark.parametrize("serialized, dtype, expected_value", [ - ([0x88, 0x13, 0x80, 0x38, 0x01, 0x00], "TIME_OF_DAY", (5000, 5000)), - ([0xE8, 0x03, 0x80, 0x3E, 0x00, 0x00], "TIME_DIFFERENCE", (1000, 1000)), + ([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) From f6d589e5c0dd864f049c907d28a41d6a2ce5d643 Mon Sep 17 00:00:00 2001 From: Anton Date: Sat, 30 May 2026 20:43:39 +0200 Subject: [PATCH 33/53] add error handling options --- .../rise_motion/rise_motion/sdo_serializer.py | 135 +++++++++++------- .../rise_motion/test/test_sdo_serializer.py | 6 +- 2 files changed, 83 insertions(+), 58 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py index f3ccbb3..9570191 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py @@ -1,6 +1,5 @@ from dataclasses import dataclass import struct -import uuid # --------------------------------------------------------------------------- # Record type @@ -167,17 +166,27 @@ def serialize( name: str | None = None, base_data_type: str | None = None, ) -> list[int]: - 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 + """ + Serializes any base data type from the tables 119 and 120 out of "ETG.1020 EtherCAT Protocol Enhancements". + Parameters: + - data to be serialized + - identifier of data's data type (index, name or base_data_type) + Refer to ETG.1000.6 and ETG.1020 at https://www.ethercat.org/en/downloads.html for details on encoding. + """ + 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], @@ -187,36 +196,42 @@ def deserialize( base_data_type: str | None = None, ): """ - - """ - if not isinstance(serialized_value, list) or not all(isinstance(b, int) for b in serialized_value): - error_msg = f"serialized_value must be a list[int] not {type(serialized_value)}" - raise TypeError(error_msg) - 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 + Deserializes any base data type from the tables 119 and 120 out of "ETG.1020 EtherCAT Protocol Enhancements". + Parameters: + - data to be serialized + - identifier of data's data type (index, name or base_data_type) + Refer to ETG.1000.6 and ETG.1020 at https://www.ethercat.org/en/downloads.html for details on encoding. + """ + 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) @@ -226,15 +241,13 @@ def serialize_bitn(val, bit_s: int) -> list[int]: """ Serialize a bit-string into a list[int]. """ - if type(val) is str: - vali = int(val, 2) - elif type(val) is int: - vali = val - else: - raise TypeError(f"val must be either int or str, not {type(val)} {val}") + 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(vali.to_bytes(byte_len, byteorder="little")) + return list(val.to_bytes(byte_len, byteorder="little")) def deserialize_bitn(ser_val: list[int], bit_s: int) -> str: """ @@ -247,6 +260,8 @@ 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)) @@ -260,6 +275,8 @@ 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")) @@ -273,6 +290,8 @@ 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: @@ -288,6 +307,8 @@ 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: @@ -298,18 +319,20 @@ def deserialize_float(ser_val: list[int], bit_s: int) -> float: def serialize_time_of_day(val, bit_s: int) -> list[int]: """ - Takes either an int or str of bits describing the whole time48 struct - or a tuple containing ms and days + Takes a tuple of ints containing ms since midnight and days since 01.01.1984 """ + if (len(val) is not 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 milliseconds since midnight need to be 0") + 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]: """ - Takes either an int or str of bits describing the whole time48 struct - or a tuple containing ms and days + Takes a tuple of ints containing ms and days """ + if (len(val) is not 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]: @@ -322,6 +345,8 @@ def serialize_visible_string(val: str, bit_s: int) -> list[int]: """ Serialize an ASCII 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("ASCII")) def deserialize_visible_string(ser_val: list[int], bit_s: int) -> str: @@ -334,6 +359,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: diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py index 49350c2..cae2eb8 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py @@ -449,8 +449,7 @@ def test_deserialize_known_values_visible_string(serialized: list[int], expected ]) def test_serialize_visible_string_rejects_non_ascii(value: str): """Characters outside the visible ASCII range should raise.""" - with pytest.raises((ValueError, UnicodeEncodeError)): - serialize(value, name="VISIBLE_STRING") + assert -1 == serialize(value, name="VISIBLE_STRING") # --------------------------------------------------------------------------- @@ -514,8 +513,7 @@ def test_unicode_string_serialized_length_is_even(value: str): def test_deserialize_unicode_string_rejects_odd_length(): """An odd number of bytes cannot be valid UTF-16-LE.""" - with pytest.raises((ValueError, UnicodeDecodeError)): - deserialize([0x41], name="UNICODE_STRING") + assert -1 == deserialize([0x41], name="UNICODE_STRING") # --------------------------------------------------------------------------- From 06123a5ec325174959957380ec00ade788b1a985 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Sun, 31 May 2026 21:05:03 +0000 Subject: [PATCH 34/53] testing_node.cpp: add sdo_write method --- .../src/rise_motion/src/testing_node.cpp | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp index 9f9ffeb..7d8c887 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include @@ -107,6 +108,44 @@ class TestNode : public rclcpp::Node { return sdo::deserialize(future.get()->value); } + template auto sdo_write( + uint16_t device_id, uint16_t index, uint8_t subindex, const T &value, uint8_t value_type = 0) +{ + auto serialized = sdo::serialize(value); + + if (!serialized){ + return rise::Result::err(serialized.error); + } + + auto client = this->create_client("sdo_write"); + + while (!client->wait_for_service(std::chrono::seconds(1))){ + if (!rclcpp::ok()){ + return rise::Result::err({ + sdo::ErrorCode::ServiceUnavailable, "Interrupted while waiting for sdo_write service"}); + } + } + + auto request = std::make_shared(); + + request->device_id = device_id; + request->index = index; + request->subindex = subindex; + request->value_type = value_type; + request->value = serialized.value; + + auto future = client->async_send_request(request); + + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::FutureReturnCode::SUCCESS){ + client->remove_pending_request(future); + + return rise::Result::err({ + sdo::ErrorCode::CallFailed, "Failed to call sdo_write service"}); + } + + return rise::Result::ok(true); +} + private: std::vector motor_pos; From b0ccf6657701ea405f5ba3a576641ccf2e7c5c8d Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 1 Jun 2026 10:38:39 +0200 Subject: [PATCH 35/53] delete copied half finished c++ serializer --- .../include/rise_motion/sdo_serializer.hpp | 607 ------------------ .../rise_motion/test/test_sdo_serializer.cpp | 358 ----------- 2 files changed, 965 deletions(-) delete mode 100644 rise_motion_dev_ws/src/rise_motion/include/rise_motion/sdo_serializer.hpp delete mode 100644 rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp 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 deleted file mode 100644 index 74ef296..0000000 --- a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/sdo_serializer.hpp +++ /dev/null @@ -1,607 +0,0 @@ -// library assumes little-endian - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - - -namespace sdo::helpers -{ - template inline constexpr bool always_false = false; - - inline void check_size(const std::vector& blob, std::size_t expectedSize, const char* type) - { - if (blob.size() != expectedSize){ - throw std::invalid_argument(std::string("deserialize<") + type + - ">: expected " + std::to_string(expectedSize) + " bytes, got " + std::to_string(blob.size())); - } - } - - template [[nodiscard]] inline T 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)) - { - throw std::invalid_argument("to_raw: numBytes does not fit T"); - } - - if (offset + numBytes > blob.size()) - { - throw std::out_of_range("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 raw; - } - - template [[nodiscard]] inline std::vector 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)) - { - throw std::invalid_argument("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 blob; - } -} - -namespace sdo -{ - // 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; - }; - - // as equivalent of the EtherCAT TIME_DIFFERENCE data type - struct TimeDifference - { - std::uint32_t ms; - std::uint16_t d; - }; - - struct Int24 - { - std::int32_t value; - }; - - struct Int40 - { - std::int64_t value; - }; - - struct Int48 - { - std::int64_t value; - }; - - struct Int56 - { - std::int64_t value; - }; - - struct UInt24 - { - std::uint32_t value; - }; - - struct UInt40 - { - std::uint64_t value; - }; - - struct UInt48 - { - std::uint64_t value; - }; - - struct UInt56 - { - std::uint64_t value; - }; - - struct Guid - { - std::array bytes; - }; - - - // ---DESERIALIZATION--- - - template T deserialize(const std::vector&) - { - static_assert(sdo::helpers::always_false, "deserialize: unsupported type"); - } - - // -Boolean- - - template <> [[nodiscard]] inline bool deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 1, "bool"); - - return blob[0] != 0; - } - - // -Signed Integer- - - template <> [[nodiscard]] inline std::int8_t deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 1, "int8_t"); - - return static_cast(blob[0]); - } - - template <> [[nodiscard]] inline std::int16_t deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 2, "int16_t"); - - std::int16_t value = static_cast(sdo::helpers::to_raw(blob, 2)); - - return value; - } - - template <> [[nodiscard]] inline std::int32_t deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 4, "int32_t"); - - std::int32_t value = static_cast(sdo::helpers::to_raw(blob, 4)); - - return value; - } - - // -Unsigned Integer / raw data / bit arrays / bit strings- - - // use for UNSIGNED8, BYTE, BITARR8, BIT1-BIT8 - template <> [[nodiscard]] inline std::uint8_t deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 1, "uint8_t"); - - return static_cast(blob[0]); - } - - // use for UNSIGNED16, WORD, BITARR16, BIT9-BIT16 - template <> [[nodiscard]] inline std::uint16_t deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 2, "uint16_t"); - - return sdo::helpers::to_raw(blob, 2); - } - - // use for UNSIGNED32, DWORD, BITARR32 - template <> [[nodiscard]] inline std::uint32_t deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 4, "uint32_t"); - - return sdo::helpers::to_raw(blob, 4); - } - - // -Floating Point- - - template <> [[nodiscard]] inline float deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 4, "float"); - - std::uint32_t raw = sdo::helpers::to_raw(blob, 4); - - float value; - std::memcpy(&value, &raw, sizeof(value)); - - return value; - } - - template <> [[nodiscard]] inline double deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 8, "double"); - - std::uint64_t raw = sdo::helpers::to_raw(blob, 8); - - double value; - std::memcpy(&value, &raw, sizeof(value)); - - return value; - } - - // -Time- - - template <> [[nodiscard]] inline TimeOfDay deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 6, "TimeOfDay"); - - TimeOfDay value{}; - - value.ms_since_midnight = sdo::helpers::to_raw(blob, 4); - value.d_since_1984_01_01 = sdo::helpers::to_raw(blob, 2, 4); - - return value; - } - - template <> [[nodiscard]] inline TimeDifference deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 6, "TimeDifference"); - - TimeDifference value{}; - - // the upper 4 bits of ms are reserved - value.ms = sdo::helpers::to_raw(blob, 4) & 0x0FFFFFFF; - - value.d = sdo::helpers::to_raw(blob, 2, 4); - - return value; - } - - // -Domain- - // (returns raw blob as equivalent to the EtherCAT Domain data type) - - template <> [[nodiscard]] inline std::vector deserialize>( - const std::vector& blob) - { - return blob; - } - - // -Extended Signed Integer- - - template <> [[nodiscard]] inline Int24 deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 3, "Int24"); - - std::uint32_t raw = sdo::helpers::to_raw(blob, 3); - - if (raw & 0x00800000){ - raw |= 0xFF000000; - } - - return Int24{static_cast(raw)}; - } - - template <> [[nodiscard]] inline Int40 deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 5, "Int40"); - - std::uint64_t raw = sdo::helpers::to_raw(blob, 5); - - if (raw & 0x0000008000000000ULL) - { - raw |= 0xFFFFFF0000000000ULL; - } - - return Int40{static_cast(raw)}; - } - - template <> [[nodiscard]] inline Int48 deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 6, "Int48"); - - std::uint64_t raw = sdo::helpers::to_raw(blob, 6); - - if (raw & 0x0000800000000000ULL) - { - raw |= 0xFFFF000000000000ULL; - } - - return Int48{static_cast(raw)}; - } - - template <> [[nodiscard]] inline Int56 deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 7, "Int56"); - - std::uint64_t raw = sdo::helpers::to_raw(blob, 7); - - if (raw & 0x0080000000000000ULL) - { - raw |= 0xFF00000000000000ULL; - } - - return Int56{static_cast(raw)}; - } - - template <> [[nodiscard]] inline std::int64_t deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 8, "int64_t"); - - return static_cast(sdo::helpers::to_raw(blob, 8)); - } - - // -Extended Unsigned Integer- - - template <> [[nodiscard]] inline UInt24 deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 3, "UInt24"); - - return UInt24{sdo::helpers::to_raw(blob, 3)}; - } - - template <> [[nodiscard]] inline UInt40 deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 5, "UInt40"); - - return UInt40{sdo::helpers::to_raw(blob, 5)}; - } - - template <> [[nodiscard]] inline UInt48 deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 6, "UInt48"); - - return UInt48{sdo::helpers::to_raw(blob, 6)}; - } - - template <> [[nodiscard]] inline UInt56 deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 7, "UInt56"); - - return UInt56{sdo::helpers::to_raw(blob, 7)}; - } - - template <> [[nodiscard]] inline std::uint64_t deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 8, "uint64_t"); - - return sdo::helpers::to_raw(blob, 8); - } - - // -GUID- - - template <> [[nodiscard]] inline Guid deserialize(const std::vector& blob) - { - sdo::helpers::check_size(blob, 16, "Guid"); - - Guid value{}; - - for (std::size_t i = 0; i < value.bytes.size(); ++i) - { - value.bytes[i] = blob[i]; - } - - return value; - } - - - // ---SERIALIZATION--- - - template std::vector serialize(const T& value) - { - static_assert(sdo::helpers::always_false, "serialize: unsupported type"); - return{}; - } - - // -Boolean- - - template <> [[nodiscard]] inline std::vector serialize(const bool& value) - { - return {static_cast(value ? 1 : 0)}; - } - - // -Signed Integer- - - template <> [[nodiscard]] inline std::vector serialize(const std::int8_t& value) - { - return {static_cast(value)}; - } - - template <> [[nodiscard]] inline std::vector serialize(const std::int16_t& value) - { - return sdo::helpers::from_raw(static_cast(value), 2); - } - - template <> [[nodiscard]] inline std::vector serialize(const std::int32_t& value) - { - return sdo::helpers::from_raw(static_cast(value), 4); - } - - // -Unsigned Integer / raw data / bit arrays / bit strings- - - // use for UNSIGNED8, BYTE, BITARR8, BIT1-BIT8 - template <> [[nodiscard]] inline std::vector serialize(const std::uint8_t& value) - { - return { value }; - } - - // use for UNSIGNED16, WORD, BITARR16, BIT9-BIT16 - template <> [[nodiscard]] inline std::vector serialize(const std::uint16_t& value) - { - return sdo::helpers::from_raw(value, 2); - } - - // use for UNSIGNED32, DWORD, BITARR32 - - template <> [[nodiscard]] inline std::vector serialize(const std::uint32_t& value) - { - return sdo::helpers::from_raw(value, 4); - } - - // -Floating Point- - - template <> [[nodiscard]] inline std::vector serialize(const float& value) - { - std::uint32_t raw; - std::memcpy(&raw, &value, sizeof(raw)); - - return sdo::helpers::from_raw(raw, 4); - } - - template <> [[nodiscard]] inline std::vector serialize(const double& value) - { - std::uint64_t raw; - std::memcpy(&raw, &value, sizeof(raw)); - - return sdo::helpers::from_raw(raw, 8); - } - - // -Time- - - template <> [[nodiscard]] inline std::vector serialize(const TimeOfDay& value) - { - return { - static_cast(value.ms_since_midnight & 0xFF), - static_cast((value.ms_since_midnight >> 8) & 0xFF), - static_cast((value.ms_since_midnight >> 16) & 0xFF), - static_cast((value.ms_since_midnight >> 24) & 0xFF), - - static_cast(value.d_since_1984_01_01 & 0xFF), - static_cast((value.d_since_1984_01_01 >> 8) & 0xFF) - }; - } - - template <> [[nodiscard]] inline std::vector serialize(const TimeDifference& value) - { - if (value.ms > 0x0FFFFFFF) - { - throw std::out_of_range("serialize: TimeDifference.ms exceeds 28 bit range"); - } - - return { - static_cast(value.ms & 0xFF), - static_cast((value.ms >> 8) & 0xFF), - static_cast((value.ms >> 16) & 0xFF), - static_cast((value.ms >> 24) & 0xFF), - - static_cast(value.d & 0xFF), - static_cast((value.d >> 8) & 0xFF) - }; - } - - // -Domain- - // (returns raw blob as equivalent to the EtherCAT Domain data type) - - template <> [[nodiscard]] inline std::vector serialize>( - const std::vector& value) - { - return value; - } - - // -Extended Signed Integer- - - template <> [[nodiscard]] inline std::vector serialize(const Int24& value) - { - if (value.value < -pow(2, 23) || value.value > pow(2, 23)-1) - { - throw std::out_of_range("Int24 value exceeds 24 bit signed range"); - } - - const auto raw = static_cast(value.value); - - return sdo::helpers::from_raw(raw, 3); - } - - template <> [[nodiscard]] inline std::vector serialize(const Int40& value) - { - if (value.value < -pow(2, 39) || value.value > pow(2, 39)-1) - { - throw std::out_of_range("Int40 value exceeds 40 bit signed range"); - } - - const auto raw = static_cast(value.value); - - return sdo::helpers::from_raw(raw, 5); - } - - template <> [[nodiscard]] inline std::vector serialize(const Int48& value) - { - if (value.value < -pow(2, 47) || value.value > pow(2, 47)-1) - { - throw std::out_of_range("Int48 value exceeds 48 bit signed range"); - } - - const auto raw = static_cast(value.value); - - return sdo::helpers::from_raw(raw, 6); - } - - template <> [[nodiscard]] inline std::vector serialize(const Int56& value) - { - if (value.value < -pow(2, 55) || value.value > pow(2, 55)-1) - { - throw std::out_of_range("Int56 value exceeds 56 bit signed range"); - } - - const auto raw = static_cast(value.value); - - return sdo::helpers::from_raw(raw, 7); - } - - template <> [[nodiscard]] inline std::vector serialize(const int64_t& value) - { - return sdo::helpers::from_raw(static_cast(value), 8); - } - - // -Extended unsigned Integer- - - template <> [[nodiscard]] inline std::vector serialize(const UInt24& value) - { - if (value.value > pow(2, 24)-1) - { - throw std::out_of_range("UInt24 value exceeds 24 bit unsigned range"); - } - - return sdo::helpers::from_raw(value.value, 3); - } - - template <> [[nodiscard]] inline std::vector serialize(const UInt40& value) - { - if (value.value > pow(2, 40)-1) - { - throw std::out_of_range("UInt40 value exceeds 40 bit unsigned range"); - } - - return sdo::helpers::from_raw(value.value, 5); - } - - template <> [[nodiscard]] inline std::vector serialize(const UInt48& value) - { - if (value.value > pow(2, 48)-1) - { - throw std::out_of_range("UInt48 value exceeds 48 bit unsigned range"); - } - - return sdo::helpers::from_raw(value.value, 6); - } - - template <> [[nodiscard]] inline std::vector serialize(const UInt56& value) - { - if (value.value > pow(2, 56)-1) - { - throw std::out_of_range("UInt56 value exceeds 56 bit unsigned range"); - } - - return sdo::helpers::from_raw(value.value, 7); - } - - template <> [[nodiscard]] inline std::vector serialize(const uint64_t& value) - { - return sdo::helpers::from_raw(value, 8); - } - - // -GUID- - - template <> [[nodiscard]] inline std::vector serialize(const Guid& value) - { - return std::vector(value.bytes.begin(), value.bytes.end()); - } -} diff --git a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp b/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp deleted file mode 100644 index e24d1a5..0000000 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.cpp +++ /dev/null @@ -1,358 +0,0 @@ -#include - -#include "rise_motion/sdo_serializer.hpp" - -TEST(SDOSerializationTest, Bool) -{ - bool value = true; - - std::vector blob = sdo::serialize(value); - auto result = sdo::deserialize(blob); - - ASSERT_EQ(result, value); -} - -TEST(SDOSerializationTest, Int8) -{ - std::int8_t value_zero = 0; - std::int8_t value_max = std::numeric_limits::max(); - std::int8_t value_min = std::numeric_limits::min(); - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); - - blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min, value_min); -} - -TEST(SDOSerializationTest, Int16) -{ - std::int16_t value_zero = 0; - std::int16_t value_max = std::numeric_limits::max(); - std::int16_t value_min = std::numeric_limits::min(); - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); - - blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min, value_min); -} - -TEST(SDOSerializationTest, Int32) -{ - std::int32_t value_zero = 0; - std::int32_t value_max = std::numeric_limits::max(); - std::int32_t value_min = std::numeric_limits::min(); - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); - - blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min, value_min); -} - -TEST(SDOSerializationTest, uInt8) -{ - std::uint8_t value_zero = 0; - std::uint8_t value_max = std::numeric_limits::max(); - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); -} - -TEST(SDOSerializationTest, uInt16) -{ - std::uint16_t value_zero = 0; - std::uint16_t value_max = std::numeric_limits::max(); - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); -} - -TEST(SDOSerializationTest, uInt32) -{ - std::uint32_t value_zero = 0; - std::uint32_t value_max = std::numeric_limits::max(); - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); -} - -TEST(SDOSerializationTest, Float) -{ - float value_zero = 0.0f; - float value_pos = 123.456f; - float value_neg = -123.456f; - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); - - blob = sdo::serialize(value_pos); - auto result_pos = sdo::deserialize(blob); - ASSERT_EQ(result_pos, value_pos); - - blob = sdo::serialize(value_neg); - auto result_neg = sdo::deserialize(blob); - ASSERT_EQ(result_neg, value_neg); -} - -TEST(SDOSerializationTest, Double) -{ - double value_zero = 0.0; - double value_pos = 123.456; - double value_neg = -123.456; - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); - - blob = sdo::serialize(value_pos); - auto result_pos = sdo::deserialize(blob); - ASSERT_EQ(result_pos, value_pos); - - blob = sdo::serialize(value_neg); - auto result_neg = sdo::deserialize(blob); - ASSERT_EQ(result_neg, value_neg); -} - -TEST(SDOSerializationTest, TimeOfDay) -{ - sdo::TimeOfDay value{12345678, 42}; - - std::vector blob = sdo::serialize(value); - auto result = sdo::deserialize(blob); - ASSERT_EQ(result.ms_since_midnight, value.ms_since_midnight); - ASSERT_EQ(result.d_since_1984_01_01, value.d_since_1984_01_01); -} - -TEST(SDOSerializationTest, TimeDifference) -{ - sdo::TimeDifference value{12345678, 42}; - - std::vector blob = sdo::serialize(value); - auto result = sdo::deserialize(blob); - ASSERT_EQ(result.ms, value.ms); - ASSERT_EQ(result.d, value.d); -} - -TEST(SDOSerializationTest, Domain) -{ - std::vector value = {0, 1, 2, 3}; - - std::vector blob = sdo::serialize>(value); - auto result = sdo::deserialize>(blob); - ASSERT_EQ(result, value); -} - -TEST(SDOSerializationTest, Int24) -{ - sdo::Int24 value_zero{0}; - sdo::Int24 value_max{(std::int64_t{1} << 23) - 1}; - sdo::Int24 value_min{-(std::int64_t{1} << 23)}; - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); - - blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min.value, value_min.value); -} - -TEST(SDOSerializationTest, Int40) -{ - sdo::Int40 value_zero{0}; - sdo::Int40 value_max{(std::int64_t{1} << 39) - 1}; - sdo::Int40 value_min{-(std::int64_t{1} << 39)}; - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); - - blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min.value, value_min.value); -} - -TEST(SDOSerializationTest, Int48) -{ - sdo::Int48 value_zero{0}; - sdo::Int48 value_max{(std::int64_t{1} << 47) - 1}; - sdo::Int48 value_min{-(std::int64_t{1} << 47)}; - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); - - blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min.value, value_min.value); -} - -TEST(SDOSerializationTest, Int56) -{ - sdo::Int56 value_zero{0}; - sdo::Int56 value_max{(std::int64_t{1} << 55) - 1}; - sdo::Int56 value_min{-(std::int64_t{1} << 55)}; - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); - - blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min.value, value_min.value); -} - -TEST(SDOSerializationTest, Int64) -{ - std::int64_t value_zero = 0; - std::int64_t value_max = std::numeric_limits::max(); - std::int64_t value_min = std::numeric_limits::min(); - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); - - blob = sdo::serialize(value_min); - auto result_min = sdo::deserialize(blob); - ASSERT_EQ(result_min, value_min); -} - -TEST(SDOSerializationTest, uInt24) -{ - sdo::UInt24 value_zero{0}; - sdo::UInt24 value_max{(std::uint64_t{1} << 23) - 1}; - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); -} - -TEST(SDOSerializationTest, uInt40) -{ - sdo::UInt40 value_zero{0}; - sdo::UInt40 value_max{(std::uint64_t{1} << 40) - 1}; - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); -} - -TEST(SDOSerializationTest, uInt48) -{ - sdo::UInt48 value_zero{0}; - sdo::UInt48 value_max{(std::uint64_t{1} << 48) - 1}; - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); -} - -TEST(SDOSerializationTest, uInt56) -{ - sdo::UInt56 value_zero{0}; - sdo::UInt56 value_max{(std::uint64_t{1} << 56) - 1}; - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero.value, value_zero.value); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max.value, value_max.value); -} - -TEST(SDOSerializationTest, uInt64) -{ - std::uint64_t value_zero = 0; - std::uint64_t value_max = std::numeric_limits::max(); - - std::vector blob = sdo::serialize(value_zero); - auto result_zero = sdo::deserialize(blob); - ASSERT_EQ(result_zero, value_zero); - - blob = sdo::serialize(value_max); - auto result_max = sdo::deserialize(blob); - ASSERT_EQ(result_max, value_max); -} - -TEST(SDOSerializationTest, Guid) -{ - sdo::Guid value{{ - 1, 2, 3, 4, - 5, 6, 7, 8, - 9, 10, 11, 12, - 13, 14, 15, 16 - }}; - - std::vector blob = sdo::serialize(value); - auto result = sdo::deserialize(blob); - ASSERT_EQ(result.bytes, value.bytes); -} From 4cb6706c84c516942025dec2bcd2965ac4e6129c Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 1 Jun 2026 11:06:11 +0200 Subject: [PATCH 36/53] delete cmakelists.txt mention of c++ sdo serialization lib --- rise_motion_dev_ws/src/rise_motion/CMakeLists.txt | 6 ------ 1 file changed, 6 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index aaa258d..f3195ba 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -72,15 +72,9 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() # --- sdo_serializer test --- - ament_add_gtest(test_sdo_serializer - test/test_sdo_serializer.cpp - ) ament_add_pytest_test(test_sdo_serializer_python test/test_sdo_serializer.py ) - if(TARGET test_sdo_serializer) - target_link_libraries(test_sdo_serializer sdo_serializer_lib) - endif() endif() From f0b4139b69e67ee95ea2c4d878015dd1252ad6e8 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Mon, 1 Jun 2026 11:20:03 +0200 Subject: [PATCH 37/53] sdo_serializer.hpp: make error messages for STRING more verbose --- .../src/rise_motion/include/rise_motion/sdo_serializer.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 index 4174520..0de2df6 100644 --- 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 @@ -533,7 +533,8 @@ namespace sdo if (blob.size() > size){ return DeserializeResult::err({ ErrorCode::InvalidSize, - "deserialize>: blob is larger than the expected size T"}); + std::string("deserialize>: blob is larger than the expected size T. Expected: ") + + std::to_string(size) + ", got: " + std::to_string(blob.size())}); } T str{}; @@ -1043,7 +1044,8 @@ namespace sdo if (value.value.size() > size){ return SerializeResult::err({ - ErrorCode::InvalidSize, "serialize>: string is longer than size T"}); + ErrorCode::InvalidSize, std::string("serialize>: string is longer than size T. Expected: ") + + std::to_string(size) + ", got: " + value.value.size()}); } std::vector blob(value.value.begin(), value.value.end()); From 4830b1511a0ee8729888110fb8a1bc215dbea96b Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 1 Jun 2026 11:41:32 +0200 Subject: [PATCH 38/53] add python test node --- .../src/rise_motion/CMakeLists.txt | 9 +- .../rise_motion/scripts/python_sdo_test.py | 159 ++++++++++++++++++ .../src/rise_motion_messages/package.xml | 4 + 3 files changed, 171 insertions(+), 1 deletion(-) create mode 100644 rise_motion_dev_ws/src/rise_motion/scripts/python_sdo_test.py diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index f3195ba..50e035f 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -7,9 +7,10 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) find_package(rise_motion_messages REQUIRED) -find_package(ament_cmake_python REQUIRED) include_directories(include) @@ -56,7 +57,13 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) +# Install Python modules ament_python_install_package(${PROJECT_NAME}) +# Install Python executables +install(PROGRAMS + scripts/python_sdo_test.py + DESTINATION lib/${PROJECT_NAME} +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rise_motion_dev_ws/src/rise_motion/scripts/python_sdo_test.py b/rise_motion_dev_ws/src/rise_motion/scripts/python_sdo_test.py new file mode 100644 index 0000000..025824e --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/scripts/python_sdo_test.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 + +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 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("Incrementing motor position with %d", 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 + + # Read device name (object 0x1008, sub 0) as a VISIBLE_STRING + result = node.sdo_read( + device_id=1, + index=0x1008, + subindex=0, + type_name="VISIBLE_STRING", + ) + + if result is -1: + rclpy.get_logger("rclcpp").error("SDO read failed") + else: + node.get_logger().info("sdo_value: %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/rise_motion_messages/package.xml b/rise_motion_dev_ws/src/rise_motion_messages/package.xml index 7aaedda..5300209 100755 --- a/rise_motion_dev_ws/src/rise_motion_messages/package.xml +++ b/rise_motion_dev_ws/src/rise_motion_messages/package.xml @@ -8,6 +8,10 @@ TODO: License declaration ament_cmake + ament_cmake_python + + rclcpp + rclpy ament_lint_auto ament_lint_common From 6a0be66e8434ace86171ee689ac5468dab482fde Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 1 Jun 2026 12:10:52 +0200 Subject: [PATCH 39/53] more reads --- .../rise_motion/scripts/python_sdo_test.py | 27 ++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/scripts/python_sdo_test.py b/rise_motion_dev_ws/src/rise_motion/scripts/python_sdo_test.py index 025824e..62eb316 100644 --- a/rise_motion_dev_ws/src/rise_motion/scripts/python_sdo_test.py +++ b/rise_motion_dev_ws/src/rise_motion/scripts/python_sdo_test.py @@ -138,18 +138,21 @@ def main(): while not node.request_enable_ethercat(): pass - # Read device name (object 0x1008, sub 0) as a VISIBLE_STRING - result = node.sdo_read( - device_id=1, - index=0x1008, - subindex=0, - type_name="VISIBLE_STRING", - ) - - if result is -1: - rclpy.get_logger("rclcpp").error("SDO read failed") - else: - node.get_logger().info("sdo_value: %s", str(result)) + 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 is -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() From 309c6486dbd340aaad845f23a8d216315b048d14 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Mon, 1 Jun 2026 12:16:05 +0200 Subject: [PATCH 40/53] sdo_serializer.hpp: fix error message --- .../src/rise_motion/include/rise_motion/sdo_serializer.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 0de2df6..0f234ac 100644 --- 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 @@ -1045,7 +1045,7 @@ namespace sdo if (value.value.size() > size){ return SerializeResult::err({ ErrorCode::InvalidSize, std::string("serialize>: string is longer than size T. Expected: ") + - std::to_string(size) + ", got: " + value.value.size()}); + std::to_string(size) + ", got: " + std::to_string(value.value.size())}); } std::vector blob(value.value.begin(), value.value.end()); From 670f5a37565f50f76eafe6c63ac262385d42c125 Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 1 Jun 2026 13:45:22 +0200 Subject: [PATCH 41/53] remove unnecessary change --- rise_motion_dev_ws/src/rise_motion_messages/package.xml | 4 ---- 1 file changed, 4 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion_messages/package.xml b/rise_motion_dev_ws/src/rise_motion_messages/package.xml index 5300209..7aaedda 100755 --- a/rise_motion_dev_ws/src/rise_motion_messages/package.xml +++ b/rise_motion_dev_ws/src/rise_motion_messages/package.xml @@ -8,10 +8,6 @@ TODO: License declaration ament_cmake - ament_cmake_python - - rclcpp - rclpy ament_lint_auto ament_lint_common From 79d723879b69474a6911ac65d2d0b3bea4845c30 Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 1 Jun 2026 14:35:20 +0200 Subject: [PATCH 42/53] move into own package --- .../scripts}/__init__.py | 0 .../scripts}/sdo_serializer.py | 4 ++-- .../scripts/temp_test_node.py} | 7 +++++-- 3 files changed, 7 insertions(+), 4 deletions(-) rename rise_motion_dev_ws/src/{rise_motion/rise_motion => py_sdo_serialization/scripts}/__init__.py (100%) rename rise_motion_dev_ws/src/{rise_motion/rise_motion => py_sdo_serialization/scripts}/sdo_serializer.py (98%) rename rise_motion_dev_ws/src/{rise_motion/scripts/python_sdo_test.py => py_sdo_serialization/scripts/temp_test_node.py} (97%) diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/__init__.py b/rise_motion_dev_ws/src/py_sdo_serialization/scripts/__init__.py similarity index 100% rename from rise_motion_dev_ws/src/rise_motion/rise_motion/__init__.py rename to rise_motion_dev_ws/src/py_sdo_serialization/scripts/__init__.py diff --git a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py b/rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py similarity index 98% rename from rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py rename to rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py index 9570191..7483b9a 100644 --- a/rise_motion_dev_ws/src/rise_motion/rise_motion/sdo_serializer.py +++ b/rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py @@ -321,7 +321,7 @@ def serialize_time_of_day(val, bit_s: int) -> list[int]: """ Takes a tuple of ints containing ms since midnight and days since 01.01.1984 """ - if (len(val) is not 2) or (not isinstance(val[0], int)) or (not isinstance(val[1], int)): + 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") @@ -331,7 +331,7 @@ def serialize_time_difference(val, bit_s: int) -> list[int]: """ Takes a tuple of ints containing ms and days """ - if (len(val) is not 2) or (not isinstance(val[0], int)) or (not isinstance(val[1], int)): + 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")) diff --git a/rise_motion_dev_ws/src/rise_motion/scripts/python_sdo_test.py b/rise_motion_dev_ws/src/py_sdo_serialization/scripts/temp_test_node.py similarity index 97% rename from rise_motion_dev_ws/src/rise_motion/scripts/python_sdo_test.py rename to rise_motion_dev_ws/src/py_sdo_serialization/scripts/temp_test_node.py index 62eb316..4251c78 100644 --- a/rise_motion_dev_ws/src/rise_motion/scripts/python_sdo_test.py +++ b/rise_motion_dev_ws/src/py_sdo_serialization/scripts/temp_test_node.py @@ -1,5 +1,8 @@ #!/usr/bin/env python3 +# execute this using +# python3 path/to/this/file + import sys import rclpy from rclpy.node import Node @@ -56,7 +59,7 @@ def _publish_cb(self): def request_enable_ethercat(self) -> bool: """Returns True on success (mirrors the C++ int == 0 success check).""" - self.get_logger().info("Incrementing motor position with %d", self.increment) + 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): @@ -149,7 +152,7 @@ def main(): type_name=p[3], ) - if result is -1: + 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)) From 4610a4a80b9d933f24e269affffc5dc5285eba26 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Mon, 1 Jun 2026 15:56:46 +0200 Subject: [PATCH 43/53] ec_manager.cpp: fix sdo_write --- rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp index 8b8cfd6..e03765d 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -329,7 +329,7 @@ bool ECManager::sdo_read(uint16 device_id, uint16 index, uint8 subindex, bool ECManager::sdo_write(uint16 device_id, uint16 index, uint8 subindex, std::vector &value) { int psize = value.size(); - uint8 *buf = new uint8[psize]; + uint8 *buf = &value[0]; boolean CA = FALSE; int wkc = ecx_SDOwrite(&ctx, device_id, index, subindex, CA, psize, From 978a5ac9f6e0fa5900f1e3f6501a4c9b52aa7818 Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 1 Jun 2026 17:50:42 +0200 Subject: [PATCH 44/53] encode VISIBLE_STRING as UTF-8 --- .../src/py_sdo_serialization/scripts/sdo_serializer.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py b/rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py index 7483b9a..0db3efa 100644 --- a/rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py +++ b/rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py @@ -343,17 +343,17 @@ def deserialize_time48(ser_val: list[int], bit_s: int) -> tuple[int]: def serialize_visible_string(val: str, bit_s: int) -> list[int]: """ - Serialize an ASCII encoded string into a 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("ASCII")) + return list(val.encode("UTF-8")) def deserialize_visible_string(ser_val: list[int], bit_s: int) -> str: """ - Deerialize a list[int] into an ASCII encoded string. + Deerialize a list[int] into an UTF-8 encoded string. """ - return bytes(ser_val).decode("ASCII") + return bytes(ser_val).decode("UTF-8") def serialize_unicode_string(val: str, bit_s: int) -> list[int]: """ From b83a7f59a1acb6a77a466f4cfd2a629d67a0c007 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Mon, 1 Jun 2026 18:58:48 +0000 Subject: [PATCH 45/53] adapt psize in sdo_read dynamically to actual value --- .../src/rise_motion/include/rise_motion/ec_manager.hpp | 2 +- rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp | 4 ++-- rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp | 2 +- rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp | 7 ++++--- .../src/rise_motion_messages/srv/SDOReadSrv.srv | 1 + 5 files changed, 9 insertions(+), 7 deletions(-) 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/src/ec_manager.cpp b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp index e03765d..6a63faa 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -309,8 +309,8 @@ uint16 ECManager::transition_ec(uint16 state) { } bool ECManager::sdo_read(uint16 device_id, uint16 index, uint8 subindex, - std::vector &value) { - int psize = 64; + std::vector &value, uint8 value_size) { + int psize = value_size; uint8 *buf = new uint8[psize]; boolean CA = FALSE; diff --git a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp index c979098..b67968c 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -118,7 +118,7 @@ void EthercatNode::sdoReadServiceCallback( std::vector value; bool success = ec_manager_.sdo_read(request->device_id, request->index, - request->subindex, value); + request->subindex, value, request->value_size); if (!success) { RCLCPP_WARN(get_logger(), "Read failed"); diff --git a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp index 7d8c887..4059f98 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp @@ -78,7 +78,7 @@ class TestNode : public rclcpp::Node { } template auto sdo_read( - uint16_t device_id, uint16_t index, uint8_t subindex, uint8_t value_type = 0) + uint16_t device_id, uint16_t index, uint8_t subindex, uint8_t value_size, uint8_t value_type = 0) { auto client = this->create_client("sdo_read"); @@ -94,6 +94,7 @@ class TestNode : public rclcpp::Node { request->device_id = device_id; request->index = index; request->subindex = subindex; + request->value_size = value_size; request->value_type = value_type; auto future = client->async_send_request(request); @@ -188,8 +189,8 @@ int main(int argc, char **argv) { while (!node->request_enable_ethercat()) { } - auto result = node->sdo_read>(1, 0x1008, 0, 0); - + // example sdo read + auto result = node->sdo_read>(1, 0x1008, 0, 0, 50); if(!result){ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), result.error.message.std::string::c_str()); } diff --git a/rise_motion_dev_ws/src/rise_motion_messages/srv/SDOReadSrv.srv b/rise_motion_dev_ws/src/rise_motion_messages/srv/SDOReadSrv.srv index e36b472..e3b9138 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/srv/SDOReadSrv.srv +++ b/rise_motion_dev_ws/src/rise_motion_messages/srv/SDOReadSrv.srv @@ -2,6 +2,7 @@ uint16 device_id uint16 index uint8 subindex uint8 value_type +uint8 value_size --- int8 status_code uint16 device_id From 72eb915edaf11630b6ed37fad646c3ec1f58ffbd Mon Sep 17 00:00:00 2001 From: Anton Date: Tue, 2 Jun 2026 12:08:15 +0200 Subject: [PATCH 46/53] fix CMakeLists.txt --- rise_motion_dev_ws/src/rise_motion/CMakeLists.txt | 7 ------- 1 file changed, 7 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index 50e035f..746862a 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -57,13 +57,6 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) -# Install Python modules -ament_python_install_package(${PROJECT_NAME}) -# Install Python executables -install(PROGRAMS - scripts/python_sdo_test.py - DESTINATION lib/${PROJECT_NAME} -) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) From 0d4443c420aef544f41163c662367411e35e71ab Mon Sep 17 00:00:00 2001 From: Anton Date: Tue, 2 Jun 2026 12:58:36 +0200 Subject: [PATCH 47/53] better function docstrings --- .../scripts/sdo_serializer.py | 51 ++++++++++++++----- 1 file changed, 39 insertions(+), 12 deletions(-) diff --git a/rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py b/rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py index 0db3efa..1006693 100644 --- a/rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py +++ b/rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py @@ -166,13 +166,28 @@ def serialize( 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] """ - Serializes any base data type from the tables 119 and 120 out of "ETG.1020 EtherCAT Protocol Enhancements". - Parameters: - - data to be serialized - - identifier of data's data type (index, name or base_data_type) - Refer to ETG.1000.6 and ETG.1020 at https://www.ethercat.org/en/downloads.html for details on encoding. - """ + try: object_info = get_type_info(index=index, name=name, base_data_type=base_data_type) @@ -195,12 +210,24 @@ def deserialize( name: str | None = None, base_data_type: str | None = None, ): - """ - Deserializes any base data type from the tables 119 and 120 out of "ETG.1020 EtherCAT Protocol Enhancements". - Parameters: - - data to be serialized - - identifier of data's data type (index, name or base_data_type) - Refer to ETG.1000.6 and ETG.1020 at https://www.ethercat.org/en/downloads.html for details on encoding. + """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 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): From 0280d9683ff54ba9b7dedb0cef889399feab8a4a Mon Sep 17 00:00:00 2001 From: Anton Date: Wed, 3 Jun 2026 13:40:11 +0200 Subject: [PATCH 48/53] provisional python package --- .../src/py_sdo_serializer/package.xml | 18 +++++++++++ .../py_sdo_serializer}/__init__.py | 0 .../py_sdo_serializer}/sdo_serializer.py | 0 .../py_sdo_serializer}/temp_test_node.py | 2 +- .../resource/py_sdo_serializer | 0 .../src/py_sdo_serializer/setup.cfg | 4 +++ .../src/py_sdo_serializer/setup.py | 30 +++++++++++++++++++ .../py_sdo_serializer/test/test_copyright.py | 25 ++++++++++++++++ .../src/py_sdo_serializer/test/test_flake8.py | 25 ++++++++++++++++ .../src/py_sdo_serializer/test/test_pep257.py | 23 ++++++++++++++ .../test/test_sdo_serializer.py | 2 +- .../src/rise_motion/CMakeLists.txt | 9 ------ 12 files changed, 127 insertions(+), 11 deletions(-) create mode 100644 rise_motion_dev_ws/src/py_sdo_serializer/package.xml rename rise_motion_dev_ws/src/{py_sdo_serialization/scripts => py_sdo_serializer/py_sdo_serializer}/__init__.py (100%) rename rise_motion_dev_ws/src/{py_sdo_serialization/scripts => py_sdo_serializer/py_sdo_serializer}/sdo_serializer.py (100%) rename rise_motion_dev_ws/src/{py_sdo_serialization/scripts => py_sdo_serializer/py_sdo_serializer}/temp_test_node.py (98%) create mode 100644 rise_motion_dev_ws/src/py_sdo_serializer/resource/py_sdo_serializer create mode 100644 rise_motion_dev_ws/src/py_sdo_serializer/setup.cfg create mode 100644 rise_motion_dev_ws/src/py_sdo_serializer/setup.py create mode 100644 rise_motion_dev_ws/src/py_sdo_serializer/test/test_copyright.py create mode 100644 rise_motion_dev_ws/src/py_sdo_serializer/test/test_flake8.py create mode 100644 rise_motion_dev_ws/src/py_sdo_serializer/test/test_pep257.py rename rise_motion_dev_ws/src/{rise_motion => py_sdo_serializer}/test/test_sdo_serializer.py (99%) 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_serialization/scripts/__init__.py b/rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/__init__.py similarity index 100% rename from rise_motion_dev_ws/src/py_sdo_serialization/scripts/__init__.py rename to rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/__init__.py diff --git a/rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py b/rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/sdo_serializer.py similarity index 100% rename from rise_motion_dev_ws/src/py_sdo_serialization/scripts/sdo_serializer.py rename to rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/sdo_serializer.py diff --git a/rise_motion_dev_ws/src/py_sdo_serialization/scripts/temp_test_node.py b/rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/temp_test_node.py similarity index 98% rename from rise_motion_dev_ws/src/py_sdo_serialization/scripts/temp_test_node.py rename to rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/temp_test_node.py index 4251c78..d80bbbe 100644 --- a/rise_motion_dev_ws/src/py_sdo_serialization/scripts/temp_test_node.py +++ b/rise_motion_dev_ws/src/py_sdo_serializer/py_sdo_serializer/temp_test_node.py @@ -8,7 +8,7 @@ from rclpy.node import Node from rise_motion_messages.msg import MotorPositions from rise_motion_messages.srv import EnableEthercatSrv, SDOReadSrv -from sdo_serializer import deserialize, serialize +from rise_motion_dev_ws.src.py_sdo_serialization.sdo_serializer import deserialize, serialize class TestNode(Node): 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/rise_motion/test/test_sdo_serializer.py b/rise_motion_dev_ws/src/py_sdo_serializer/test/test_sdo_serializer.py similarity index 99% rename from rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py rename to rise_motion_dev_ws/src/py_sdo_serializer/test/test_sdo_serializer.py index cae2eb8..26d596e 100644 --- a/rise_motion_dev_ws/src/rise_motion/test/test_sdo_serializer.py +++ b/rise_motion_dev_ws/src/py_sdo_serializer/test/test_sdo_serializer.py @@ -3,7 +3,7 @@ import random from math import isnan import uuid -from rise_motion.sdo_serializer import serialize, deserialize +from py_sdo_serializer.sdo_serializer import serialize, deserialize # --------------------------------------------------------------------------- # Bit strings BIT1 - BIT16 tests diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index 746862a..cf8256b 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -7,9 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclpy REQUIRED) find_package(rise_motion_messages REQUIRED) include_directories(include) @@ -61,7 +59,6 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) - find_package(ament_cmake_pytest 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) @@ -70,12 +67,6 @@ if(BUILD_TESTING) # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() - - # --- sdo_serializer test --- - ament_add_pytest_test(test_sdo_serializer_python - test/test_sdo_serializer.py - ) - endif() ament_package() From 14d4211960ae0ffef5422a257a2d08a6f83ba1cd Mon Sep 17 00:00:00 2001 From: Anton Date: Wed, 3 Jun 2026 14:19:16 +0200 Subject: [PATCH 49/53] update tests ASCII -> UTF-8 --- .../test/test_sdo_serializer.py | 27 ++++++++++--------- 1 file changed, 14 insertions(+), 13 deletions(-) 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 index 26d596e..061f8aa 100644 --- 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 @@ -2,7 +2,6 @@ import pytest import random from math import isnan -import uuid from py_sdo_serializer.sdo_serializer import serialize, deserialize # --------------------------------------------------------------------------- @@ -428,13 +427,16 @@ def test_serialize_known_values_visible_string(value: str, expected: list[int]): # --- 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"), + ([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") @@ -443,12 +445,11 @@ def test_deserialize_known_values_visible_string(serialized: list[int], expected # --- Invalid input --- @pytest.mark.parametrize("value", [ - "café", # non-ASCII (é is > 0x7F, invalid for VISIBLE_STRING) - "日本語", # CJK characters - "😀", # emoji + ["h", "b"], + 5 ]) -def test_serialize_visible_string_rejects_non_ascii(value: str): - """Characters outside the visible ASCII range should raise.""" +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") From 59a2c7e9c7dde98d209fe55a2405679082a05728 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Wed, 3 Jun 2026 14:30:10 +0200 Subject: [PATCH 50/53] sdo_serializer.hpp: fix typo in data type alias --- .../src/rise_motion/include/rise_motion/sdo_serializer.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 0f234ac..5998870 100644 --- 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 @@ -308,7 +308,7 @@ namespace sdo template using VISIBLE_STRING = STRING; template using UNICODE_STRING = WSTRING; - template using OKTET_STRING = ARRAY; + template using OCTET_STRING = ARRAY; template using ARRAY_OF_USINT = ARRAY; template using ARRAY_OF_UINT = ARRAY; template using ARRAY_OF_INT = ARRAY; From d0844422be363c30c3a5bcf0132e7f83dd31114c Mon Sep 17 00:00:00 2001 From: Anton Date: Wed, 3 Jun 2026 14:41:37 +0200 Subject: [PATCH 51/53] replace double quotes --- .../py_sdo_serializer/sdo_serializer.py | 160 +++++++++--------- 1 file changed, 80 insertions(+), 80 deletions(-) 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 index 1006693..516c58c 100644 --- 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 @@ -8,8 +8,8 @@ @dataclass(frozen=True) class TypeInfo: 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") + 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 @@ -22,83 +22,83 @@ class TypeInfo: 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"), + 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"), + 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"), + 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"), + 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"), + 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"), + 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"), + 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"), + 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) + 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) + 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) } @@ -131,8 +131,8 @@ def get_type_info( Examples -------- get_type_info(index=0x0003) - get_type_info(name="INTEGER16") - get_type_info(base_data_type="INT") + get_type_info(name='INTEGER16') + get_type_info(base_data_type='INT') """ if index is not None: try: @@ -152,7 +152,7 @@ def get_type_info( 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") + raise ValueError('Provide at least one of: index, name, base_data_type') # --------------------------------------------------------------------------- @@ -181,7 +181,7 @@ def serialize( :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. + 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: @@ -192,7 +192,7 @@ def serialize( 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": + 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: @@ -225,7 +225,7 @@ def deserialize( :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. + 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. """ @@ -234,14 +234,14 @@ def deserialize( 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" + 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 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.") + '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): @@ -274,7 +274,7 @@ def serialize_bitn(val, bit_s: int) -> list[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")) + return list(val.to_bytes(byte_len, byteorder='little')) def deserialize_bitn(ser_val: list[int], bit_s: int) -> str: """ @@ -290,7 +290,7 @@ def serialize_int(val: int, bit_s: int) -> 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)) + return list(val.to_bytes(byte_len, byteorder='little', signed=True)) def deserialize_int(ser_val: list[int], bit_s: int) -> int: """ @@ -305,7 +305,7 @@ def serialize_uint(val: int, bit_s: int) -> 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")) + return list(val.to_bytes(byte_len, byteorder='little')) def deserialize_uint(ser_val: list[int], bit_s: int) -> int: """ @@ -351,8 +351,8 @@ def serialize_time_of_day(val, bit_s: int) -> list[int]: 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")) + 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]: """ @@ -360,13 +360,13 @@ def serialize_time_difference(val, bit_s: int) -> list[int]: """ 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")) + 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]: """ Takes list[int] and returns a tuple containing ms and days """ - return (int.from_bytes(ser_val[:4], byteorder="big"), int.from_bytes(ser_val[4:], byteorder="big")) + 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]: """ @@ -374,13 +374,13 @@ def serialize_visible_string(val: str, bit_s: int) -> 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")) + return list(val.encode('UTF-8')) def deserialize_visible_string(ser_val: list[int], bit_s: int) -> str: """ Deerialize a list[int] into an UTF-8 encoded string. """ - return bytes(ser_val).decode("UTF-8") + return bytes(ser_val).decode('UTF-8') def serialize_unicode_string(val: str, bit_s: int) -> list[int]: """ @@ -388,10 +388,10 @@ def serialize_unicode_string(val: str, bit_s: int) -> 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")) + 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") + return bytes(ser_val).decode('utf_16_le') From a56baef1d3e3d573988d3874ca109d64f79bda29 Mon Sep 17 00:00:00 2001 From: elo0elo <195834261+elo0elo@users.noreply.github.com> Date: Wed, 3 Jun 2026 17:40:14 +0000 Subject: [PATCH 52/53] make sdo_serializer.hpp available to downstream packages --- .../src/rise_motion/CMakeLists.txt | 19 ++++++++++++++++++- .../include/rise_motion/sdo_serializer.hpp | 2 +- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index 3f49c4f..a10458d 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -16,7 +16,10 @@ add_subdirectory(SOEM) # --- sdo_serializer library --- add_library(sdo_serializer_lib INTERFACE) -target_include_directories(sdo_serializer_lib INTERFACE include) +target_include_directories(sdo_serializer_lib INTERFACE + $ + $ +) add_executable( rise_motion_main @@ -76,4 +79,18 @@ if(BUILD_TESTING) 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/sdo_serializer.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/sdo_serializer.hpp index 5998870..ca6d80a 100644 --- 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 @@ -10,7 +10,7 @@ #include #include #include -#include "result.hpp" +#include namespace sdo { From 1860e172c3a6ae7413e691c20efe895c62b2e365 Mon Sep 17 00:00:00 2001 From: Anton Date: Wed, 24 Jun 2026 12:16:14 +0200 Subject: [PATCH 53/53] more docstrings + formatting --- .../py_sdo_serializer/sdo_serializer.py | 151 +++++++++--------- 1 file changed, 79 insertions(+), 72 deletions(-) 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 index 516c58c..a9d00c5 100644 --- 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 @@ -1,12 +1,27 @@ -from dataclasses import dataclass +"""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') @@ -22,14 +37,14 @@ class TypeInfo: 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'), + 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'), + 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'), @@ -83,9 +98,9 @@ class TypeInfo: # - 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) - + 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) @@ -138,19 +153,25 @@ def get_type_info( try: return BASE_DATA_TYPES[index] except KeyError: - raise KeyError(f"No EtherCAT type with index {index:#04x}") from None + 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 + 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 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') @@ -171,8 +192,8 @@ def serialize( 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 + 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. @@ -181,24 +202,26 @@ def serialize( :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. + 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) + 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)) + out.extend(globals()[object_info.serialize_fn]( + item, object_info.bit_size)) return out except: return -1 @@ -215,8 +238,8 @@ def deserialize( 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 + 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. @@ -225,7 +248,8 @@ def deserialize( :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. + 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. """ @@ -264,10 +288,9 @@ def deserialize( # Specific functions (called by generic functions) # --------------------------------------------------------------------------- + def serialize_bitn(val, bit_s: int) -> list[int]: - """ - Serialize a bit-string into a list[int]. - """ + """Serialize a bit-string into a list[int].""" if isinstance(val, str): val = int(val, 2) elif not isinstance(val, int): @@ -276,47 +299,41 @@ def serialize_bitn(val, bit_s: int) -> list[int]: 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. - """ + """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]. - """ + """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. - """ + """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]. - """ + """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. - """ + """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]. - """ + """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: @@ -324,74 +341,64 @@ def serialize_bool(val, bit_s: int) -> list[int]: 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. - """ + """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]. - """ + """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. - """ + """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]: - """ - Takes a tuple of ints containing ms since midnight and days since 01.01.1984 - """ + """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]: - """ - Takes a tuple of ints containing ms and days - """ + """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]: - """ - Takes list[int] and returns a tuple containing ms and days - """ + """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]. - """ + """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: - """ - Deerialize a list[int] into an UTF-8 encoded string. - """ + """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]. - """ + """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. - """ + """Deerialize a list[int] into a utf_16_le encoded string.""" return bytes(ser_val).decode('utf_16_le')