From a7cc61ca8c11afb45475723dc5950027e397548f Mon Sep 17 00:00:00 2001 From: Anton Date: Fri, 22 May 2026 17:48:39 +0200 Subject: [PATCH 01/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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 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 13/25] 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 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 14/25] 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 15/25] 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 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 16/25] 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 17/25] 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 18/25] 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 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 19/25] 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 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 20/25] 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 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 21/25] 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 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 22/25] 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 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 23/25] 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 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 24/25] 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 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 25/25] 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 {