diff --git a/CMakeLists.txt b/CMakeLists.txt index f73b530..bc2be57 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,234 +1,31 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(obu) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - roscpp - rospy - message_generation - std_msgs - pix_driver_msgs - autoware_perception_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) +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() -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# geometry_msgs -# ) +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 -add_service_files(FILES OBUinterflowVehicle.srv) -generate_messages(DEPENDENCIES std_msgs autoware_perception_msgs) -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES obu -# CATKIN_DEPENDS geometry_msgs roscpp rospy -# DEPENDS system_lib - CATKIN_DEPENDS roscpp rospy std_msgs message_runtime pix_driver_msgs autoware_perception_msgs ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/obu.cpp -# ) - - - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/obu_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -catkin_install_python(PROGRAMS - scripts/obu/chassisAbnormalnformationUpload.py - scripts/obu/chassisRealTimeInformationUpload.py - scripts/obu/chassisRegistrationInformationReport.py - scripts/obu/chassisStatusInformationUpload.py - scripts/obu/config.py - scripts/obu/mySocket.py - scripts/obu/udp_interface_node.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(TARGETS - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -install(DIRECTORY +# install +ament_auto_package( + INSTALL_TO_SHARE launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_obu.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) + include + # config +) diff --git a/README.md b/README.md deleted file mode 100644 index e8eea9d..0000000 --- a/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# pix_obu -obu通信地盘基于citybot_v2 diff --git a/include/obu/data_process.hpp b/include/obu/data_process.hpp new file mode 100644 index 0000000..8632a1b --- /dev/null +++ b/include/obu/data_process.hpp @@ -0,0 +1,62 @@ +#pragma once +#include +#include +#include +#include "macro_scope.hpp" +#include +#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; + using Float32 = std_msgs::msg::Float32; + void callback_heading(const Float32::SharedPtr msg); + int Lanlet_id = 0; + + rclcpp::Publisher::SharedPtr traffic_signal_array_pub_; + rclcpp::Subscription::SharedPtr heading_sub_; + + Message receive_message; + TrafficSignalArray traffic_signal_array_msg_; + TrafficSignal traffic_signal_msg_; + TrafficLight traffic_light_msg_; + +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/launch/obu.launch.xml b/launch/obu.launch.xml new file mode 100644 index 0000000..4b58b0f --- /dev/null +++ b/launch/obu.launch.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/launch/obu_run.launch b/launch/obu_run.launch deleted file mode 100644 index 1661797..0000000 --- a/launch/obu_run.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - --> - - - - - - - diff --git a/package.xml b/package.xml index a9416ee..a38bf34 100644 --- a/package.xml +++ b/package.xml @@ -1,78 +1,24 @@ - + + obu 0.0.0 - The obu package + TODO: Package description + runze + TODO: License declaration - - - - six + ament_cmake + rclcpp + std_msgs + geometry_msgs + jsoncpp + autoware_auto_perception_msgs + ublox_msgs + ament_lint_auto + ament_lint_common - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - geometry_msgs - roscpp - rospy - pix_driver_msgs - autoware_perception_msgs - geometry_msgs - roscpp - rospy - pix_driver_msgs - autoware_perception_msgs - geometry_msgs - roscpp - rospy - std_msgs - std_msgs - pix_driver_msgs - autoware_perception_msgs - message_generation - message_runtime - - - - - + ament_cmake diff --git a/scripts/obu/chassisAbnormalnformationUpload.py b/scripts/obu/chassisAbnormalnformationUpload.py deleted file mode 100755 index 227ceac..0000000 --- a/scripts/obu/chassisAbnormalnformationUpload.py +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/env python -# -*- coding:utf-8 -*- -""" -3.3.1.4 车载主机车辆异常信息 -- 触发 chassis abnormal Information report - -""" -import rospy -import json -from obu.srv import * -from pix_driver_msgs.msg import VcuReport -from pix_driver_msgs.msg import BrakeReport - -class AbnormalInformationUpload: - def __init__(self): - self.server = rospy.Service("/obu/chassis/AbnormalInformationUpload",OBUinterflowVehicle, self.callback_doReq) - self.sub_vcu_report = rospy.Subscriber('/pix/vcu_report', VcuReport, self.vcu_report_callback) - self.sub_brake_report = rospy.Subscriber("/pix/break_report",BrakeReport,self.brake_report_callback) - self.msgMap={ - "vehicleElectronicState":"0", - "carAbnormal":1 - } - # self.msgMap={"warningType":1, - # "signType":0, - # "evenType":0, - # "warningInfo":" 前 车 碰 撞 预 警", - # "warningSuggestion":"请减速，前方有碰撞危险", - # "distant":40} - - def callback_doReq(self, req): - msg = json.dumps(self.msgMap, ensure_ascii=False) - return msg - - def vcu_report_callback(self,msg): - self.msgMap["vehicleElectronicState"] = str(msg.chassis_errcode) - - def brake_report_callback(self,msg): - if (msg.brake_flt2 == 1 or msg.brake_flt1 ==1 ): - self.msgMap["carAbnormal"] = 64 - - - - - -if __name__ == "__main__": - rospy.init_node("obu_chassis_AbnormalInformationUpload") - AbnormalInformationUpload() - rospy.spin() \ No newline at end of file diff --git a/scripts/obu/chassisRealTimeInformationUpload.py b/scripts/obu/chassisRealTimeInformationUpload.py deleted file mode 100755 index 9d2b07d..0000000 --- a/scripts/obu/chassisRealTimeInformationUpload.py +++ /dev/null @@ -1,223 +0,0 @@ -#!/usr/bin/env python -# -*- coding:utf-8 -*- -import rospy -import json -from obu.srv import * -from pix_driver_msgs.msg import BrakeReport -from pix_driver_msgs.msg import GearReport -from pix_driver_msgs.msg import SteerReport -from pix_driver_msgs.msg import VcuReport -from pix_driver_msgs.msg import ParkReport -from pix_driver_msgs.msg import ThrottleReport -from sensor_msgs.msg import NavSatFix, Imu -from pix_driver_msgs.msg import VehicleModeCommand -from tf import transformations - - -class RealTimeInformationUpload: - def __init__(self): - self.server = rospy.Service("/obu/chassis/RealTimeInformationUpload",OBUinterflowVehicle, self.callback_doReq) - - #转向，车速，启动状态，驾驶模式，车辆状态，加速度，刹车灯 - self.sub_vcu_report = rospy.Subscriber('/pix/vcu_report', VcuReport, self.vcu_report_callback) - #刹车 - self.sub_brake_report = rospy.Subscriber("/pix/break_report",BrakeReport,self.brake_report_callback) - #倒车 - self.sub_park_report = rospy.Subscriber("/pix/park_report",ParkReport,self.park_report_callback) - #档位状态 - self.sub_gear_report = rospy.Subscriber("/pix/gear_report",GearReport,self.gear_report_callback) - #方向盘转角，转角速度 - self.sub_steering_report = rospy.Subscriber("/pix/steering_report",SteerReport,self.steering_report_callback) - #油门状态 - self.sub_throttle_report = rospy.Subscriber("/pix/throttle_report",ThrottleReport,self.throttle_report_callback) - #车门 - self.vehicle_command = rospy.Subscriber("/pix/vehicle_command",VehicleModeCommand,self.vehicle_command_callback) - # ？？？？ - # 经纬度，海拔，车身姿态，定位状态，从车速度和距离，电机功率转速，横向纵向加速度，住车灯，刹车灯，倒车灯？？ - self.sub_lon_lat = rospy.Subscriber("/fix", NavSatFix, self.lon_lat_callback)#经纬度 - # /sensing/imu/imu_data - self.sub_imu= rospy.Subscriber("/chc_imu", Imu, self.Imu_callback)#imu数据 - - - self.msgMap={ - "LocationType":31, - #经纬度海拔 - "longitude":113.5951157, - "latitude":22.74467066, - "altitude":26.04, - #定位精度 - "accuracy":1.0, - - "braking":0, - "gear":bytes.decode("N"), - "speed":0, - "SWA":0, - "turnLight":0, - #车身位姿，imu数据deg - "vehiclePosture":{ - "postureX":0, - "postureY":0, - "postureZ":0}, - - #从车位置 - "relativePosition":{ - "speed2":45, - "distanceR0":1, - "distanceRY":3, - "distanceRX":3, - "distance0":3}, - - "ifCarOnline":1, - "throttle":30, - #电机转速 - "rotationRate":50, - - #电机功率 - " power":100, - - "carMode1":2, - "carStatus":3, - "SterringAngleSpd":34, - - # 车辆纵向，横向加速度 - "VehLatrlAccel":1, - "VehLongAccell":1, - - #驾驶位门锁 - "DriverDoorLockSt":0, - - - "BrakeLightSwitchSt":0, - "ParkingLampSt":1, - - #住车灯，倒车灯 - "Reverse_light_status":0 - } - - def park_report_callback(self,msg): - self.msgMap["ParkingLampSt"] = msg.parking_actual - - - def callback_doReq(self, req): - print(self.msgMap) - return json.dumps(self.msgMap,ensure_ascii=False) - - def vcu_report_callback(self,msg): - self.msgMap["speed"] = msg.speed - self.msgMap["turnLight"] =msg.turn_light_actual - - #启动判断 - # if ( msg.CarPower_State < 2): - # # 0不再线1在线 - # self.msgMap["ifCarOnline"] = msg.CarPower_State - # else: - # self.msgMap["ifCarOnline"] = 0 - - - # 模式判断 - # 0x3: Standby Mode - # 0x2: Emergency Mode - # 0x1: Auto Mode - # 0x0: Manual Remote Mode - # msg.Vehicle_ModeState - #msg.vehicle_mode_state - if (msg.vehicle_mode_state == 0):#手动远程模式 - self.msgMap["carMode1"] = 1 - - elif(msg.vehicle_mode_state == 1):#自动驾驶模式 - self.msgMap["carMode1"] = 2 - - elif(msg.vehicle_mode_state == 2):#紧急模式 - self.msgMap["carMode1"] = 255 - - elif(msg.vehicle_mode_state == 3):#待机模式 - self.msgMap["carMode1"] = 255 - - # 车辆状态 - # "0x7: crash - # 0x6: error - # 0x5: E-stop - # 0x4: work - # 0x3: 3 - # 0x2: 2 - # 0x1: 1 - # 0x0: init - # " - - # 表示车辆状态，有未知 - # （FF），未启动(1)，停车 - # （2）或驾驶中(3)四种，数 - # 值类型如：1，未知为 255 - - # if (msg.CarWork_State == 4):# - # self.msgMap["carStatus"] = 3 - - # elif (msg.CarWork_State == 5):# - # self.msgMap["carStatus"] = 2 - # else: - # self.msgMap["carStatus"] = 255 - - # 车辆纵向，横向加速度 - self.msgMap["VehLongAccell"] = msg.acc#车辆加速度 - - #刹车灯 - self.msgMap["BrakeLightSwitchSt"] = msg.brake_light_actual - - def brake_report_callback(self,msg): - self.msgMap["braking"] = msg.brake_pedal_actual - - def gear_report_callback(self,msg): - if (msg.gear_actual == 1): - self.msgMap["gear"] = 80 - - elif (msg.gear_actual == 2): - self.msgMap["gear"] = 82 - - elif (msg.gear_actual == 3): - self.msgMap["gear"] = 78 - - elif (msg.gear_actual == 4): - self.msgMap["gear"] = 68 - else: - self.msgMap["gear"] = 0xff - - def steering_report_callback(self,msg): - if(msg != None): - self.msgMap["SWA"] = msg.steer_angle_actual - self.msgMap["SterringAngleSpd"] = msg.steer_angle_spd_actual - - else: - self.msgMap["SWA"] = 0 - self.msgMap["SterringAngleSpd"] = 0 - - return 1 - - def throttle_report_callback(self,msg): - self.msgMap["throttle"] = msg.throttle_pedal_actual - return 1 - -#0-invaild 1-close_door 2-open_door - def vehicle_command_callback(self,msg): - if (msg.door_status_ctrl ==1 ): - self.msgMap["DriverDoorLockSt"] = 1 - elif (msg.door_status_ctrl ==2 ): - self.msgMap["DriverDoorLockSt"] = 0 - else: - self.msgMap["DriverDoorLockSt"] = 255 - - def lon_lat_callback(self, msg): - self.msgMap["longitude"] = msg.longitude - self.msgMap["latitude"] = msg.latitude - self.msgMap["altitude"] = msg.altitude - - def Imu_callback(self,msg): - (r, p, y) = transformations.euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]) - self.msgMap["vehiclePosture"]["postureX"]= p - self.msgMap["vehiclePosture"]["postureY"]= r - self.msgMap["vehiclePosture"]["postureZ"]= y - self.msgMap["VehLatrlAccel"] = msg.linear_acceleration.y - -if __name__ == "__main__": - rospy.init_node("obu_chassis_RealTimeInformationUpload") - RealTimeInformationUpload() - rospy.spin() \ No newline at end of file diff --git a/scripts/obu/chassisRegistrationInformationReport.py b/scripts/obu/chassisRegistrationInformationReport.py deleted file mode 100755 index 6c6d7d0..0000000 --- a/scripts/obu/chassisRegistrationInformationReport.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python -# -*- coding:utf-8 -*- -""" -3.3.1.1 车载主机信息注册 -- 双向 一次 chassis Registration Information report -""" -import rospy -import json -from obu.srv import OBUinterflowVehicleResponse, OBUinterflowVehicle, OBUinterflowVehicleRequest -from pix_driver_msgs.msg import VehicleModeCommand - -class RegistrationInformationReport: - def __init__(self): - self.server = rospy.Service("/obu/chassis/RegistratIonInformationReport",OBUinterflowVehicle, self.callback_doReq) - self.vehicle_command = rospy.Subscriber("/pix/vehicle_command",VehicleModeCommand,self.vehicle_command_callback) - self.msgMap={ - # "vin_IN":"车辆标识,车架号, 字符类型", - "vin":"P-CQZW4110072A0N08001", - - # "plateNumber_IN":"车牌号, 字符类型。如:粤A0V08", - "plateNumber":"粤A0V09", - - # "personInCharge_IN":"安全员名字", - "personInCharge":"PIXMOVING", - - # "manufacturer_IN":"车辆厂商名字", - "manufacturer":"PIXMOVING", - - # "company_IN":"无人驾驶公司名字", - "company":"PIXMOVING", - - # "vehicleType_IN":"车辆类型 uint8 有未知(FF)纯电动(1),纯油 (2)，混合动力(3)四种", - "vehicleType":1, - - # "vehicleProperty_IN":"车辆类型 uint8 有未知(FF) 测试(1) 运营(2)", - "vehicleProperty":1, - - # "carType_IN":"车辆种类, A为轿车(5 座及以下)、B为小巴、C为中巴、D为大巴、E为货车、F为卡车、G挂车、H为物流车(包含外卖、快递等)、I 为清洁车、J 为巡逻车、K 为消毒车、L 为售卖车（销售 饮料等)", - "carType":"B", - - # "brand_IN":"车辆品牌, 如:奥迪、阿尔法·罗密欧、奔驰、宝马、红旗、Jeep、捷豹、雷克萨斯、凯迪拉克、路 虎、林肯、讴歌、乔治·巴顿、沃尔沃、英菲尼迪……。 如:日产,注意不填时,brand为null,即不上报, 不要填字符串”null”、””、“none", - "brand":"PIXMOVING", - - # "series_IN":"车系,车型，如奥迪 a6中的a6, 林肯MKZ。如:MKZ", - "series":"PIXMOVING", - - # "equip_code_IN":"测试车企设备编码", - "equip_code":"PIXMOVING004", - - # "equip_name_IN":"测试车企设备名称", - "equip_name":"PIXMOVING" - } - def vehicle_command_callback(self,msg): - self.msgMap["vin"] = msg.vin_req - - def callback_doReq(self, req): - msg = json.dumps(self.msgMap,ensure_ascii=False) - return msg - - - -if __name__ == "__main__": - rospy.init_node("obu_chassis_RegistratIonInformationReport") - RegistrationInformationReport() - rospy.spin() \ No newline at end of file diff --git a/scripts/obu/chassisStatusInformationUpload.py b/scripts/obu/chassisStatusInformationUpload.py deleted file mode 100755 index 9999a66..0000000 --- a/scripts/obu/chassisStatusInformationUpload.py +++ /dev/null @@ -1,68 +0,0 @@ -#!/usr/bin/env python -# -*- coding:utf-8 -*- -import rospy -import json -import time -from obu.srv import OBUinterflowVehicle,OBUinterflowVehicleRequest,OBUinterflowVehicleResponse -from pix_driver_msgs.msg import BmsReport - - -class StatusInformationUpload: - def __init__(self): - self.server = rospy.Service("/obu/chassis/StatusInformationUpload",OBUinterflowVehicle, self.callback_doReq) - self.sub_bms_report = rospy.Subscriber("/pix/bms_report",BmsReport,self.bms_report_callback) - self.count = 0 - - self.msgMap={ - #剩余电量% - "batteryQuantity":0, - - "oilQuantity":255, - #理论可以计算得出（电池电压×电池容量×时速÷电机功率） - "restdistance":0, - #定位状态1就绪,2未就绪,255未知 - "locationModelState":1, - #电机状态1就绪,2未就绪,255未知 - "electricalMachineState":1, - #传感器状态1就绪，2未就绪，255未知 - "sensorState":1, - - #无法获取不上报 - # "departure":" 广 州 市 南 沙 区 海 滨 路", - # "dep_longitude":113.6145444, - # "dep_latitude":22.74776556, - - "destination":"广州市", - "des_longitude":113.28064, - "des_latitude":23.125177, - - "cloudState":1, - "_4gState":2, - "_5gState":2, - "ltevState":4, - #摄像头状态 - "camerNum":4, - - "cameraIp":"192.168.19.11", - "cameraConfig":0, - "markType":1, - "time":"0", - "UTC":"0", - "markNum":0 - } - - def bms_report_callback(self,msg): - self.msgMap["batteryQuantity"] = msg.battery_soc - self.msgMap["restdistance"] = msg.battery_soc*100/100 - self.msgMap["time"] = time.strftime('%Y-%m-%d %H:%M:%S',time.localtime()) - self.msgMap["UTC"] = str(time.time()) - self.msgMap["markNum"] = 0 - - def callback_doReq(self, req): - return json.dumps(self.msgMap,ensure_ascii=False) - - -if __name__ == "__main__": - rospy.init_node("obu_chassis_StatusInformationUpload") - StatusInformationUpload() - rospy.spin() \ No newline at end of file diff --git a/scripts/obu/config.py b/scripts/obu/config.py deleted file mode 100644 index a76314e..0000000 --- a/scripts/obu/config.py +++ /dev/null @@ -1,31 +0,0 @@ -#!/usr/bin/env python -# -*- coding:utf-8 -*- - -# 服务器配置信息 -server_ip="192.168.4.103" -# server_port=1111 -# server_ip="127.0.0.1" -server_port=1247 - -""" -一般网络人为协议都是前缀+消息体+后缀 -<小端 >大端 -消息协议：命令码+消息体长度+消息体 -命令码 1Byte -消息体长度 4Byte -消息体 NByte -""" -msg_format_prefix="create_publisher("/perception/traffic_light_recognition/traffic_signals", rclcpp::QoS(1)); + + heading_sub_ = node_->create_subscription("/sensing/gnss/heading", 10, + std::bind(&data_process::callback_heading, this, std::placeholders::_1)); +}; +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; +} + +void data_process::callback_heading(const Float32::SharedPtr msg){ + if (msg->data < 180){ + traffic_signal_msg_.map_primitive_id = 11071; + //0 左转,1 直行,2 右转 + Lanlet_id = 0; + + } + else{ + traffic_signal_msg_.map_primitive_id = 15666; + Lanlet_id = 1; + } + std::cout<<"test"< 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){ + receive_message = received_data; + traffic_signal_msg_.lights.clear(); + traffic_signal_array_msg_.signals.clear(); + json parsed_json = json::parse(receive_message.message_body); + // 3：红，6：绿，7：黄 + 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..a63ae3d --- /dev/null +++ b/src/obu_socket_node.cpp @@ -0,0 +1,31 @@ +#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()); + std::cout<<"send over"< 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_) { + std::cout<<"std::"<