From 43bdd367590f7843daf223f876654852d0f1383d Mon Sep 17 00:00:00 2001 From: six9527 <487844521@qq.com> Date: Wed, 8 Nov 2023 22:32:17 +0800 Subject: [PATCH] first commit --- CMakeLists.txt | 31 ++++ include/obu/data_process.hpp | 51 +++++++ include/obu/data_stream_converter.hpp | 147 ++++++++++++++++++ include/obu/macro_scope.hpp | 194 ++++++++++++++++++++++++ include/obu/udp_server.hpp | 26 ++++ launch/obu.lacunh | 0 package.xml | 23 +++ src/data_process.cpp | 207 ++++++++++++++++++++++++++ src/obu_socket_node.cpp | 26 ++++ src/udp_server.cpp | 56 +++++++ test/server.py | 17 +++ 11 files changed, 778 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 include/obu/data_process.hpp create mode 100644 include/obu/data_stream_converter.hpp create mode 100644 include/obu/macro_scope.hpp create mode 100644 include/obu/udp_server.hpp create mode 100644 launch/obu.lacunh create mode 100644 package.xml create mode 100644 src/data_process.cpp create mode 100644 src/obu_socket_node.cpp create mode 100644 src/udp_server.cpp create mode 100755 test/server.py diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..bc2be57 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.5) +project(obu) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +ament_export_include_directories(include/obu) + +ament_auto_add_executable(${PROJECT_NAME}_server_node + src/udp_server.cpp + src/obu_socket_node.cpp + src/data_process.cpp + +) + +# install +ament_auto_package( + INSTALL_TO_SHARE + launch + include + # config +) diff --git a/include/obu/data_process.hpp b/include/obu/data_process.hpp new file mode 100644 index 0000000..3a03921 --- /dev/null +++ b/include/obu/data_process.hpp @@ -0,0 +1,51 @@ +#pragma once +#include +#include +#include +#include "macro_scope.hpp" +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +using json = nlohmann::json; +struct Message +{ + uint16_t header=0x5AA5; + uint16_t version=0x2001; + uint64_t timestamp=0; + uint16_t device_type=0x0001; + int device_id=1234567; + uint8_t data_direction=0x1; + uint8_t data_type=0x1; + uint8_t encryption_type=0x0; + uint32_t reserved=0xffffffff; + uint16_t message_length=0; + std::string message_body; + uint16_t check_code=0x0005; + Message() = default; + operator std::vector() const; + Message& operator=(const std::vector& byte_stream); +}; + +class data_process { +public: + data_process(rclcpp::Node::SharedPtr node); + ~data_process(); + // 获取message的消息体 + rclcpp::Node::SharedPtr node_; + std::string get_string(); + // 将message打包为字节流 + std::vector packed_data(); + void parse_data(std::vector received_data); + using TrafficSignalArray = autoware_auto_perception_msgs::msg::TrafficSignalArray; + using TrafficSignal = autoware_auto_perception_msgs::msg::TrafficSignal; + using TrafficLight = autoware_auto_perception_msgs::msg::TrafficLight; + + rclcpp::Publisher::SharedPtr traffic_signal_array_pub_; + +private: + json message_json; +}; + diff --git a/include/obu/data_stream_converter.hpp b/include/obu/data_stream_converter.hpp new file mode 100644 index 0000000..7adfbeb --- /dev/null +++ b/include/obu/data_stream_converter.hpp @@ -0,0 +1,147 @@ +#pragma once + +/* +1.0 结构体和字节流之间的序列化和反序列化 +2.0 结构体组成:基础类型,string, std::vector +*/ +#include +#include +#include +#include +#include +#include + +template void show(const T &byteStream) { + // 输出存储结果 + for (uint8_t byte : byteStream) { + std::cout << std::hex << std::setfill('0') << std::setw(2) + << static_cast(byte) << ' '; + } + std::cout << '\n'; +} + +// 判断主机字节序 +constexpr bool isLittleEndian() { + uint16_t num = 1; + return *(reinterpret_cast(&num)) == 1; +} + +// 定义模板结构体来检查类型是否是 std::array 的特化 +template struct is_std_array_of_uint8 : std::false_type {}; + +// 对模板进行特化,当类型是 std::array 时返回 true +template +struct is_std_array_of_uint8> : std::true_type {}; + +template +bool storeToByteStreamBigEndian(const T &data, + std::vector &byteStream) { + // data 类型是否是 arrry std::string std::vector + if constexpr (std::is_same_v || + std::is_same_v> || + is_std_array_of_uint8::value) { + byteStream.insert(byteStream.end(), data.begin(), data.end()); + return true; + } + + // 基础数据类型 + if constexpr (std::is_fundamental::value) { + const uint8_t *bytePtr = reinterpret_cast(&data); + if (isLittleEndian()) { + for (size_t i = 0; i < sizeof(T); ++i) { + size_t index = sizeof(T) - 1 - i; + byteStream.push_back(bytePtr[index]); + } + } else { + byteStream.insert(byteStream.end(), bytePtr, bytePtr + sizeof(T)); + } + return true; + } else { + std::cerr << "[storeToByteStreamBigEndian]不是基础类型, " + "std::vector, string, array:" + << typeid(T).name() + << std::endl; + return false; + } + + return false; +} + +template +bool storeFromByteStreamBigEndian(T &data, + const std::vector &byteStream) { + + if constexpr (std::is_same_v || + std::is_same_v>) { + + data.resize(byteStream.size()); + std::copy(byteStream.begin(), byteStream.end(), data.begin()); + return true; + } + // data 类型是否是 arrry + if constexpr (is_std_array_of_uint8::value) { + std::copy(byteStream.begin(), byteStream.end(), data.begin()); + return true; + } + + // 基础数据类型 + if constexpr (std::is_fundamental::value) { + if (byteStream.size() != sizeof(T)) { + std::cout + << "Size of byte stream does not match the size of the target type" + << '\n'; + return false; + } + if (isLittleEndian()) { + std::vector reversedStream(byteStream.rbegin(), + byteStream.rend()); + std::memcpy(&data, reversedStream.data(), sizeof(T)); + + } else { + std::memcpy(&data, byteStream.data(), sizeof(T)); + } + return true; + } else { + std::cerr << "[storeFromByteStreamBigEndian]不是基础类型, " + "std::vector, string, array:" + << typeid(T).name() + << std::endl; + return false; + } + return false; +} + +template uint16_t getStoreByteSize(T &data) { + if constexpr (std::is_same_v) { + return 0; + } + + if constexpr (std::is_same_v> || + is_std_array_of_uint8::value) { + return data.size(); + } else if constexpr (std::is_fundamental::value) { + return sizeof(data); + } else { + std::cerr << "[getStoreByteSize]不是基础类型, std::vector, " + "string, array:" + << typeid(T).name() + << std::endl; + return 0xffff; + } + return 0xffff; +} + +// template +// bool is_size_illegal(const T &struct_data, +// const std::vector &byteStream) { +// if constexpr (std::is_same_v || +// std::is_same_v> || +// std::is_fundamental::value) { +// return data.size(); +// } else if (std::is_fundamental::value) { +// return sizeof(data); +// } else { +// std::cerr << "不是基础类型, std::vector, string, array" << std::endl; +// } +// } \ No newline at end of file diff --git a/include/obu/macro_scope.hpp b/include/obu/macro_scope.hpp new file mode 100644 index 0000000..e48718b --- /dev/null +++ b/include/obu/macro_scope.hpp @@ -0,0 +1,194 @@ +#pragma once +#ifndef MACRO_SCOPE_HPP +#define MACRO_SCOPE_HPP +#include +#include +#include +#include "data_stream_converter.hpp" +#define BYTE_STREAM_EXPAND( x ) x +#define BYTE_STREAM_GET_MACRO(_1, _2, _3, _4, _5, _6, _7, _8, _9, _10, _11, _12, _13, _14, _15, _16, _17, _18, _19, _20, _21, _22, _23, _24, _25, _26, _27, _28, _29, _30, _31, _32, _33, _34, _35, _36, _37, _38, _39, _40, _41, _42, _43, _44, _45, _46, _47, _48, _49, _50, _51, _52, _53, _54, _55, _56, _57, _58, _59, _60, _61, _62, _63, _64, NAME,...) NAME +#define BYTE_STREAM_PASTE(...) BYTE_STREAM_EXPAND(BYTE_STREAM_GET_MACRO(__VA_ARGS__, \ + BYTE_STREAM_PASTE64, \ + BYTE_STREAM_PASTE63, \ + BYTE_STREAM_PASTE62, \ + BYTE_STREAM_PASTE61, \ + BYTE_STREAM_PASTE60, \ + BYTE_STREAM_PASTE59, \ + BYTE_STREAM_PASTE58, \ + BYTE_STREAM_PASTE57, \ + BYTE_STREAM_PASTE56, \ + BYTE_STREAM_PASTE55, \ + BYTE_STREAM_PASTE54, \ + BYTE_STREAM_PASTE53, \ + BYTE_STREAM_PASTE52, \ + BYTE_STREAM_PASTE51, \ + BYTE_STREAM_PASTE50, \ + BYTE_STREAM_PASTE49, \ + BYTE_STREAM_PASTE48, \ + BYTE_STREAM_PASTE47, \ + BYTE_STREAM_PASTE46, \ + BYTE_STREAM_PASTE45, \ + BYTE_STREAM_PASTE44, \ + BYTE_STREAM_PASTE43, \ + BYTE_STREAM_PASTE42, \ + BYTE_STREAM_PASTE41, \ + BYTE_STREAM_PASTE40, \ + BYTE_STREAM_PASTE39, \ + BYTE_STREAM_PASTE38, \ + BYTE_STREAM_PASTE37, \ + BYTE_STREAM_PASTE36, \ + BYTE_STREAM_PASTE35, \ + BYTE_STREAM_PASTE34, \ + BYTE_STREAM_PASTE33, \ + BYTE_STREAM_PASTE32, \ + BYTE_STREAM_PASTE31, \ + BYTE_STREAM_PASTE30, \ + BYTE_STREAM_PASTE29, \ + BYTE_STREAM_PASTE28, \ + BYTE_STREAM_PASTE27, \ + BYTE_STREAM_PASTE26, \ + BYTE_STREAM_PASTE25, \ + BYTE_STREAM_PASTE24, \ + BYTE_STREAM_PASTE23, \ + BYTE_STREAM_PASTE22, \ + BYTE_STREAM_PASTE21, \ + BYTE_STREAM_PASTE20, \ + BYTE_STREAM_PASTE19, \ + BYTE_STREAM_PASTE18, \ + BYTE_STREAM_PASTE17, \ + BYTE_STREAM_PASTE16, \ + BYTE_STREAM_PASTE15, \ + BYTE_STREAM_PASTE14, \ + BYTE_STREAM_PASTE13, \ + BYTE_STREAM_PASTE12, \ + BYTE_STREAM_PASTE11, \ + BYTE_STREAM_PASTE10, \ + BYTE_STREAM_PASTE9, \ + BYTE_STREAM_PASTE8, \ + BYTE_STREAM_PASTE7, \ + BYTE_STREAM_PASTE6, \ + BYTE_STREAM_PASTE5, \ + BYTE_STREAM_PASTE4, \ + BYTE_STREAM_PASTE3, \ + BYTE_STREAM_PASTE2, \ + BYTE_STREAM_PASTE1)(__VA_ARGS__)) + + +#define BYTE_STREAM_PASTE2(func, v1) func(v1) +#define BYTE_STREAM_PASTE3(func, v1, v2) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE2(func, v2) +#define BYTE_STREAM_PASTE4(func, v1, v2, v3) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE3(func, v2, v3) +#define BYTE_STREAM_PASTE5(func, v1, v2, v3, v4) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE4(func, v2, v3, v4) +#define BYTE_STREAM_PASTE6(func, v1, v2, v3, v4, v5) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE5(func, v2, v3, v4, v5) +#define BYTE_STREAM_PASTE7(func, v1, v2, v3, v4, v5, v6) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE6(func, v2, v3, v4, v5, v6) +#define BYTE_STREAM_PASTE8(func, v1, v2, v3, v4, v5, v6, v7) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE7(func, v2, v3, v4, v5, v6, v7) +#define BYTE_STREAM_PASTE9(func, v1, v2, v3, v4, v5, v6, v7, v8) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE8(func, v2, v3, v4, v5, v6, v7, v8) +#define BYTE_STREAM_PASTE10(func, v1, v2, v3, v4, v5, v6, v7, v8, v9) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE9(func, v2, v3, v4, v5, v6, v7, v8, v9) +#define BYTE_STREAM_PASTE11(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE10(func, v2, v3, v4, v5, v6, v7, v8, v9, v10) +#define BYTE_STREAM_PASTE12(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE11(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11) +#define BYTE_STREAM_PASTE13(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE12(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12) +#define BYTE_STREAM_PASTE14(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE13(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13) +#define BYTE_STREAM_PASTE15(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE14(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14) +#define BYTE_STREAM_PASTE16(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE15(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15) +#define BYTE_STREAM_PASTE17(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE16(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) +#define BYTE_STREAM_PASTE18(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE17(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17) +#define BYTE_STREAM_PASTE19(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE18(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18) +#define BYTE_STREAM_PASTE20(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE19(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19) +#define BYTE_STREAM_PASTE21(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE20(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20) +#define BYTE_STREAM_PASTE22(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE21(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21) +#define BYTE_STREAM_PASTE23(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE22(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22) +#define BYTE_STREAM_PASTE24(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE23(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23) +#define BYTE_STREAM_PASTE25(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE24(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24) +#define BYTE_STREAM_PASTE26(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE25(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25) +#define BYTE_STREAM_PASTE27(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE26(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26) +#define BYTE_STREAM_PASTE28(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE27(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27) +#define BYTE_STREAM_PASTE29(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE28(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28) +#define BYTE_STREAM_PASTE30(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE29(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29) +#define BYTE_STREAM_PASTE31(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE30(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30) +#define BYTE_STREAM_PASTE32(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE31(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31) +#define BYTE_STREAM_PASTE33(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE32(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32) +#define BYTE_STREAM_PASTE34(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE33(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33) +#define BYTE_STREAM_PASTE35(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE34(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34) +#define BYTE_STREAM_PASTE36(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE35(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35) +#define BYTE_STREAM_PASTE37(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE36(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36) +#define BYTE_STREAM_PASTE38(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE37(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37) +#define BYTE_STREAM_PASTE39(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE38(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38) +#define BYTE_STREAM_PASTE40(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE39(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39) +#define BYTE_STREAM_PASTE41(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE40(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40) +#define BYTE_STREAM_PASTE42(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE41(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41) +#define BYTE_STREAM_PASTE43(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE42(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42) +#define BYTE_STREAM_PASTE44(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE43(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43) +#define BYTE_STREAM_PASTE45(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE44(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44) +#define BYTE_STREAM_PASTE46(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE45(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45) +#define BYTE_STREAM_PASTE47(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE46(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46) +#define BYTE_STREAM_PASTE48(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE47(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47) +#define BYTE_STREAM_PASTE49(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE48(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48) +#define BYTE_STREAM_PASTE50(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE49(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49) +#define BYTE_STREAM_PASTE51(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE50(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50) +#define BYTE_STREAM_PASTE52(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE51(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51) +#define BYTE_STREAM_PASTE53(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE52(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52) +#define BYTE_STREAM_PASTE54(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE53(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53) +#define BYTE_STREAM_PASTE55(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE54(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54) +#define BYTE_STREAM_PASTE56(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE55(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55) +#define BYTE_STREAM_PASTE57(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE56(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56) +#define BYTE_STREAM_PASTE58(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE57(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57) +#define BYTE_STREAM_PASTE59(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE58(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58) +#define BYTE_STREAM_PASTE60(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE59(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59) +#define BYTE_STREAM_PASTE61(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE60(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60) +#define BYTE_STREAM_PASTE62(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE61(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61) +#define BYTE_STREAM_PASTE63(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61, v62) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE62(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61, v62) +#define BYTE_STREAM_PASTE64(func, v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61, v62, v63) BYTE_STREAM_PASTE2(func, v1) BYTE_STREAM_PASTE63(func, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49, v50, v51, v52, v53, v54, v55, v56, v57, v58, v59, v60, v61, v62, v63) + +#define BYTE_STREAM_TO(v1) \ +ret = storeToByteStreamBigEndian(struct_data.v1, byte_stream_data); \ +if(!ret){std::cerr << "转换字节流失败" << std::endl;} + +#define BYTE_STREAM_FROM(v1) \ + start_pose = end_pose; \ + if (std::is_same_v){ \ + end_pose = end_pose + (byte_stream_data.size() - struct_size); \ + } else{ \ + end_pose = end_pose + getStoreByteSize(struct_data.v1); \ + } \ + ret = storeFromByteStreamBigEndian(struct_data.v1, \ + std::vector(byte_stream_data.begin()+start_pose, byte_stream_data.begin()+end_pose) \ + );\ + if(!ret){std::cerr << "转换结构体失败" << std::endl;} +#define STRUCT_SIZE(v1) struct_size = struct_size + getStoreByteSize(struct_data.v1); + +#define PIXMOVING_SERDE_BS_STRUCT(StructType, ...) \ + uint16_t get_struct_non_string_size(StructType&struct_data){ \ + std::uint16_t struct_size = 0; \ + BYTE_STREAM_EXPAND(BYTE_STREAM_PASTE(STRUCT_SIZE, __VA_ARGS__)) \ + return struct_size;} \ + void to_byte_stream(std::vector&byte_stream_data ,const StructType&struct_data) { \ + bool ret; \ + byte_stream_data.clear(); \ + BYTE_STREAM_EXPAND(BYTE_STREAM_PASTE(BYTE_STREAM_TO, __VA_ARGS__))} \ + void from_byte_stream(StructType&struct_data, const std::vector&byte_stream_data) { \ + bool ret; \ + std::uint16_t start_pose=0; \ + std::uint16_t end_pose=0; \ + std::uint16_t struct_size=get_struct_non_string_size(struct_data); \ + if(struct_size>=byte_stream_data.size()){ \ + std::cout << "struct_size>=byte_stream_data.size()" \ + <=" << byte_stream_data.size() \ + << std::endl; \ + return; \ + }\ + BYTE_STREAM_EXPAND(BYTE_STREAM_PASTE(BYTE_STREAM_FROM, __VA_ARGS__))} \ + StructType::operator std::vector() const{ \ + std::vector byteStream; \ + to_byte_stream(byteStream, *this); \ + return byteStream; \ + }\ + StructType& StructType::operator=(const std::vector& byte_stream){ \ + from_byte_stream(*this, byte_stream); \ + return *this; \ + } +#endif + +/* +PIXMOVING_SERIALIZE_DESERIALIZE_BYTE_STREAM_STRUCT -> PIXMOVING_SERDE_BS_STRUCT +SERialize/DEserialize -> SERDE 序列反序列化 +BYTE_STREAM -> BS 字节流 +*/ \ No newline at end of file diff --git a/include/obu/udp_server.hpp b/include/obu/udp_server.hpp new file mode 100644 index 0000000..425423e --- /dev/null +++ b/include/obu/udp_server.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +class udp_server { +public: + udp_server(const std::string& serverAddress, int serverPort); + ~udp_server(); + + // 启动UDP服务器并开始接收数据 + std::vector receive_data(); + + // 发送数据到客户端 + void send_to_client(std::vector message,size_t len_); + +private: + int socket_fd_; + char buffer[2048]; + struct sockaddr_in client_address; + struct sockaddr_in server_address_; +}; \ No newline at end of file diff --git a/launch/obu.lacunh b/launch/obu.lacunh new file mode 100644 index 0000000..e69de29 diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..cb693a9 --- /dev/null +++ b/package.xml @@ -0,0 +1,23 @@ + + + + obu + 0.0.0 + TODO: Package description + runze + TODO: License declaration + + ament_cmake + rclcpp + std_msgs + geometry_msgs + jsoncpp + autoware_auto_perception_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/data_process.cpp b/src/data_process.cpp new file mode 100644 index 0000000..150c605 --- /dev/null +++ b/src/data_process.cpp @@ -0,0 +1,207 @@ +#include "obu/data_process.hpp" +data_process::data_process(rclcpp::Node::SharedPtr node){ + node_ = node; +}; +data_process:: ~data_process(){}; +std::string data_process::get_string(){ + json sceneData = { + {"scene", { + {"fcw", { + {"vehicleID", 12548456}, + {"position", 0}, + {"distance", 20.0}, + {"alarm_class", 1} + }}, + {"icw", { + {"vehicleID", 12548456}, + {"position", 0}, + {"direction", 0}, + {"distance", 20.0}, + {"alarm_class", 1} + }}, + {"bsw", { + {"vehicleID", 12548456}, + {"position", 0}, + {"direction", 0}, + {"distance", 20.0}, + {"alarm_class", 1} + }}, + {"lcw", { + {"vehicleID", 12548456}, + {"position", 0}, + {"direction", 0}, + {"distance", 20.0}, + {"alarm_class", 1} + }}, + {"dnpw", { + {"vehicleID", 12548456}, + {"position", 0}, + {"direction", 0}, + {"distance", 20.0}, + {"alarm_class", 1} + }}, + {"ebw", { + {"vehicleID", 12548456}, + {"position", 0}, + {"direction", 0}, + {"distance", 20.0}, + {"alarm_class", 1} + }}, + {"avw", { + {"vehicleID", 12548456}, + {"position", 0}, + {"distance", 20.0}, + {"alarm_class", 1} + }}, + {"clw", { + {"vehicleID", 12548456}, + {"position", 0}, + {"distance", 20.0}, + {"alarm_class", 1} + }}, + {"evw", { + {"vehicleID", 12548456}, + {"position", 0}, + {"hdistance", 10.0}, + {"vdistance", 20.0}, + {"alarm_class", 1} + }}, + {"lta", { + {"vehicleID", 12548456}, + {"position", 0}, + {"hdistance", 10.0}, + {"ttp", 0.0}, + {"alarm_class", 1} + }}, + {"ivs", json::array({ + { + {"id", 1}, + {"sign type", 10}, + {"description", "注意行人"}, + {"distance", 120}, + {"latitude", 0.0}, + {"longitude", 0.0} + }, + { + {"id", 2}, + {"sign type", 38}, + {"description", "施工"}, + {"distance", 120}, + {"latitude", 0.0}, + {"longitude", 0.0} + } + })}, + {"tjw", { + {"congestion_level", 1}, + {"range_latitude", 0.0}, + {"range_longitude", 0.0} + }}, + {"slw", { + {"speed", 10.0}, + {"latitude", 0.0}, + {"longitude", 0.0} + }}, + {"preempt", { + {"direction", 1} + }}, + {"glosa", { + {"latitude", 0.0}, + {"longitude", 0.0}, + {"glosa_info", json::array({ + { + {"state_active", 1}, + {"state", 3}, + {"timing", 22}, + {"maneuvers", 2}, + {"advisory_active", 1}, + {"max_speed", 0}, + {"min_speed", 0} + }, + { + {"state_active", 0}, + {"state", 0}, + {"timing", 0}, + {"maneuvers", 0}, + {"advisory_active", 0}, + {"max_speed", 0}, + {"min_speed", 0} + } + })} + }}, + {"rlvw", { + {"distance", 120}, + {"maneuvers", 0}, + {"state", 0}, + {"timing", 12}, + {"latitude", 0}, + {"longitude", 0} + }}, + {"vrucw", { + {"type", 0}, + {"latitude", 0.0}, + {"longitude", 0.0}, + {"position", 0}, + {"planning_count", 1}, + {"planning_list", json::array({ + { + {"path_planning_count", 1}, + {"path_planning_points", json::array({ + { + {"latitude", 0.0}, + {"longitude", 0.0} + }, + { + {"latitude", 0.0}, + {"longitude", 0.0} + } + })} + } + })} + }} + }} + }; + std::string jsonStr = sceneData.dump(); + return jsonStr; +} + +std::vector data_process::packed_data(){ + Message msg; + time_t timep; + std::vector byte_stream_data; + std::string msgbody = get_string(); + time(&timep); /*当前time_t类型UTC时间*/ + msg.timestamp = (uint64_t)timep; + msg.message_length = msgbody.length(); + msg.message_body = msgbody.data(); + msg.check_code = 0x05; + byte_stream_data = msg; + // 输出字节流数据 + // show(byte_stream_data); + return byte_stream_data; +} + +void data_process::parse_data(std::vector received_data){ + Message receive_message; + TrafficSignalArray traffic_signal_array_msg_; + TrafficSignal traffic_signal_msg_; + TrafficLight traffic_light_msg_; + traffic_signal_array_pub_ = node_->create_publisher("/perception/traffic_light_recognition/traffic_signals", rclcpp::QoS(1)); + + receive_message = received_data; + json parsed_json = json::parse(receive_message.message_body); + std::cout<<"state_active:"<now(); + traffic_signal_array_msg_.header.frame_id = "obu"; + traffic_signal_array_msg_.signals.push_back(traffic_signal_msg_); + traffic_signal_array_pub_->publish(traffic_signal_array_msg_); +} \ No newline at end of file diff --git a/src/obu_socket_node.cpp b/src/obu_socket_node.cpp new file mode 100644 index 0000000..1ed2a4a --- /dev/null +++ b/src/obu_socket_node.cpp @@ -0,0 +1,26 @@ +#include "obu/data_process.hpp" +#include "rclcpp/rclcpp.hpp" +#include "obu/udp_server.hpp" + +// 创建字节流转化函数 +PIXMOVING_SERDE_BS_STRUCT(Message,header,version,timestamp,device_type,device_id,data_direction,data_type, + encryption_type,reserved,message_length,message_body,check_code); + +int main(int argc, char** argv) { + + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("obu_server_node"); + // 创建UDP服务器实例 + udp_server udp_server("127.0.0.1", 8751); + data_process data_process_(node); + + while(rclcpp::ok()){ + udp_server.send_to_client(data_process_.packed_data(),29+data_process_.get_string().length()); + data_process_.parse_data(udp_server.receive_data()); + } + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; + +} diff --git a/src/udp_server.cpp b/src/udp_server.cpp new file mode 100644 index 0000000..518c581 --- /dev/null +++ b/src/udp_server.cpp @@ -0,0 +1,56 @@ +#include "obu/udp_server.hpp" +udp_server::udp_server(const std::string& serverAddress, int serverPort) { + // 创建UDP套接字 + socket_fd_ = socket(AF_INET, SOCK_DGRAM, 0); + if (socket_fd_ == -1) { + std::cerr << "Error: Could not create socket." << std::endl; + exit(EXIT_FAILURE); + } + + // 设置服务器地址 + memset(&server_address_, 0, sizeof(server_address_)); + server_address_.sin_family = AF_INET; + server_address_.sin_port = htons(serverPort); + server_address_.sin_addr.s_addr = inet_addr(serverAddress.c_str()); + + // // 设置客户端地址 + memset(&client_address, 0, sizeof(client_address)); + client_address.sin_family = AF_INET; + client_address.sin_port = htons(9886); // 目标端口 + client_address.sin_addr.s_addr = inet_addr("127.0.0.2"); // 目标 IP 地址 + + // 将套接字与服务器地址绑定 + if (bind(socket_fd_, (struct sockaddr*)&server_address_, sizeof(server_address_)) == -1) { + std::cerr << "Error: Bind failed." << std::endl; + exit(EXIT_FAILURE); + } +} + +udp_server::~udp_server() { + close(socket_fd_); +} + +std::vector udp_server::receive_data(){ + + // received_data.clear(); + memset(buffer, 0, sizeof(buffer)); + socklen_t client_address_len_ = sizeof(client_address); + + ssize_t bytes_received = recvfrom(socket_fd_, buffer, sizeof(buffer), 0, + (struct sockaddr*)&client_address, &client_address_len_); + if (bytes_received == -1) { + std::cerr << "Error: Send failed.bytesReceived:" < received_data(buffer, buffer + bytes_received); + return received_data; +} + +void udp_server::send_to_client(std::vector message , size_t len_) { + ssize_t bytes_sent = sendto(socket_fd_, message.data(),len_, 0, + (struct sockaddr*)&client_address, sizeof(client_address)); + if (bytes_sent == -1) { + std::cerr << "Error: Send failed." << std::endl; + } + std::cout<<"bytesSent:"<