[primary] Print a warning on additional data in RobotState submessages (UniversalRobots#460)

Before, parsing sub-messages of a RobotState message with added bytes
was causing a failure when parsing the RobotState message. With this
change by default the primary parser only prints a warning in that case.

Mainly for testing purposes we added a "strict" mode that throws an
exception if unknown bytes are inside the message. This way we will
achieve a failing test when running against a version with added bytes
that we don't know, yet.

I broke up strict mode in a runtime setting for the parser and a compile
setting for the primary client. This way we can test the parser with
both, while deciding the behavior for the primary client at configure
time.

Author: urfeex

.github/workflows/ci.yml

@@ -58,7 +58,14 @@ jobs:
       - name: setup chrome
         uses: browser-actions/setup-chrome@v2
       - name: configure
-        run: mkdir build && cd build && cmake .. -DBUILDING_TESTS=1 -DINTEGRATION_TESTS=1 -DWITH_ASAN=ON
+        run: >
+          mkdir build &&
+          cd build &&
+          cmake ..
+          -DBUILDING_TESTS=1
+          -DINTEGRATION_TESTS=1
+          -DWITH_ASAN=ON
+          -DPRIMARY_CLIENT_STRICT_PARSING=ON
         env:
           CXXFLAGS: -g -O2  -fprofile-arcs -ftest-coverage
           CFLAGS: -g -O2 -fprofile-arcs -ftest-coverage

CMakeLists.txt

@@ -8,6 +8,7 @@ endif()
 
 option(WITH_ASAN "Compile with address sanitizer support" OFF)
 option(BUILD_SHARED_LIBS "Build using shared libraries" ON)
+option(PRIMARY_CLIENT_STRICT_PARSING_ "Enable strict parsing of primary client messages" OFF)
 
 if(MSVC)
 set(BUILD_SHARED_LIBS OFF)
@@ -63,6 +64,8 @@ add_library(urcl
 )
 add_library(ur_client_library::urcl ALIAS urcl)
 target_compile_features(urcl PUBLIC cxx_std_17)
+configure_file("src/compile_options.h.in"
+	"include/ur_client_library/compile_options.h")
 
 if(MSVC)
   target_link_libraries(urcl ws2_32)
@@ -94,6 +97,9 @@ target_include_directories(urcl PUBLIC
 target_include_directories(urcl PRIVATE
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty>
 )
+target_include_directories(urcl PRIVATE
+  "${CMAKE_CURRENT_BINARY_DIR}/include"
+)
 
 find_package(Threads REQUIRED)
 if(THREADS_HAVE_PTHREAD_ARG)

include/ur_client_library/primary/primary_parser.h

@@ -49,6 +49,22 @@ class PrimaryParser : public comm::Parser<PrimaryPackage>
   PrimaryParser() = default;
   virtual ~PrimaryParser() = default;
 
+  /**!
+   * \brief Set the strict mode for parsing. If strict mode is enabled, the parser will throw an
+   * exception if a sub-package is not parsed completely. If strict mode is disabled, the parser
+   * will just log a warning and continue parsing the next sub-package.
+   *
+   * Strict mode is meant for testing only, as it may break existing applications when running
+   * against a newer software version that adds new fields to existing packages, which would lead
+   * to parsing errors of the respective sub-package.
+   *
+   * \param strict_mode Whether to enable strict mode or not.
+   */
+  void setStrictMode(bool strict_mode)
+  {
+    strict_mode_ = strict_mode;
+  }
+
   /*!
    * \brief Uses the given BinParser to create package objects from the contained serialization.
    *
@@ -109,9 +125,14 @@ class PrimaryParser : public comm::Parser<PrimaryPackage>
 
           if (!sbp.empty())
           {
-            URCL_LOG_ERROR("Sub-package of type %d was not parsed completely!", static_cast<int>(type));
             sbp.debug();
-            return false;
+            if (strict_mode_)
+            {
+              throw UrException("Sub-package of type " + std::string(robotStateString(type)) +
+                                " was not parsed completely, and strict mode is enabled, so aborting parsing!");
+            }
+            URCL_LOG_WARN("Sub-package of type %s was not parsed completely!", robotStateString(type));
+            sbp.consume();
           }
         }
 
@@ -224,6 +245,10 @@ class PrimaryParser : public comm::Parser<PrimaryPackage>
         return new RobotMessage(timestamp, source, type);
     }
   }
+
+  bool strict_mode_ = false;  //!< If true, the parser will return false if it encounters unknown
+  // package types or sub-package types. If false, it will ignore unknown types and try to continue
+  // parsing.
 };
 
 }  // namespace primary_interface

src/compile_options.h.in

@@ -0,0 +1,13 @@
+#pragma once
+
+#cmakedefine01 PRIMARY_CLIENT_STRICT_PARSING_
+
+namespace urcl
+{
+struct CompileOptions
+{
+  constexpr static bool PRIMARY_CLIENT_STRICT_PARSING = static_cast<bool>(PRIMARY_CLIENT_STRICT_PARSING_);
+};
+
+inline constexpr CompileOptions COMPILE_OPTIONS;
+}  // namespace urcl

src/primary/primary_client.cpp

@@ -33,6 +33,8 @@
 #include <ur_client_library/primary/robot_state.h>
 #include "ur_client_library/exceptions.h"
 #include <ur_client_library/helpers.h>
+#include <ur_client_library/compile_options.h>
+
 #include <chrono>
 namespace urcl
 {
@@ -41,6 +43,7 @@ namespace primary_interface
 PrimaryClient::PrimaryClient(const std::string& robot_ip, comm::INotifier& notifier)
   : stream_(robot_ip, UR_PRIMARY_PORT)
 {
+  parser_.setStrictMode(COMPILE_OPTIONS.PRIMARY_CLIENT_STRICT_PARSING);
   prod_.reset(new comm::URProducer<PrimaryPackage>(stream_, parser_));
 
   consumer_.reset(new PrimaryConsumer());

tests/test_primary_parser.cpp

@@ -182,15 +182,30 @@ const unsigned char RUNTIME_EXCEPTION_MESSAGE[] = {
   0x6e, 0x6f, 0x74, 0x5f, 0x66, 0x6f, 0x75, 0x6e, 0x64, 0x3a, 0x74, 0x78, 0x74, 0x6d, 0x73, 0x67, 0x3a
 };
 
-TEST(primary_parser, parse_calibration_data)
+class PrimaryParserTest : public ::testing::Test
+{
+protected:
+  virtual void SetUp() override
+  {
+    // In these tests we use strict mode, which means that the parser will not ignore any extra
+    // bytes in the payload. This allows us to verify that the parser correctly identifies the size
+    // of each message and does not consume more bytes than it should.
+    // If new software versions add new fields to the messages, these tests will fail, which is
+    // desirable as it will prompt us to update the parser with the new fields.
+    parser_.setStrictMode(true);
+  }
+
+  primary_interface::PrimaryParser parser_;
+};
+
+TEST_F(PrimaryParserTest, parse_calibration_data)
 {
   unsigned char raw_data[sizeof(ROBOT_STATE)];
   memcpy(raw_data, ROBOT_STATE, sizeof(ROBOT_STATE));
   comm::BinParser bp(raw_data, sizeof(raw_data));
 
   std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
-  primary_interface::PrimaryParser parser;
-  parser.parse(bp, products);
+  parser_.parse(bp, products);
 
   EXPECT_EQ(products.size(), 13);
 
@@ -205,26 +220,24 @@ TEST(primary_parser, parse_calibration_data)
   }
 }
 
-TEST(primary_parser, parse_robot_state_with_single_parser)
+TEST_F(PrimaryParserTest, parse_robot_state_with_single_parser)
 {
   unsigned char raw_data[sizeof(ROBOT_STATE)];
   memcpy(raw_data, ROBOT_STATE, sizeof(ROBOT_STATE));
   comm::BinParser bp(raw_data, sizeof(raw_data));
 
   std::unique_ptr<primary_interface::PrimaryPackage> product;
-  primary_interface::PrimaryParser parser;
-  ASSERT_FALSE(parser.parse(bp, product));
+  ASSERT_FALSE(parser_.parse(bp, product));
 };
 
-TEST(primary_parser, parse_version_message)
+TEST_F(PrimaryParserTest, parse_version_message)
 {
   unsigned char raw_data[sizeof(VERSION_MESSAGE)];
   memcpy(raw_data, VERSION_MESSAGE, sizeof(VERSION_MESSAGE));
   comm::BinParser bp(raw_data, sizeof(raw_data));
 
   std::unique_ptr<primary_interface::PrimaryPackage> product;
-  primary_interface::PrimaryParser parser;
-  ASSERT_TRUE(parser.parse(bp, product));
+  ASSERT_TRUE(parser_.parse(bp, product));
 
   EXPECT_NE(product, nullptr);
   if (primary_interface::VersionMessage* data = dynamic_cast<primary_interface::VersionMessage*>(product.get()))
@@ -241,15 +254,14 @@ TEST(primary_parser, parse_version_message)
   }
 }
 
-TEST(primary_parser, parse_key_message)
+TEST_F(PrimaryParserTest, parse_key_message)
 {
   unsigned char raw_data[sizeof(KEY_MESSAGE)];
   memcpy(raw_data, KEY_MESSAGE, sizeof(KEY_MESSAGE));
   comm::BinParser bp(raw_data, sizeof(raw_data));
 
   std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
-  primary_interface::PrimaryParser parser;
-  ASSERT_TRUE(parser.parse(bp, products));
+  ASSERT_TRUE(parser_.parse(bp, products));
 
   ASSERT_EQ(products.size(), 1);
   if (auto data = dynamic_cast<primary_interface::KeyMessage*>(products[0].get()))
@@ -274,15 +286,14 @@ TEST(primary_parser, parse_key_message)
   }
 }
 
-TEST(primary_parser, parse_runtime_exception_message)
+TEST_F(PrimaryParserTest, parse_runtime_exception_message)
 {
   unsigned char raw_data[sizeof(RUNTIME_EXCEPTION_MESSAGE)];
   memcpy(raw_data, RUNTIME_EXCEPTION_MESSAGE, sizeof(RUNTIME_EXCEPTION_MESSAGE));
   comm::BinParser bp(raw_data, sizeof(raw_data));
 
   std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
-  primary_interface::PrimaryParser parser;
-  ASSERT_TRUE(parser.parse(bp, products));
+  ASSERT_TRUE(parser_.parse(bp, products));
 
   EXPECT_EQ(products.size(), 1);
 
@@ -305,6 +316,78 @@ TEST(primary_parser, parse_runtime_exception_message)
   }
 }
 
+TEST_F(PrimaryParserTest, parse_robot_state_with_oversized_submessage)
+{
+  // KinematicsInfo parses exactly 220 bytes of payload:
+  //   6×uint32 (checksum) + 4×vector6d (DH params) + uint32 (calibration_status)
+  const size_t ki_payload_size = 6 * sizeof(uint32_t) + 4 * 6 * sizeof(double) + sizeof(uint32_t);
+  const size_t extra_bytes = 10;
+  const size_t sub1_size = sizeof(uint32_t) + sizeof(uint8_t) + ki_payload_size + extra_bytes;
+  const size_t sub2_size = sizeof(uint32_t) + sizeof(uint8_t) + 1;
+  const size_t total_size = sizeof(int32_t) + sizeof(uint8_t) + sub1_size + sub2_size;
+
+  std::vector<uint8_t> test_data(total_size, 0);
+  size_t offset = 0;
+
+  // Total packet size (big-endian int32)
+  test_data[offset++] = (total_size >> 24) & 0xFF;
+  test_data[offset++] = (total_size >> 16) & 0xFF;
+  test_data[offset++] = (total_size >> 8) & 0xFF;
+  test_data[offset++] = total_size & 0xFF;
+
+  // Packet type: ROBOT_STATE (0x10)
+  test_data[offset++] = 0x10;
+
+  // Submessage 1: KinematicsInfo with extra trailing bytes
+  test_data[offset++] = (sub1_size >> 24) & 0xFF;
+  test_data[offset++] = (sub1_size >> 16) & 0xFF;
+  test_data[offset++] = (sub1_size >> 8) & 0xFF;
+  test_data[offset++] = sub1_size & 0xFF;
+  test_data[offset++] = 0x05;  // KINEMATICS_INFO
+  offset += ki_payload_size;   // payload is all zeros
+  memset(test_data.data() + offset, 0xAA, extra_bytes);
+  offset += extra_bytes;
+
+  // Submessage 2: ADDITIONAL_INFO (parsed by base RobotState → rawData)
+  test_data[offset++] = (sub2_size >> 24) & 0xFF;
+  test_data[offset++] = (sub2_size >> 16) & 0xFF;
+  test_data[offset++] = (sub2_size >> 8) & 0xFF;
+  test_data[offset++] = sub2_size & 0xFF;
+  test_data[offset++] = 0x08;  // ADDITIONAL_INFO
+  test_data[offset++] = 0x42;  // 1 byte of payload
+
+  ASSERT_EQ(offset, total_size);
+
+  {
+    // With strict mode enabled, an execption should be thrown due to the oversized KinematicsInfo
+    // submessage
+    std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
+    unsigned char raw_data[total_size];
+    memcpy(raw_data, test_data.data(), test_data.size());
+    comm::BinParser bp(raw_data, total_size);
+    EXPECT_THROW(parser_.parse(bp, products), UrException);
+  }
+
+  {
+    // Using a non-strict parser that allows oversized submessages should succeed and parse all
+    // submessages
+    std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
+    unsigned char raw_data[total_size];
+    memcpy(raw_data, test_data.data(), test_data.size());
+    comm::BinParser bp(raw_data, total_size);
+    primary_interface::PrimaryParser non_strict_parser;
+    EXPECT_TRUE(non_strict_parser.parse(bp, products));
+
+    // Both submessages should be parsed despite the oversized first one
+    ASSERT_EQ(products.size(), 2);
+
+    auto* ki = dynamic_cast<primary_interface::KinematicsInfo*>(products[0].get());
+    ASSERT_NE(ki, nullptr);
+    EXPECT_EQ(ki->calibration_status_, 0u);
+    EXPECT_EQ(ki->dh_theta_, vector6d_t({ 0, 0, 0, 0, 0, 0 }));
+  }
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);