From a5b3438e9554197b5d172a228837ba20f509e875 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 23 Apr 2026 08:30:09 +0200 Subject: [PATCH 1/3] Add parsing masterboard data --- CMakeLists.txt | 3 +- .../primary/abstract_primary_consumer.h | 2 + .../primary/primary_consumer.h | 17 ++ .../primary/primary_parser.h | 7 +- .../primary/robot_state/masterboard_data.h | 135 +++++++++++++ src/primary/robot_state/masterboard_data.cpp | 146 ++++++++++++++ tests/test_primary_parser.cpp | 180 ++++++++++++++++++ 7 files changed, 485 insertions(+), 5 deletions(-) create mode 100644 include/ur_client_library/primary/robot_state/masterboard_data.h create mode 100644 src/primary/robot_state/masterboard_data.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1226dfc30..693d6780d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,6 +38,7 @@ add_library(urcl src/primary/robot_state/kinematics_info.cpp src/primary/robot_state/robot_mode_data.cpp src/primary/robot_state/configuration_data.cpp + src/primary/robot_state/masterboard_data.cpp src/rtde/control_package_pause.cpp src/rtde/control_package_setup_inputs.cpp src/rtde/control_package_setup_outputs.cpp @@ -190,4 +191,4 @@ file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/ros_package_pa install(FILES ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/ros_package_path.dsv DESTINATION share/${PROJECT_NAME}/hook) #### End iport #### -install(DIRECTORY resources DESTINATION share/${PROJECT_NAME}) \ No newline at end of file +install(DIRECTORY resources DESTINATION share/${PROJECT_NAME}) diff --git a/include/ur_client_library/primary/abstract_primary_consumer.h b/include/ur_client_library/primary/abstract_primary_consumer.h index 9bc558f7d..fdb545dd9 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -36,6 +36,7 @@ #include "ur_client_library/primary/robot_state/kinematics_info.h" #include "ur_client_library/primary/robot_state/robot_mode_data.h" #include "ur_client_library/primary/robot_state/configuration_data.h" +#include "ur_client_library/primary/robot_state/masterboard_data.h" #include "ur_client_library/primary/robot_message/safety_mode_message.h" namespace urcl @@ -80,6 +81,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer virtual bool consume(ErrorCodeMessage& pkg) = 0; virtual bool consume(RobotModeData& pkg) = 0; virtual bool consume(ConfigurationData& pkg) = 0; + virtual bool consume(MasterboardData& pkg) = 0; virtual bool consume(SafetyModeMessage& pkg) = 0; private: diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h index 824bbf18f..f8e65e159 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -30,6 +30,7 @@ #include "ur_client_library/primary/abstract_primary_consumer.h" #include "ur_client_library/primary/robot_state/robot_mode_data.h" +#include "ur_client_library/primary/robot_state/masterboard_data.h" #include "ur_client_library/ur/datatypes.h" #include "ur_client_library/ur/version_information.h" @@ -185,6 +186,20 @@ class PrimaryConsumer : public AbstractPrimaryConsumer return true; } + /*! + * \brief Handle a MasterboardData package + * + * \param pkg MasterboardData + * + * \returns True + */ + virtual bool consume(MasterboardData& pkg) override + { + std::scoped_lock lock(masterboard_data_mutex_); + masterboard_data_ = std::make_shared(pkg); + return true; + } + /*! * \brief Set callback function which will be triggered whenever error code messages are received * @@ -267,6 +282,8 @@ class PrimaryConsumer : public AbstractPrimaryConsumer std::mutex configuration_data_mutex_; std::mutex safety_mode_mutex_; std::shared_ptr safety_mode_; + std::mutex masterboard_data_mutex_; + std::shared_ptr masterboard_data_; }; } // namespace primary_interface diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index 7f6833295..e284aa3c3 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -34,6 +34,7 @@ #include "ur_client_library/primary/robot_message/error_code_message.h" #include "ur_client_library/primary/robot_state/robot_mode_data.h" #include "ur_client_library/primary/robot_state/configuration_data.h" +#include "ur_client_library/primary/robot_state/masterboard_data.h" #include "ur_client_library/primary/robot_message/safety_mode_message.h" namespace urcl @@ -209,10 +210,8 @@ class PrimaryParser : public comm::Parser { case RobotStateType::ROBOT_MODE_DATA: return new RobotModeData(type); - - // return new rmd; - // case robot_state_type::MASTERBOARD_DATA: - // return new MBD;*/ + case RobotStateType::MASTERBOARD_DATA: + return new MasterboardData(type); case RobotStateType::KINEMATICS_INFO: return new KinematicsInfo(type); case RobotStateType::CONFIGURATION_DATA: diff --git a/include/ur_client_library/primary/robot_state/masterboard_data.h b/include/ur_client_library/primary/robot_state/masterboard_data.h new file mode 100644 index 000000000..332b5c6e6 --- /dev/null +++ b/include/ur_client_library/primary/robot_state/masterboard_data.h @@ -0,0 +1,135 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_MASTERBOARD_DATA_H_INCLUDED +#define UR_CLIENT_LIBRARY_MASTERBOARD_DATA_H_INCLUDED + +#include + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +#include "ur_client_library/ur/datatypes.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief The MasterboardData class handles the masterboard data sub-package sent as part of the + * Robot State message on the primary UR interface. + * + * This package contains information about the control box IOs, analog inputs/outputs, voltages, + * currents, temperatures, the safety mode, as well as information about the optional IMMI + * interface. See the Universal Robots primary/secondary guide for details: + * https://docs.universal-robots.com/tutorials/communication-protocol-tutorials/primary-secondary-guide.html + */ +class MasterboardData : public RobotState +{ +public: + MasterboardData() = delete; + + /*! + * \brief Creates a new MasterboardData object. + * + * \param type The type of RobotState message received + */ + MasterboardData(const RobotStateType type) : RobotState(type) + { + } + + /*! + * \brief Creates a copy of a MasterboardData object. + * + * \param pkg The MasterboardData object to be copied + */ + MasterboardData(const MasterboardData& pkg); + + virtual ~MasterboardData() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this specific package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + std::bitset<32> digital_input_bits_; + std::bitset<32> digital_output_bits_; + uint8_t analog_input_range_0_; + uint8_t analog_input_range_1_; + double analog_input_0_; + double analog_input_1_; + char analog_output_domain_0_; + char analog_output_domain_1_; + double analog_output_0_; + double analog_output_1_; + float master_board_temperature_; + float robot_voltage_48v_; + float robot_current_; + float master_io_current_; + SafetyMode safety_mode_; + bool in_reduced_mode_; + bool immi_interface_installed_{ false }; + + // The following four fields are only valid if immi_interface_installed_ is true. + std::bitset<32> immi_input_bits_; + std::bitset<32> immi_output_bits_; + float immi_voltage_24v_{ 0.0f }; + float immi_current_{ 0.0f }; + + uint32_t reserved_1_{ 0 }; + uint8_t operational_mode_selector_input_{ 0 }; + bool three_position_enabling_device_input_{ false }; + uint8_t reserved_2_{ 0 }; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_MASTERBOARD_DATA_H_INCLUDED diff --git a/src/primary/robot_state/masterboard_data.cpp b/src/primary/robot_state/masterboard_data.cpp new file mode 100644 index 000000000..491bc0eb2 --- /dev/null +++ b/src/primary/robot_state/masterboard_data.cpp @@ -0,0 +1,146 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/primary/robot_state/masterboard_data.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +MasterboardData::MasterboardData(const MasterboardData& pkg) : RobotState(RobotStateType::MASTERBOARD_DATA) +{ + digital_input_bits_ = pkg.digital_input_bits_; + digital_output_bits_ = pkg.digital_output_bits_; + analog_input_range_0_ = pkg.analog_input_range_0_; + analog_input_range_1_ = pkg.analog_input_range_1_; + analog_input_0_ = pkg.analog_input_0_; + analog_input_1_ = pkg.analog_input_1_; + analog_output_domain_0_ = pkg.analog_output_domain_0_; + analog_output_domain_1_ = pkg.analog_output_domain_1_; + analog_output_0_ = pkg.analog_output_0_; + analog_output_1_ = pkg.analog_output_1_; + master_board_temperature_ = pkg.master_board_temperature_; + robot_voltage_48v_ = pkg.robot_voltage_48v_; + robot_current_ = pkg.robot_current_; + master_io_current_ = pkg.master_io_current_; + safety_mode_ = pkg.safety_mode_; + in_reduced_mode_ = pkg.in_reduced_mode_; + immi_interface_installed_ = pkg.immi_interface_installed_; + immi_input_bits_ = pkg.immi_input_bits_; + immi_output_bits_ = pkg.immi_output_bits_; + immi_voltage_24v_ = pkg.immi_voltage_24v_; + immi_current_ = pkg.immi_current_; + reserved_1_ = pkg.reserved_1_; + operational_mode_selector_input_ = pkg.operational_mode_selector_input_; + three_position_enabling_device_input_ = pkg.three_position_enabling_device_input_; + reserved_2_ = pkg.reserved_2_; +} + +bool MasterboardData::parseWith(comm::BinParser& bp) +{ + bp.parse(digital_input_bits_); + bp.parse(digital_output_bits_); + bp.parse(analog_input_range_0_); + bp.parse(analog_input_range_1_); + bp.parse(analog_input_0_); + bp.parse(analog_input_1_); + bp.parse(analog_output_domain_0_); + bp.parse(analog_output_domain_1_); + bp.parse(analog_output_0_); + bp.parse(analog_output_1_); + bp.parse(master_board_temperature_); + bp.parse(robot_voltage_48v_); + bp.parse(robot_current_); + bp.parse(master_io_current_); + bp.parse(safety_mode_); + bp.parse(in_reduced_mode_); + bp.parse(immi_interface_installed_); + + if (immi_interface_installed_) + { + bp.parse(immi_input_bits_); + bp.parse(immi_output_bits_); + bp.parse(immi_voltage_24v_); + bp.parse(immi_current_); + } + + bp.parse(reserved_1_); + bp.parse(operational_mode_selector_input_); + bp.parse(three_position_enabling_device_input_); + bp.parse(reserved_2_); + + return true; +} + +bool MasterboardData::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string MasterboardData::toString() const +{ + std::stringstream os; + os << "MasterboardData:" << std::endl; + os << "Digital input bits: 0b" << digital_input_bits_ << std::endl; + os << "Digital output bits: 0b" << digital_output_bits_ << std::endl; + os << "Analog input range 0: " << unsigned(analog_input_range_0_) << std::endl; + os << "Analog input range 1: " << unsigned(analog_input_range_1_) << std::endl; + os << "Analog input 0: " << analog_input_0_ << std::endl; + os << "Analog input 1: " << analog_input_1_ << std::endl; + os << "Analog output domain 0: " << static_cast(analog_output_domain_0_) << std::endl; + os << "Analog output domain 1: " << static_cast(analog_output_domain_1_) << std::endl; + os << "Analog output 0: " << analog_output_0_ << std::endl; + os << "Analog output 1: " << analog_output_1_ << std::endl; + os << "Masterboard temperature: " << master_board_temperature_ << std::endl; + os << "Robot voltage 48V: " << robot_voltage_48v_ << std::endl; + os << "Robot current: " << robot_current_ << std::endl; + os << "Master I/O current: " << master_io_current_ << std::endl; + os << "Safety mode: " << safetyModeString(safety_mode_) << std::endl; + os << "In reduced mode: " << in_reduced_mode_ << std::endl; + os << "IMMI interface installed: " << immi_interface_installed_ << std::endl; + if (immi_interface_installed_) + { + os << "IMMI input bits: 0b" << immi_input_bits_ << std::endl; + os << "IMMI output bits: 0b" << immi_output_bits_ << std::endl; + os << "IMMI voltage 24V: " << immi_voltage_24v_ << std::endl; + os << "IMMI current: " << immi_current_ << std::endl; + } + os << "Operational mode selector input: " << unsigned(operational_mode_selector_input_) << std::endl; + os << "Three position enabling device input: " << three_position_enabling_device_input_ << std::endl; + + return os.str(); +} + +} // namespace primary_interface +} // namespace urcl diff --git a/tests/test_primary_parser.cpp b/tests/test_primary_parser.cpp index 91468dbcd..359b105ce 100644 --- a/tests/test_primary_parser.cpp +++ b/tests/test_primary_parser.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include "ur_client_library/primary/robot_message/key_message.h" using namespace urcl; @@ -75,6 +76,75 @@ std::vector makeRobotStatePacketWithConfigurationSubmessage(const std:: assert(offset == packet.size()); return packet; } + +// Masterboard payload sizes (not including packageSize + packageType header). +// Base payload up to and including `immiInterfaceInstalled` flag. +constexpr size_t kMasterboardPayloadBytesBeforeImmi = 2 * sizeof(uint32_t) + // digital input/output bits + 2 * sizeof(uint8_t) + // analog input ranges + 2 * sizeof(double) + // analog inputs + 2 * sizeof(char) + // analog output domains + 2 * sizeof(double) + // analog outputs + 4 * sizeof(float) + // temperature + voltages + currents + sizeof(uint8_t) + // safety mode + sizeof(uint8_t) + // in reduced mode (bool) + sizeof(uint8_t); // immi interface installed (bool) +constexpr size_t kMasterboardOptionalImmiBytes = 2 * sizeof(uint32_t) + 2 * sizeof(float); +constexpr size_t kMasterboardPayloadBytesAfterImmi = sizeof(uint32_t) + // reserved + sizeof(uint8_t) + // operational mode selector input + sizeof(uint8_t) + // three position enabling device (bool) + sizeof(uint8_t); // reserved + +std::vector makeRobotStatePacketWithMasterboardSubmessage(const std::vector& masterboard_payload) +{ + const uint32_t sub_size = static_cast(sizeof(uint32_t) + sizeof(uint8_t) + masterboard_payload.size()); + const uint32_t total_packet_size = static_cast(sizeof(int32_t) + sizeof(uint8_t) + sub_size); + + std::vector packet(total_packet_size); + size_t offset = 0; + packet[offset++] = static_cast((total_packet_size >> 24) & 0xFF); + packet[offset++] = static_cast((total_packet_size >> 16) & 0xFF); + packet[offset++] = static_cast((total_packet_size >> 8) & 0xFF); + packet[offset++] = static_cast(total_packet_size & 0xFF); + packet[offset++] = static_cast(primary_interface::RobotPackageType::ROBOT_STATE); + + packet[offset++] = static_cast((sub_size >> 24) & 0xFF); + packet[offset++] = static_cast((sub_size >> 16) & 0xFF); + packet[offset++] = static_cast((sub_size >> 8) & 0xFF); + packet[offset++] = static_cast(sub_size & 0xFF); + packet[offset++] = static_cast(primary_interface::RobotStateType::MASTERBOARD_DATA); + + memcpy(packet.data() + offset, masterboard_payload.data(), masterboard_payload.size()); + offset += masterboard_payload.size(); + assert(offset == packet.size()); + return packet; +} + +// Helpers for big-endian serialization of primitive values used by masterboard payload tests. +template +void appendBigEndian(std::vector& buffer, T value) +{ + uint8_t bytes[sizeof(T)]; + std::memcpy(bytes, &value, sizeof(T)); + // Host may be little-endian; the UR wire format is big-endian. + for (size_t i = 0; i < sizeof(T); ++i) + { + buffer.push_back(bytes[sizeof(T) - 1 - i]); + } +} + +void appendBigEndianDouble(std::vector& buffer, double value) +{ + uint64_t inner; + std::memcpy(&inner, &value, sizeof(double)); + appendBigEndian(buffer, inner); +} + +void appendBigEndianFloat(std::vector& buffer, float value) +{ + uint32_t inner; + std::memcpy(&inner, &value, sizeof(float)); + appendBigEndian(buffer, inner); +} } // namespace /* First RobotState of UR5e from URSim v5.8 @@ -628,6 +698,116 @@ TEST_F(PrimaryParserTest, parse_configuration_data_with_reserved_fields) EXPECT_EQ(config->reserved_2_, static_cast(0x5678)); } +TEST_F(PrimaryParserTest, parse_masterboard_data_without_immi) +{ + std::vector payload; + payload.reserve(kMasterboardPayloadBytesBeforeImmi + kMasterboardPayloadBytesAfterImmi); + + appendBigEndian(payload, 0xDEADBEEFu); // digital input bits + appendBigEndian(payload, 0xCAFEBABEu); // digital output bits + payload.push_back(0x01); // analog input range 0 + payload.push_back(0x02); // analog input range 1 + appendBigEndianDouble(payload, 1.5); // analog input 0 + appendBigEndianDouble(payload, -2.25); // analog input 1 + payload.push_back(0x03); // analog output domain 0 + payload.push_back(0x04); // analog output domain 1 + appendBigEndianDouble(payload, 3.75); // analog output 0 + appendBigEndianDouble(payload, -4.5); // analog output 1 + appendBigEndianFloat(payload, 42.0f); // master board temperature + appendBigEndianFloat(payload, 48.1f); // robot voltage 48V + appendBigEndianFloat(payload, 1.23f); // robot current + appendBigEndianFloat(payload, 0.5f); // master IO current + payload.push_back(0x01); // safety mode (NORMAL) + payload.push_back(0x00); // in reduced mode (false) + payload.push_back(0x00); // immi interface installed (false) + appendBigEndian(payload, 0u); // reserved + payload.push_back(0x02); // operational mode selector input + payload.push_back(0x01); // three position enabling device input (true) + payload.push_back(0x00); // reserved + + ASSERT_EQ(payload.size(), kMasterboardPayloadBytesBeforeImmi + kMasterboardPayloadBytesAfterImmi); + + std::vector packet = makeRobotStatePacketWithMasterboardSubmessage(payload); + comm::BinParser bp(packet.data(), packet.size()); + std::vector> products; + ASSERT_TRUE(parser_.parse(bp, products)); + ASSERT_EQ(products.size(), 1u); + + auto* data = dynamic_cast(products[0].get()); + ASSERT_NE(data, nullptr); + EXPECT_EQ(data->digital_input_bits_, std::bitset<32>(0xDEADBEEFu)); + EXPECT_EQ(data->digital_output_bits_, std::bitset<32>(0xCAFEBABEu)); + EXPECT_EQ(data->analog_input_range_0_, 0x01); + EXPECT_EQ(data->analog_input_range_1_, 0x02); + EXPECT_DOUBLE_EQ(data->analog_input_0_, 1.5); + EXPECT_DOUBLE_EQ(data->analog_input_1_, -2.25); + EXPECT_EQ(static_cast(data->analog_output_domain_0_), 0x03); + EXPECT_EQ(static_cast(data->analog_output_domain_1_), 0x04); + EXPECT_DOUBLE_EQ(data->analog_output_0_, 3.75); + EXPECT_DOUBLE_EQ(data->analog_output_1_, -4.5); + EXPECT_FLOAT_EQ(data->master_board_temperature_, 42.0f); + EXPECT_FLOAT_EQ(data->robot_voltage_48v_, 48.1f); + EXPECT_FLOAT_EQ(data->robot_current_, 1.23f); + EXPECT_FLOAT_EQ(data->master_io_current_, 0.5f); + EXPECT_EQ(data->safety_mode_, SafetyMode::NORMAL); + EXPECT_FALSE(data->in_reduced_mode_); + EXPECT_FALSE(data->immi_interface_installed_); + EXPECT_EQ(data->operational_mode_selector_input_, 2); + EXPECT_TRUE(data->three_position_enabling_device_input_); +} + +TEST_F(PrimaryParserTest, parse_masterboard_data_with_immi) +{ + std::vector payload; + payload.reserve(kMasterboardPayloadBytesBeforeImmi + kMasterboardOptionalImmiBytes + + kMasterboardPayloadBytesAfterImmi); + + appendBigEndian(payload, 0x00000001u); + appendBigEndian(payload, 0x00000002u); + payload.push_back(0x00); + payload.push_back(0x00); + appendBigEndianDouble(payload, 0.0); + appendBigEndianDouble(payload, 0.0); + payload.push_back(0x00); + payload.push_back(0x00); + appendBigEndianDouble(payload, 0.0); + appendBigEndianDouble(payload, 0.0); + appendBigEndianFloat(payload, 20.0f); + appendBigEndianFloat(payload, 48.0f); + appendBigEndianFloat(payload, 0.0f); + appendBigEndianFloat(payload, 0.0f); + payload.push_back(0x02); // safety mode (REDUCED) + payload.push_back(0x01); // in reduced mode (true) + payload.push_back(0x01); // immi interface installed (true) + appendBigEndian(payload, 0x11112222u); // immi input bits + appendBigEndian(payload, 0x33334444u); // immi output bits + appendBigEndianFloat(payload, 24.0f); // immi voltage 24V + appendBigEndianFloat(payload, 0.75f); // immi current + appendBigEndian(payload, 0u); // reserved + payload.push_back(0x00); + payload.push_back(0x00); + payload.push_back(0x00); + + ASSERT_EQ(payload.size(), + kMasterboardPayloadBytesBeforeImmi + kMasterboardOptionalImmiBytes + kMasterboardPayloadBytesAfterImmi); + + std::vector packet = makeRobotStatePacketWithMasterboardSubmessage(payload); + comm::BinParser bp(packet.data(), packet.size()); + std::vector> products; + ASSERT_TRUE(parser_.parse(bp, products)); + ASSERT_EQ(products.size(), 1u); + + auto* data = dynamic_cast(products[0].get()); + ASSERT_NE(data, nullptr); + EXPECT_EQ(data->safety_mode_, SafetyMode::REDUCED); + EXPECT_TRUE(data->in_reduced_mode_); + EXPECT_TRUE(data->immi_interface_installed_); + EXPECT_EQ(data->immi_input_bits_, std::bitset<32>(0x11112222u)); + EXPECT_EQ(data->immi_output_bits_, std::bitset<32>(0x33334444u)); + EXPECT_FLOAT_EQ(data->immi_voltage_24v_, 24.0f); + EXPECT_FLOAT_EQ(data->immi_current_, 0.75f); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From 4fbe84ea8534fbf00ecab582400f3e045a68f09e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 4 May 2026 21:49:40 +0200 Subject: [PATCH 2/3] Add test for IMMI to string --- src/primary/robot_state/masterboard_data.cpp | 1 + tests/test_primary_parser.cpp | 84 ++++++++++++-------- 2 files changed, 52 insertions(+), 33 deletions(-) diff --git a/src/primary/robot_state/masterboard_data.cpp b/src/primary/robot_state/masterboard_data.cpp index 491bc0eb2..731509a8c 100644 --- a/src/primary/robot_state/masterboard_data.cpp +++ b/src/primary/robot_state/masterboard_data.cpp @@ -111,6 +111,7 @@ bool MasterboardData::consumeWith(AbstractPrimaryConsumer& consumer) std::string MasterboardData::toString() const { std::stringstream os; + os << std::boolalpha; os << "MasterboardData:" << std::endl; os << "Digital input bits: 0b" << digital_input_bits_ << std::endl; os << "Digital output bits: 0b" << digital_output_bits_ << std::endl; diff --git a/tests/test_primary_parser.cpp b/tests/test_primary_parser.cpp index 359b105ce..114612a32 100644 --- a/tests/test_primary_parser.cpp +++ b/tests/test_primary_parser.cpp @@ -44,13 +44,14 @@ using namespace urcl; namespace { // ConfigurationData payload before optional int16 reserved fields (older UR software). -constexpr size_t kConfigurationDataPayloadBytes = 6 * 2 * sizeof(double) + // joint position limits - 6 * 2 * sizeof(double) + // joint motion limits - 5 * sizeof(double) + // v/a joint/tool defaults + eq_radius - 4 * 6 * sizeof(double) + // dh_a, dh_d, dh_alpha, dh_theta - 4 * sizeof(int32_t); // masterboard, controller, robot type, sub type +constexpr size_t CONFIGURATION_DATA_PAYLOAD_BYTES = + 6 * 2 * sizeof(double) + // joint position limits + 6 * 2 * sizeof(double) + // joint motion limits + 5 * sizeof(double) + // v/a joint/tool defaults + eq_radius + 4 * 6 * sizeof(double) + // dh_a, dh_d, dh_alpha, dh_theta + 4 * sizeof(int32_t); // masterboard, controller, robot type, sub type -static_assert(kConfigurationDataPayloadBytes == 440); +static_assert(CONFIGURATION_DATA_PAYLOAD_BYTES == 440); std::vector makeRobotStatePacketWithConfigurationSubmessage(const std::vector& configuration_payload) { @@ -79,20 +80,20 @@ std::vector makeRobotStatePacketWithConfigurationSubmessage(const std:: // Masterboard payload sizes (not including packageSize + packageType header). // Base payload up to and including `immiInterfaceInstalled` flag. -constexpr size_t kMasterboardPayloadBytesBeforeImmi = 2 * sizeof(uint32_t) + // digital input/output bits - 2 * sizeof(uint8_t) + // analog input ranges - 2 * sizeof(double) + // analog inputs - 2 * sizeof(char) + // analog output domains - 2 * sizeof(double) + // analog outputs - 4 * sizeof(float) + // temperature + voltages + currents - sizeof(uint8_t) + // safety mode - sizeof(uint8_t) + // in reduced mode (bool) - sizeof(uint8_t); // immi interface installed (bool) -constexpr size_t kMasterboardOptionalImmiBytes = 2 * sizeof(uint32_t) + 2 * sizeof(float); -constexpr size_t kMasterboardPayloadBytesAfterImmi = sizeof(uint32_t) + // reserved - sizeof(uint8_t) + // operational mode selector input - sizeof(uint8_t) + // three position enabling device (bool) - sizeof(uint8_t); // reserved +constexpr size_t MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI = 2 * sizeof(uint32_t) + // digital input/output bits + 2 * sizeof(uint8_t) + // analog input ranges + 2 * sizeof(double) + // analog inputs + 2 * sizeof(char) + // analog output domains + 2 * sizeof(double) + // analog outputs + 4 * sizeof(float) + // temperature + voltages + currents + sizeof(uint8_t) + // safety mode + sizeof(uint8_t) + // in reduced mode (bool) + sizeof(uint8_t); // immi interface installed (bool) +constexpr size_t MASTERBOARD_OPTIONAL_IMMI_BYTES = 2 * sizeof(uint32_t) + 2 * sizeof(float); +constexpr size_t MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI = sizeof(uint32_t) + // reserved + sizeof(uint8_t) + // operational mode selector input + sizeof(uint8_t) + // three position enabling device (bool) + sizeof(uint8_t); // reserved std::vector makeRobotStatePacketWithMasterboardSubmessage(const std::vector& masterboard_payload) { @@ -611,7 +612,7 @@ TEST_F(PrimaryParserTest, parse_safetymode_msg) } } -std::string construct_string(int data_type, std::string data) +std::string constructString(int data_type, std::string data) { std::stringstream ss; ss << "SafetyModeMessage\n"; @@ -642,19 +643,19 @@ TEST_F(PrimaryParserTest, parsing_safetymode_results_in_correct_string) EXPECT_EQ(data->report_data_type_, static_cast(i)); if (data->report_data_type_ == 0 || data->report_data_type_ == 1) { - EXPECT_EQ(data->toString(), construct_string(data->report_data_type_, "2147943941")); + EXPECT_EQ(data->toString(), constructString(data->report_data_type_, "2147943941")); } else if (data->report_data_type_ == 2) { - EXPECT_EQ(data->toString(), construct_string(data->report_data_type_, "-2147023355")); + EXPECT_EQ(data->toString(), constructString(data->report_data_type_, "-2147023355")); } else if (data->report_data_type_ == 3) { - EXPECT_EQ(data->toString(), construct_string(data->report_data_type_, "6.30203e-36")); + EXPECT_EQ(data->toString(), constructString(data->report_data_type_, "6.30203e-36")); } else { - EXPECT_EQ(data->toString(), construct_string(data->report_data_type_, "0x80070605")); + EXPECT_EQ(data->toString(), constructString(data->report_data_type_, "0x80070605")); } } } @@ -662,7 +663,7 @@ TEST_F(PrimaryParserTest, parsing_safetymode_results_in_correct_string) TEST_F(PrimaryParserTest, parse_configuration_data_without_reserved_fields) { - std::vector payload(kConfigurationDataPayloadBytes, 0); + std::vector payload(CONFIGURATION_DATA_PAYLOAD_BYTES, 0); std::vector packet = makeRobotStatePacketWithConfigurationSubmessage(payload); comm::BinParser bp(packet.data(), packet.size()); @@ -678,7 +679,7 @@ TEST_F(PrimaryParserTest, parse_configuration_data_without_reserved_fields) TEST_F(PrimaryParserTest, parse_configuration_data_with_reserved_fields) { - std::vector payload(kConfigurationDataPayloadBytes, 0); + std::vector payload(CONFIGURATION_DATA_PAYLOAD_BYTES, 0); // Big-endian int16 values appended after the legacy payload. payload.push_back(0x12); payload.push_back(0x34); @@ -701,7 +702,7 @@ TEST_F(PrimaryParserTest, parse_configuration_data_with_reserved_fields) TEST_F(PrimaryParserTest, parse_masterboard_data_without_immi) { std::vector payload; - payload.reserve(kMasterboardPayloadBytesBeforeImmi + kMasterboardPayloadBytesAfterImmi); + payload.reserve(MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI + MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI); appendBigEndian(payload, 0xDEADBEEFu); // digital input bits appendBigEndian(payload, 0xCAFEBABEu); // digital output bits @@ -725,7 +726,7 @@ TEST_F(PrimaryParserTest, parse_masterboard_data_without_immi) payload.push_back(0x01); // three position enabling device input (true) payload.push_back(0x00); // reserved - ASSERT_EQ(payload.size(), kMasterboardPayloadBytesBeforeImmi + kMasterboardPayloadBytesAfterImmi); + ASSERT_EQ(payload.size(), MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI + MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI); std::vector packet = makeRobotStatePacketWithMasterboardSubmessage(payload); comm::BinParser bp(packet.data(), packet.size()); @@ -754,13 +755,21 @@ TEST_F(PrimaryParserTest, parse_masterboard_data_without_immi) EXPECT_FALSE(data->immi_interface_installed_); EXPECT_EQ(data->operational_mode_selector_input_, 2); EXPECT_TRUE(data->three_position_enabling_device_input_); + + auto msg_str = data->toString(); + std::cout << msg_str << std::endl; + EXPECT_NE(msg_str.find("IMMI interface installed: false"), std::string::npos); + EXPECT_EQ(msg_str.find("IMMI input bits:"), std::string::npos); + EXPECT_EQ(msg_str.find("IMMI output bits:"), std::string::npos); + EXPECT_EQ(msg_str.find("IMMI voltage 24V:"), std::string::npos); + EXPECT_EQ(msg_str.find("IMMI current:"), std::string::npos); } TEST_F(PrimaryParserTest, parse_masterboard_data_with_immi) { std::vector payload; - payload.reserve(kMasterboardPayloadBytesBeforeImmi + kMasterboardOptionalImmiBytes + - kMasterboardPayloadBytesAfterImmi); + payload.reserve(MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI + MASTERBOARD_OPTIONAL_IMMI_BYTES + + MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI); appendBigEndian(payload, 0x00000001u); appendBigEndian(payload, 0x00000002u); @@ -788,8 +797,8 @@ TEST_F(PrimaryParserTest, parse_masterboard_data_with_immi) payload.push_back(0x00); payload.push_back(0x00); - ASSERT_EQ(payload.size(), - kMasterboardPayloadBytesBeforeImmi + kMasterboardOptionalImmiBytes + kMasterboardPayloadBytesAfterImmi); + ASSERT_EQ(payload.size(), MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI + MASTERBOARD_OPTIONAL_IMMI_BYTES + + MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI); std::vector packet = makeRobotStatePacketWithMasterboardSubmessage(payload); comm::BinParser bp(packet.data(), packet.size()); @@ -806,6 +815,15 @@ TEST_F(PrimaryParserTest, parse_masterboard_data_with_immi) EXPECT_EQ(data->immi_output_bits_, std::bitset<32>(0x33334444u)); EXPECT_FLOAT_EQ(data->immi_voltage_24v_, 24.0f); EXPECT_FLOAT_EQ(data->immi_current_, 0.75f); + + auto msg_str = data->toString(); // also verify that the presence of IMMI fields does not cause issues in the + // toString() method + std::cout << msg_str << std::endl; + EXPECT_NE(msg_str.find("IMMI interface installed: true"), std::string::npos); + EXPECT_NE(msg_str.find("IMMI input bits: 0b00010001000100010010001000100010"), std::string::npos); + EXPECT_NE(msg_str.find("IMMI output bits: 0b00110011001100110100010001000100"), std::string::npos); + EXPECT_NE(msg_str.find("IMMI voltage 24V: 24"), std::string::npos); + EXPECT_NE(msg_str.find("IMMI current: 0.75"), std::string::npos); } int main(int argc, char* argv[]) From ce695b8158b1c503a9db31aa4b5d75c59237ad90 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 5 May 2026 09:00:04 +0200 Subject: [PATCH 3/3] Do not store masterboard data in primary consumer Currently, there's no use-case. Adding a getter for a RobotStateMessage seems not the right way to go. --- include/ur_client_library/primary/primary_consumer.h | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h index f8e65e159..b27799115 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -193,10 +193,9 @@ class PrimaryConsumer : public AbstractPrimaryConsumer * * \returns True */ - virtual bool consume(MasterboardData& pkg) override + virtual bool consume(MasterboardData&) override { - std::scoped_lock lock(masterboard_data_mutex_); - masterboard_data_ = std::make_shared(pkg); + // Nothing to do here for now. return true; } @@ -282,8 +281,6 @@ class PrimaryConsumer : public AbstractPrimaryConsumer std::mutex configuration_data_mutex_; std::mutex safety_mode_mutex_; std::shared_ptr safety_mode_; - std::mutex masterboard_data_mutex_; - std::shared_ptr masterboard_data_; }; } // namespace primary_interface