Add support for byte array params (#199)

### Public-Facing Changes
- Add support for byte array params [ROS2]

### Description
Adds support for byte array parameters according to the ws-protocol
specification update made in
foxglove/ws-protocol#396

Fixes #198

CMakeLists.txt

@@ -43,6 +43,7 @@ endif()
 
 # Build the foxglove_bridge_base library
 add_library(foxglove_bridge_base SHARED
+  foxglove_bridge_base/src/base64.cpp
   foxglove_bridge_base/src/foxglove_bridge.cpp
   foxglove_bridge_base/src/parameter.cpp
   foxglove_bridge_base/src/serialization.cpp
@@ -169,6 +170,10 @@ if(ROS_BUILD_TYPE STREQUAL "catkin")
     target_link_libraries(serialization_test foxglove_bridge_base)
     enable_strict_compiler_warnings(foxglove_bridge)
 
+    catkin_add_gtest(base64_test foxglove_bridge_base/tests/base64_test.cpp)
+    target_link_libraries(base64_test foxglove_bridge_base)
+    enable_strict_compiler_warnings(foxglove_bridge)
+
     add_rostest_gtest(smoke_test ros1_foxglove_bridge/tests/smoke.test ros1_foxglove_bridge/tests/smoke_test.cpp)
     target_include_directories(smoke_test SYSTEM PRIVATE
       $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/foxglove_bridge_base/include>
@@ -194,6 +199,10 @@ elseif(ROS_BUILD_TYPE STREQUAL "ament_cmake")
     target_link_libraries(serialization_test foxglove_bridge_base)
     enable_strict_compiler_warnings(serialization_test)
 
+    ament_add_gtest(base64_test foxglove_bridge_base/tests/base64_test.cpp)
+    target_link_libraries(base64_test foxglove_bridge_base)
+    enable_strict_compiler_warnings(base64_test)
+
     ament_add_gtest(smoke_test ros2_foxglove_bridge/tests/smoke_test.cpp)
     ament_target_dependencies(smoke_test rclcpp rclcpp_components std_msgs std_srvs)
     target_link_libraries(smoke_test foxglove_bridge_base)

foxglove_bridge_base/include/foxglove_bridge/base64.hpp

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <cstdint>
+#include <string>
+#include <string_view>
+#include <vector>
+
+namespace foxglove {
+
+std::string base64Encode(const std::string_view& input);
+
+std::vector<unsigned char> base64Decode(const std::string& input);
+
+}  // namespace foxglove

foxglove_bridge_base/include/foxglove_bridge/parameter.hpp

@@ -18,6 +18,7 @@ enum class ParameterType {
   PARAMETER_INTEGER,
   PARAMETER_DOUBLE,
   PARAMETER_STRING,
+  PARAMETER_BYTE_ARRAY,
   PARAMETER_BOOL_ARRAY,
   PARAMETER_INTEGER_ARRAY,
   PARAMETER_DOUBLE_ARRAY,
@@ -34,6 +35,7 @@ class Parameter {
   Parameter(const std::string& name, double value);
   Parameter(const std::string& name, std::string value);
   Parameter(const std::string& name, const char* value);
+  Parameter(const std::string& name, const std::vector<unsigned char>& value);
   Parameter(const std::string& name, const std::vector<bool>& value);
   Parameter(const std::string& name, const std::vector<int>& value);
   Parameter(const std::string& name, const std::vector<int64_t>& value);

foxglove_bridge_base/src/base64.cpp

@@ -0,0 +1,106 @@
+#include <stdexcept>
+
+#include <foxglove_bridge/base64.hpp>
+
+namespace foxglove {
+
+// Adapted from:
+// https://gist.github.com/tomykaira/f0fd86b6c73063283afe550bc5d77594
+// https://github.com/protocolbuffers/protobuf/blob/01fe22219a0/src/google/protobuf/compiler/csharp/csharp_helpers.cc#L346
+std::string base64Encode(const std::string_view& input) {
+  constexpr const char ALPHABET[] =
+    "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
+  std::string result;
+  // Every 3 bytes of data yields 4 bytes of output
+  result.reserve((input.size() + (3 - 1 /* round up */)) / 3 * 4);
+
+  // Unsigned values are required for bit-shifts below to work properly
+  const unsigned char* data = reinterpret_cast<const unsigned char*>(input.data());
+
+  size_t i = 0;
+  for (; i + 2 < input.size(); i += 3) {
+    result.push_back(ALPHABET[data[i] >> 2]);
+    result.push_back(ALPHABET[((data[i] & 0b11) << 4) | (data[i + 1] >> 4)]);
+    result.push_back(ALPHABET[((data[i + 1] & 0b1111) << 2) | (data[i + 2] >> 6)]);
+    result.push_back(ALPHABET[data[i + 2] & 0b111111]);
+  }
+  switch (input.size() - i) {
+    case 2:
+      result.push_back(ALPHABET[data[i] >> 2]);
+      result.push_back(ALPHABET[((data[i] & 0b11) << 4) | (data[i + 1] >> 4)]);
+      result.push_back(ALPHABET[(data[i + 1] & 0b1111) << 2]);
+      result.push_back('=');
+      break;
+    case 1:
+      result.push_back(ALPHABET[data[i] >> 2]);
+      result.push_back(ALPHABET[(data[i] & 0b11) << 4]);
+      result.push_back('=');
+      result.push_back('=');
+      break;
+  }
+
+  return result;
+}
+
+// Adapted from:
+// https://github.com/mvorbrodt/blog/blob/cd46051e180/src/base64.hpp#L55-L110
+std::vector<unsigned char> base64Decode(const std::string& input) {
+  if (input.length() % 4) {
+    throw std::runtime_error("Invalid base64 length!");
+  }
+
+  constexpr char kPadCharacter = '=';
+
+  std::size_t padding{};
+
+  if (input.length()) {
+    if (input[input.length() - 1] == kPadCharacter) padding++;
+    if (input[input.length() - 2] == kPadCharacter) padding++;
+  }
+
+  std::vector<unsigned char> decoded;
+  decoded.reserve(((input.length() / 4) * 3) - padding);
+
+  std::uint32_t temp{};
+  auto it = input.begin();
+
+  while (it < input.end()) {
+    for (std::size_t i = 0; i < 4; ++i) {
+      temp <<= 6;
+      if (*it >= 0x41 && *it <= 0x5A)
+        temp |= *it - 0x41;
+      else if (*it >= 0x61 && *it <= 0x7A)
+        temp |= *it - 0x47;
+      else if (*it >= 0x30 && *it <= 0x39)
+        temp |= *it + 0x04;
+      else if (*it == 0x2B)
+        temp |= 0x3E;
+      else if (*it == 0x2F)
+        temp |= 0x3F;
+      else if (*it == kPadCharacter) {
+        switch (input.end() - it) {
+          case 1:
+            decoded.push_back((temp >> 16) & 0x000000FF);
+            decoded.push_back((temp >> 8) & 0x000000FF);
+            return decoded;
+          case 2:
+            decoded.push_back((temp >> 10) & 0x000000FF);
+            return decoded;
+          default:
+            throw std::runtime_error("Invalid padding in base64!");
+        }
+      } else
+        throw std::runtime_error("Invalid character in base64!");
+
+      ++it;
+    }
+
+    decoded.push_back((temp >> 16) & 0x000000FF);
+    decoded.push_back((temp >> 8) & 0x000000FF);
+    decoded.push_back((temp)&0x000000FF);
+  }
+
+  return decoded;
+}
+
+}  // namespace foxglove

foxglove_bridge_base/src/parameter.cpp

@@ -38,6 +38,11 @@ Parameter::Parameter(const std::string& name, std::string value)
     , _type(ParameterType::PARAMETER_STRING)
     , _value(value) {}
 
+Parameter::Parameter(const std::string& name, const std::vector<unsigned char>& value)
+    : _name(name)
+    , _type(ParameterType::PARAMETER_BYTE_ARRAY)
+    , _value(value) {}
+
 Parameter::Parameter(const std::string& name, const std::vector<bool>& value)
     : _name(name)
     , _type(ParameterType::PARAMETER_BOOL_ARRAY)

foxglove_bridge_base/src/serialization.cpp

@@ -1,3 +1,4 @@
+#include <foxglove_bridge/base64.hpp>
 #include <foxglove_bridge/serialization.hpp>
 
 namespace foxglove {
@@ -37,6 +38,12 @@ void to_json(nlohmann::json& j, const Parameter& p) {
     j["value"] = p.getValue<double>();
   } else if (paramType == ParameterType::PARAMETER_STRING) {
     j["value"] = p.getValue<std::string>();
+  } else if (paramType == ParameterType::PARAMETER_BYTE_ARRAY) {
+    const auto& paramValue = p.getValue<std::vector<unsigned char>>();
+    const std::string_view strValue(reinterpret_cast<const char*>(paramValue.data()),
+                                    paramValue.size());
+    j["value"] = base64Encode(strValue);
+    j["type"] = "byte_array";
   } else if (paramType == ParameterType::PARAMETER_BOOL_ARRAY) {
     j["value"] = p.getValue<std::vector<bool>>();
   } else if (paramType == ParameterType::PARAMETER_INTEGER_ARRAY) {
@@ -64,7 +71,13 @@ void from_json(const nlohmann::json& j, Parameter& p) {
   const auto jsonType = j["value"].type();
 
   if (jsonType == nlohmann::detail::value_t::string) {
-    p = Parameter(name, value.get<std::string>());
+    if (j.find("type") == j.end()) {
+      p = Parameter(name, value.get<std::string>());
+    } else if (j["type"] == "byte_array") {
+      p = Parameter(name, base64Decode(value.get<std::string>()));
+    } else {
+      throw std::runtime_error("Unsupported parameter 'type' value: " + j.dump());
+    }
   } else if (jsonType == nlohmann::detail::value_t::boolean) {
     p = Parameter(name, value.get<bool>());
   } else if (jsonType == nlohmann::detail::value_t::number_integer) {

foxglove_bridge_base/tests/base64_test.cpp

@@ -0,0 +1,27 @@
+#include <gtest/gtest.h>
+
+#include <foxglove_bridge/base64.hpp>
+
+TEST(Base64Test, EncodingTest) {
+  constexpr char arr[] = {'A', 'B', 'C', 'D'};
+  const std::string_view sv(arr, sizeof(arr));
+  const std::string b64encoded = foxglove::base64Encode(sv);
+  EXPECT_EQ(b64encoded, "QUJDRA==");
+}
+
+TEST(Base64Test, DecodeTest) {
+  const std::vector<unsigned char> expectedVal = {0x00, 0xFF, 0x01, 0xFE};
+  EXPECT_EQ(foxglove::base64Decode("AP8B/g=="), expectedVal);
+}
+
+TEST(Base64Test, DecodeInvalidStringTest) {
+  // String length not multiple of 4
+  EXPECT_THROW(foxglove::base64Decode("faefa"), std::runtime_error);
+  // Invalid characters
+  EXPECT_THROW(foxglove::base64Decode("fa^ef a"), std::runtime_error);
+}
+
+int main(int argc, char** argv) {
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

ros2_foxglove_bridge/src/parameter_interface.cpp

@@ -33,6 +33,8 @@ static rclcpp::Parameter toRosParam(const foxglove::Parameter& p) {
     return rclcpp::Parameter(p.getName(), p.getValue<double>());
   } else if (paramType == ParameterType::PARAMETER_STRING) {
     return rclcpp::Parameter(p.getName(), p.getValue<std::string>());
+  } else if (paramType == ParameterType::PARAMETER_BYTE_ARRAY) {
+    return rclcpp::Parameter(p.getName(), p.getValue<std::vector<unsigned char>>());
   } else if (paramType == ParameterType::PARAMETER_BOOL_ARRAY) {
     return rclcpp::Parameter(p.getName(), p.getValue<std::vector<bool>>());
   } else if (paramType == ParameterType::PARAMETER_INTEGER_ARRAY) {
@@ -61,6 +63,8 @@ static foxglove::Parameter fromRosParam(const rclcpp::Parameter& p) {
     return foxglove::Parameter(p.get_name(), p.as_double());
   } else if (type == rclcpp::ParameterType::PARAMETER_STRING) {
     return foxglove::Parameter(p.get_name(), p.as_string());
+  } else if (type == rclcpp::ParameterType::PARAMETER_BYTE_ARRAY) {
+    return foxglove::Parameter(p.get_name(), p.as_byte_array());
   } else if (type == rclcpp::ParameterType::PARAMETER_BOOL_ARRAY) {
     return foxglove::Parameter(p.get_name(), p.as_bool_array());
   } else if (type == rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY) {