diff --git a/.gitignore b/.gitignore index 202644c..ef3c35a 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ *.txt.user +.vscode +build diff --git a/.gitmodules b/.gitmodules index 7df1396..907d543 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "src/utils/nlink_unpack"] path = src/utils/nlink_unpack url = https://github.com/nooploop-dev/nlink_unpack.git +[submodule "extern/serial"] + path = extern/serial + url = https://github.com/wjwwood/serial.git diff --git a/CMakeLists.txt b/CMakeLists.txt index c659564..ec9923e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,20 +1,25 @@ cmake_minimum_required(VERSION 2.8.3) project(nlink_parser) -## Compile as C++11, supported in ROS Kinetic and newer -#add_compile_options(-std=c++11) +# # Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) add_definitions(-std=c++11) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages +# # 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 roscpp std_msgs message_generation - ) +) + +add_subdirectory(extern/serial) +include_directories(extern/serial/include) -## Generate messages in the 'msg' folder +# find_package(serial CONFIG REQUIRED) + +# # Generate messages in the 'msg' folder add_message_files( FILES LinktrackAnchorframe0.msg @@ -31,46 +36,59 @@ add_message_files( TofsenseFrame0.msg LinktrackAoaNode0.msg LinktrackAoaNodeframe0.msg - ) + LinktrackNode4Anchor.msg + LinktrackNode4Tag.msg + LinktrackNodeframe4.msg + LinktrackNode5.msg + LinktrackNodeframe5.msg + LinktrackNode6.msg + LinktrackNodeframe6.msg + TofsenseMFrame0.msg + TofsenseMFrame0Pixel.msg + IotFrame0.msg + IotFrame0Node.msg +) -## Generate services in the 'srv' folder +# # Generate services in the 'srv' folder # add_service_files( -# FILES -# Service1.srv -# Service2.srv +# FILES +# Service1.srv +# Service2.srv # ) -## Generate actions in the 'action' folder +# # Generate actions in the 'action' folder # add_action_files( -# FILES -# Action1.action -# Action2.action +# FILES +# Action1.action +# Action2.action # ) -## Generate added messages and services with any dependencies listed here +# # Generate added messages and services with any dependencies listed here generate_messages( DEPENDENCIES std_msgs - ) +) catkin_package( - # INCLUDE_DIRS include - # LIBRARIES nlink + + # INCLUDE_DIRS include + # LIBRARIES nlink CATKIN_DEPENDS message_runtime - # DEPENDS system_lib - ) -########### -## Build ## -########### + # DEPENDS system_lib +) -## Specify additional locations of header files -## Your package locations should be listed before other locations +# ########## +# # Build ## +# ########## + +# # Specify additional locations of header files +# # Your package locations should be listed before other locations include_directories( include ${catkin_INCLUDE_DIRS} src/utils - ) +) set(nutils "nutils") add_library(${nutils} @@ -83,15 +101,20 @@ add_library(${nutils} src/utils/nlink_unpack/nlink_linktrack_nodeframe1.c src/utils/nlink_unpack/nlink_linktrack_nodeframe2.c src/utils/nlink_unpack/nlink_linktrack_nodeframe3.c + src/utils/nlink_unpack/nlink_linktrack_nodeframe4.c + src/utils/nlink_unpack/nlink_linktrack_nodeframe5.c + src/utils/nlink_unpack/nlink_linktrack_nodeframe6.c src/utils/nlink_unpack/nlink_tofsense_frame0.c + src/utils/nlink_unpack/nlink_tofsensem_frame0.c src/utils/nlink_unpack/nlink_linktrack_aoa_nodeframe0.c + src/utils/nlink_unpack/nlink_iot_frame0.c src/utils/init_serial.cpp src/utils/nlink_protocol.cpp src/utils/nutils.cpp ) target_link_libraries(${nutils} - ${catkin_LIBRARIES} - serial + ${catkin_LIBRARIES} + serial ) set(LINKTRACK_NAME "linktrack") @@ -99,62 +122,87 @@ add_executable(${LINKTRACK_NAME} src/linktrack/init.cpp src/linktrack/main.cpp src/linktrack/protocols.cpp - ) +) target_link_libraries(${LINKTRACK_NAME} -# ${catkin_LIBRARIES} + + # ${catkin_LIBRARIES} ${nutils} - ) +) add_dependencies(${LINKTRACK_NAME} ${PROJECT_NAME}_generate_messages_cpp) set(LINKTRACK_RVIZ_NAME "linktrack_rviz_converter") add_executable(${LINKTRACK_RVIZ_NAME} src/linktrack/main_rviz_converter.cpp - ) +) target_link_libraries(${LINKTRACK_RVIZ_NAME} ${nutils} - ) +) add_dependencies(${LINKTRACK_RVIZ_NAME} ${PROJECT_NAME}_generate_messages_cpp) set(TOFSENSE_NAME "tofsense") add_executable(${TOFSENSE_NAME} src/tofsense/init.cpp src/tofsense/main.cpp - ) +) target_link_libraries(${TOFSENSE_NAME} -# ${catkin_LIBRARIES} + + # ${catkin_LIBRARIES} ${nutils} - ) +) add_dependencies(${TOFSENSE_NAME} ${PROJECT_NAME}_generate_messages_cpp) -set(LINKTRACK_AOA_NAME "linktrack_aoa") -add_executable(${LINKTRACK_AOA_NAME} +set(EXE_NAME "tofsensem") +add_executable(${EXE_NAME} + src/tofsensem/init.cpp + src/tofsensem/main.cpp +) +target_link_libraries(${EXE_NAME} + ${nutils} +) +add_dependencies(${EXE_NAME} ${PROJECT_NAME}_generate_messages_cpp) + +set(EXE_NAME "linktrack_aoa") +add_executable(${EXE_NAME} src/linktrack_aoa/init.cpp src/linktrack_aoa/main.cpp src/linktrack/protocols.cpp - ) -target_link_libraries(${LINKTRACK_AOA_NAME} -# ${catkin_LIBRARIES} +) +target_link_libraries(${EXE_NAME} + + # ${catkin_LIBRARIES} ${nutils} - ) -add_dependencies(${LINKTRACK_AOA_NAME} ${PROJECT_NAME}_generate_messages_cpp) +) +add_dependencies(${EXE_NAME} ${PROJECT_NAME}_generate_messages_cpp) +set(EXE_NAME "iot") +add_executable(${EXE_NAME} + src/iot/init.cpp + src/iot/main.cpp +) +target_link_libraries(${EXE_NAME} + ${nutils} +) +add_dependencies(${EXE_NAME} ${PROJECT_NAME}_generate_messages_cpp) -############# -## Testing ## -############# +# ############ +# # Testing ## +# ############ if(${CATKIN_ENABLE_TESTING}) find_package(rostest REQUIRED) -# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") - ## Add gtest based cpp test target and link libraries - catkin_add_gtest(${PROJECT_NAME}-test + + # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") + # # Add gtest based cpp test target and link libraries + catkin_add_gtest(${PROJECT_NAME}_test src/linktrack/init.cpp src/linktrack/protocols.cpp src/tofsense/init.cpp src/linktrack_aoa/init.cpp test/test_nlink_parser.cpp - ) + src/tofsensem/init.cpp + src/iot/init.cpp + ) endif() -if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test ${nutils}) +if(TARGET ${PROJECT_NAME}_test) + target_link_libraries(${PROJECT_NAME}_test ${nutils}) endif() diff --git a/README.en.md b/README.en.md index c94eda5..a0b2002 100644 --- a/README.en.md +++ b/README.en.md @@ -37,10 +37,10 @@ Products Supported - [TOFSense](#tofsense) - [How to Subscribe Our Topic](#how-to-subscribe-our-topic) - [Submodule](#submodule) - - [nlink_unpack](#nlink_unpack) - - [protocol_extracter](#protocol_extracter) + - [nlink\_unpack](#nlink_unpack) + - [protocol\_extracter](#protocol_extracter) - [License](#license) -- [Bugs & Feature Requests](#bugs--feature-requests) +- [Bugs \& Feature Requests](#bugs--feature-requests) - [FAQ](#faq) ## Getting Started @@ -104,6 +104,8 @@ Published Topics - **`/nlink_linktrack_nodeframe1`** ([nlink_parser::LinktrackNodeframe1]) - **`/nlink_linktrack_nodeframe2`** ([nlink_parser::LinktrackNodeframe2]) - **`/nlink_linktrack_nodeframe3`** ([nlink_parser::LinktrackNodeframe3]) + - **`/nlink_linktrack_nodeframe5`** ([nlink_parser::LinktrackNodeframe5]) + - **`/nlink_linktrack_nodeframe6`** ([nlink_parser::LinktrackNodeframe6]) If data is received from other nodes, then message will be released on `/nlink_linktrack_nodeframe0` @@ -183,4 +185,4 @@ Please report bugs and request features using the [Issue Tracker](https://github 1. Run `sudo gedit /etc/ld.so.conf.d/libc.conf` 2. If there is no `/usr/local/lib`, add it 3. Save the file and execute `sudo ldconfig` - 4. Restart the computer \ No newline at end of file + 4. Restart the computer diff --git a/README.md b/README.md index 2681a3a..2af5f73 100644 --- a/README.md +++ b/README.md @@ -35,12 +35,14 @@ - [LinkTrack](#linktrack) - [LinkTrack AOA](#linktrack-aoa) - [TOFSense](#tofsense) + - [TOFSense-M](#tofsense-m) + - [IOT](#iot) - [How to Subscribe Our Topic](#how-to-subscribe-our-topic) - [Submodule](#submodule) - - [nlink_unpack](#nlink_unpack) - - [protocol_extracter](#protocol_extracter) + - [nlink\_unpack](#nlink_unpack) + - [protocol\_extracter](#protocol_extracter) - [License](#license) -- [Bugs & Feature Requests](#bugs--feature-requests) +- [Bugs \& Feature Requests](#bugs--feature-requests) - [FAQ](#faq) @@ -105,6 +107,9 @@ - **`/nlink_linktrack_nodeframe1`** ([nlink_parser::LinktrackNodeframe1]) - **`/nlink_linktrack_nodeframe2`** ([nlink_parser::LinktrackNodeframe2]) - **`/nlink_linktrack_nodeframe3`** ([nlink_parser::LinktrackNodeframe3]) + - **`/nlink_linktrack_nodeframe4`** ([nlink_parser::LinktrackNodeframe4]) + - **`/nlink_linktrack_nodeframe5`** ([nlink_parser::LinktrackNodeframe5]) + - **`/nlink_linktrack_nodeframe6`** ([nlink_parser::LinktrackNodeframe6]) 如果收到来自其他节点的数传数据，则 `/nlink_linktrack_nodeframe0` 话题将会发布消息 @@ -148,6 +153,36 @@ - **`/nlink_tofsense_cascade`** ([nlink_parser::TofsenseCascade]) - **`/nlink_tofsense_frame0`** ([nlink_parser::TofsenseFrame0]) +### TOFSense-M + +运行 + + roslaunch nlink_parser tofsensem.launch + +参数 + - **`port_name`** 设备串行端口名称，默认值: `/dev/ttyUSB0`. + - **`baud_rate`** 设备波特率，默认值: `921600`. + +发布的话题 + + - **`/nlink_tofsensem_frame0`** ([nlink_parser::TofsenseMFrame0]) + + +### IOT + +运行 + + roslaunch nlink_parser iot.launch + +参数 + - **`port_name`** 设备串行端口名称，默认值: `/dev/ttyUSB0`. + - **`baud_rate`** 设备波特率，默认值: `921600`. + +发布的话题 + + - **`/nlink_iot_frame0`** ([nlink_parser::IotFrame0]) + + ## How to Subscribe Our Topic diff --git a/extern/serial b/extern/serial new file mode 160000 index 0000000..69e0372 --- /dev/null +++ b/extern/serial @@ -0,0 +1 @@ +Subproject commit 69e0372cf0d3796e84ce9a09aff1d74496f68720 diff --git a/launch/iot.launch b/launch/iot.launch new file mode 100644 index 0000000..0f8c25e --- /dev/null +++ b/launch/iot.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/launch/tofsensem.launch b/launch/tofsensem.launch new file mode 100644 index 0000000..c17bad9 --- /dev/null +++ b/launch/tofsensem.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/msg/IotFrame0.msg b/msg/IotFrame0.msg new file mode 100644 index 0000000..d0d6a5a --- /dev/null +++ b/msg/IotFrame0.msg @@ -0,0 +1,4 @@ +uint32 uid +uint32 system_time +uint8 io_status +IotFrame0Node[] nodes diff --git a/msg/IotFrame0Node.msg b/msg/IotFrame0Node.msg new file mode 100644 index 0000000..73d14a7 --- /dev/null +++ b/msg/IotFrame0Node.msg @@ -0,0 +1,7 @@ +uint32 uid +float32 dis +float32 aoa_angle_horizontal +float32 aoa_angle_vertical +float32 fp_rssi +float32 rx_rssi +string user_data diff --git a/msg/LinktrackAnchorframe0.msg b/msg/LinktrackAnchorframe0.msg index 26d922a..43eda3c 100644 --- a/msg/LinktrackAnchorframe0.msg +++ b/msg/LinktrackAnchorframe0.msg @@ -4,4 +4,3 @@ uint32 local_time uint32 system_time float32 voltage LinktrackTag[] nodes - diff --git a/msg/LinktrackNode4Anchor.msg b/msg/LinktrackNode4Anchor.msg new file mode 100644 index 0000000..f8360da --- /dev/null +++ b/msg/LinktrackNode4Anchor.msg @@ -0,0 +1,2 @@ +uint8 id +float32 dis diff --git a/msg/LinktrackNode4Tag.msg b/msg/LinktrackNode4Tag.msg new file mode 100644 index 0000000..ea73e7a --- /dev/null +++ b/msg/LinktrackNode4Tag.msg @@ -0,0 +1,3 @@ +uint8 id +float32 voltage +LinktrackNode4Anchor[] anchors diff --git a/msg/LinktrackNode5.msg b/msg/LinktrackNode5.msg new file mode 100644 index 0000000..c475e33 --- /dev/null +++ b/msg/LinktrackNode5.msg @@ -0,0 +1,5 @@ +uint8 role +uint32 id +float32 dis +float32 fp_rssi +float32 rx_rssi diff --git a/msg/LinktrackNode6.msg b/msg/LinktrackNode6.msg new file mode 100644 index 0000000..f8ca369 --- /dev/null +++ b/msg/LinktrackNode6.msg @@ -0,0 +1,3 @@ +uint8 role +uint32 id +uint8[] data diff --git a/msg/LinktrackNodeframe4.msg b/msg/LinktrackNodeframe4.msg new file mode 100644 index 0000000..3cd48ac --- /dev/null +++ b/msg/LinktrackNodeframe4.msg @@ -0,0 +1,6 @@ +uint8 role +uint8 id +uint32 local_time +uint32 system_time +float32 voltage +LinktrackNode4Tag[] tags diff --git a/msg/LinktrackNodeframe5.msg b/msg/LinktrackNodeframe5.msg new file mode 100644 index 0000000..49f125a --- /dev/null +++ b/msg/LinktrackNodeframe5.msg @@ -0,0 +1,6 @@ +uint8 role +uint32 id +uint32 local_time +uint32 system_time +float32 voltage +LinktrackNode5[] nodes diff --git a/msg/LinktrackNodeframe6.msg b/msg/LinktrackNodeframe6.msg new file mode 100644 index 0000000..c6c57b3 --- /dev/null +++ b/msg/LinktrackNodeframe6.msg @@ -0,0 +1,3 @@ +uint8 role +uint32 id +LinktrackNode6[] nodes diff --git a/msg/LinktrackTagframe0.msg b/msg/LinktrackTagframe0.msg index defc02d..e7dbdb0 100644 --- a/msg/LinktrackTagframe0.msg +++ b/msg/LinktrackTagframe0.msg @@ -11,4 +11,3 @@ float32[3] angle_3d float32[4] quaternion float32[3] imu_gyro_3d float32[3] imu_acc_3d - diff --git a/msg/TofsenseFrame0.msg b/msg/TofsenseFrame0.msg index 885508a..5d1ef32 100644 --- a/msg/TofsenseFrame0.msg +++ b/msg/TofsenseFrame0.msg @@ -3,3 +3,4 @@ uint32 system_time float32 dis uint8 dis_status uint16 signal_strength +uint8 range_precision diff --git a/msg/TofsenseMFrame0.msg b/msg/TofsenseMFrame0.msg new file mode 100644 index 0000000..0ef9207 --- /dev/null +++ b/msg/TofsenseMFrame0.msg @@ -0,0 +1,4 @@ +uint8 id +uint32 system_time +uint8 pixel_count +TofsenseMFrame0Pixel[] pixels diff --git a/msg/TofsenseMFrame0Pixel.msg b/msg/TofsenseMFrame0Pixel.msg new file mode 100644 index 0000000..73c9122 --- /dev/null +++ b/msg/TofsenseMFrame0Pixel.msg @@ -0,0 +1,3 @@ +float32 dis +uint8 dis_status +uint16 signal_strength diff --git a/src/iot/init.cpp b/src/iot/init.cpp new file mode 100644 index 0000000..15f7656 --- /dev/null +++ b/src/iot/init.cpp @@ -0,0 +1,67 @@ + +#include "init.h" + +#include "nlink_protocol.h" +#include "nlink_unpack/nlink_iot_frame0.h" +#include "nlink_unpack/nlink_utils.h" +#include "nutils.h" +#include + +namespace { +class ProtocolFrame0 : public NLinkProtocolVLength { +public: + ProtocolFrame0() + : NLinkProtocolVLength( + true, g_iot_frame0.fixed_part_size, + {g_iot_frame0.frame_header, g_iot_frame0.function_mark}) {} + +protected: + void UnpackFrameData(const uint8_t *data) override { + g_iot_frame0.UnpackData(data, length()); + } +}; + +} // namespace + +namespace iot { +nlink_parser::IotFrame0 g_msg_iotframe0; + +Init::Init(NProtocolExtracter *protocol_extraction) { + InitFrame0(protocol_extraction); +} + +void Init::InitFrame0(NProtocolExtracter *protocol_extraction) { + static auto protocol = new ProtocolFrame0; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + ros::NodeHandle nh_; + auto topic = "nlink_iot_frame0"; + publishers_[protocol] = nh_.advertise(topic, 50); + TopicAdvertisedTip(topic); + } + + const auto &data = g_iot_frame0; + g_msg_iotframe0.uid = data.uid; + g_msg_iotframe0.system_time = data.system_time; + g_msg_iotframe0.io_status = *(const uint8_t *)&(data.io_status); + g_msg_iotframe0.nodes.resize(data.node_count); + for (int i = 0; i < data.node_count; ++i) { + auto &dst = g_msg_iotframe0.nodes[i]; + const auto &src = data.nodes[i]; + dst.uid = src.uid; + dst.dis = src.dis; + dst.aoa_angle_horizontal = src.aoa_angle_horizontal; + dst.aoa_angle_vertical = src.aoa_angle_vertical; + dst.fp_rssi = src.fp_rssi; + dst.rx_rssi = src.rx_rssi; + dst.user_data.clear(); + dst.user_data.insert(dst.user_data.begin(), src.user_data, + src.user_data + src.user_data_len); + } + + publishers_.at(protocol).publish(g_msg_iotframe0); + }); +} + +} // namespace iot diff --git a/src/iot/init.h b/src/iot/init.h new file mode 100644 index 0000000..b80d0bb --- /dev/null +++ b/src/iot/init.h @@ -0,0 +1,22 @@ +#ifndef IOT_INIT_H +#define IOT_INIT_H + +#include "protocol_extracter/nprotocol_extracter.h" +#include +#include +#include +#include + +namespace iot { +class Init { +public: + explicit Init(NProtocolExtracter *protocol_extraction); + +private: + void InitFrame0(NProtocolExtracter *protocol_extraction); + std::unordered_map publishers_; + ros::NodeHandle nh_; +}; + +} // namespace iot +#endif // IOT_INIT_H diff --git a/src/iot/main.cpp b/src/iot/main.cpp new file mode 100644 index 0000000..5f8ac65 --- /dev/null +++ b/src/iot/main.cpp @@ -0,0 +1,29 @@ +#include + +#include "init.h" +#include "init_serial.h" +#include "protocol_extracter/nprotocol_extracter.h" +#include +#include + +int main(int argc, char **argv) { + ros::init(argc, argv, "iot_parser"); + ros::NodeHandle nh; + serial::Serial serial; + initSerial(&serial); + + NProtocolExtracter extracter; + iot::Init init(&extracter); + ros::Rate loop_rate(1000); + while (ros::ok()) { + auto available_bytes = serial.available(); + std::string str_received; + if (available_bytes) { + serial.read(str_received, available_bytes); + extracter.AddNewData(str_received); + } + ros::spinOnce(); + loop_rate.sleep(); + } + return EXIT_SUCCESS; +} diff --git a/src/linktrack/init.cpp b/src/linktrack/init.cpp index 5c28f21..bbe259d 100644 --- a/src/linktrack/init.cpp +++ b/src/linktrack/init.cpp @@ -5,6 +5,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -12,9 +15,9 @@ #include "nutils.h" #include "protocols.h" -#define ARRAY_ASSIGN(DEST, SRC) \ - for (size_t _CNT = 0; _CNT < sizeof(SRC) / sizeof(SRC[0]); ++_CNT) { \ - DEST[_CNT] = SRC[_CNT]; \ +#define ARRAY_ASSIGN(DEST, SRC) \ + for (size_t _CNT = 0; _CNT < sizeof(SRC) / sizeof(SRC[0]); ++_CNT) { \ + DEST[_CNT] = SRC[_CNT]; \ } namespace linktrack { @@ -24,25 +27,32 @@ nlink_parser::LinktrackNodeframe0 g_msg_nodeframe0; nlink_parser::LinktrackNodeframe1 g_msg_nodeframe1; nlink_parser::LinktrackNodeframe2 g_msg_nodeframe2; nlink_parser::LinktrackNodeframe3 g_msg_nodeframe3; +nlink_parser::LinktrackNodeframe4 g_msg_nodeframe4; +nlink_parser::LinktrackNodeframe5 g_msg_nodeframe5; +nlink_parser::LinktrackNodeframe6 g_msg_nodeframe6; serial::Serial *serial_; Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) { serial_ = serial; - InitDataTransmission(); + initDataTransmission(); initAnchorFrame0(protocol_extraction); initTagFrame0(protocol_extraction); - InitNodeFrame0(protocol_extraction); + initNodeFrame0(protocol_extraction); initNodeFrame1(protocol_extraction); initNodeFrame2(protocol_extraction); initNodeFrame3(protocol_extraction); + initNodeFrame4(protocol_extraction); + initNodeFrame5(protocol_extraction); + initNodeFrame6(protocol_extraction); } static void DTCallback(const std_msgs::String::ConstPtr &msg) { - if (serial_) serial_->write(msg->data); + if (serial_) + serial_->write(msg->data); } -void Init::InitDataTransmission() { +void Init::initDataTransmission() { dt_sub_ = nh_.subscribe("nlink_linktrack_data_transmission", 1000, DTCallback); } @@ -110,7 +120,7 @@ void Init::initTagFrame0(NProtocolExtracter *protocol_extraction) { }); } -void Init::InitNodeFrame0(NProtocolExtracter *protocol_extraction) { +void Init::initNodeFrame0(NProtocolExtracter *protocol_extraction) { auto protocol = new NLT_ProtocolNodeFrame0; protocol_extraction->AddProtocol(protocol); protocol->SetHandleDataCallback([=] { @@ -251,4 +261,108 @@ void Init::initNodeFrame3(NProtocolExtracter *protocol_extraction) { publishers_.at(protocol).publish(msg_data); }); } -} // namespace linktrack + +void Init::initNodeFrame4(NProtocolExtracter *protocol_extraction) { + auto protocol = new NLT_ProtocolNodeFrame4; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + auto topic = "nlink_linktrack_nodeframe4"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nlt_nodeframe4.result; + auto &msg_data = g_msg_nodeframe4; + msg_data.role = data.role; + msg_data.id = data.id; + msg_data.local_time = data.local_time; + msg_data.system_time = data.system_time; + msg_data.voltage = data.voltage; + msg_data.tags.resize(data.tag_count); + for (int i = 0; i < data.tag_count; ++i) { + auto &msg_tag = msg_data.tags[i]; + auto tag = data.tags[i]; + msg_tag.id = tag->id; + msg_tag.voltage = tag->voltage; + msg_tag.anchors.resize(tag->anchor_count); + for (int j = 0; j < tag->anchor_count; ++j) { + auto &msg_anchor = msg_tag.anchors[j]; + auto anchor = tag->anchors[j]; + msg_anchor.id = anchor->id; + msg_anchor.dis = anchor->dis; + } + } + + publishers_.at(protocol).publish(msg_data); + }); +} + +void Init::initNodeFrame5(NProtocolExtracter *protocol_extraction) { + auto protocol = new NLT_ProtocolNodeFrame5; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + auto topic = "nlink_linktrack_nodeframe5"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nlt_nodeframe5.result; + auto &msg_data = g_msg_nodeframe5; + auto &msg_nodes = msg_data.nodes; + + msg_data.role = data.role; + msg_data.id = data.id; + msg_data.local_time = data.local_time; + msg_data.system_time = data.system_time; + msg_data.voltage = data.voltage; + + msg_nodes.resize(data.valid_node_count); + for (size_t i = 0; i < data.valid_node_count; ++i) { + auto &msg_node = msg_nodes[i]; + auto node = data.nodes[i]; + msg_node.id = node->id; + msg_node.role = node->role; + msg_node.dis = node->dis; + msg_node.fp_rssi = node->fp_rssi; + msg_node.rx_rssi = node->rx_rssi; + } + + publishers_.at(protocol).publish(msg_data); + }); +} + +void Init::initNodeFrame6(NProtocolExtracter *protocol_extraction) { + auto protocol = new NLT_ProtocolNodeFrame6; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + auto topic = "nlink_linktrack_nodeframe6"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + ; + } + const auto &data = g_nlt_nodeframe6.result; + auto &msg_data = g_msg_nodeframe6; + auto &msg_nodes = msg_data.nodes; + + msg_data.role = data.role; + msg_data.id = data.id; + + msg_nodes.resize(data.valid_node_count); + for (size_t i = 0; i < data.valid_node_count; ++i) { + auto &msg_node = msg_nodes[i]; + auto node = data.nodes[i]; + msg_node.id = node->id; + msg_node.role = node->role; + msg_node.data.resize(node->data_length); + memcpy(msg_node.data.data(), node->data, node->data_length); + } + + publishers_.at(protocol).publish(msg_data); + }); +} + +} // namespace linktrack diff --git a/src/linktrack/init.h b/src/linktrack/init.h index 0bf5e07..d68e3b6 100644 --- a/src/linktrack/init.h +++ b/src/linktrack/init.h @@ -12,23 +12,26 @@ class NProtocolExtracter; namespace linktrack { class Init { - public: +public: explicit Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial); - private: - void InitDataTransmission(); +private: + void initDataTransmission(); void initAnchorFrame0(NProtocolExtracter *protocol_extraction); void initTagFrame0(NProtocolExtracter *protocol_extraction); - void InitNodeFrame0(NProtocolExtracter *protocol_extraction); + void initNodeFrame0(NProtocolExtracter *protocol_extraction); void initNodeFrame1(NProtocolExtracter *protocol_extraction); void initNodeFrame2(NProtocolExtracter *protocol_extraction); void initNodeFrame3(NProtocolExtracter *protocol_extraction); + void initNodeFrame4(NProtocolExtracter *protocol_extraction); + void initNodeFrame5(NProtocolExtracter *protocol_extraction); + void initNodeFrame6(NProtocolExtracter *protocol_extraction); std::unordered_map publishers_; ros::NodeHandle nh_; ros::Subscriber dt_sub_; }; -} // namespace linktrack +} // namespace linktrack -#endif // LINKTRACKINIT_H +#endif // LINKTRACKINIT_H diff --git a/src/linktrack/main.cpp b/src/linktrack/main.cpp index 535fb48..899900c 100644 --- a/src/linktrack/main.cpp +++ b/src/linktrack/main.cpp @@ -4,6 +4,20 @@ #include "init_serial.h" #include "protocol_extracter/nprotocol_extracter.h" +#include +#include + +void printHexData(const std::string &data) { + if (!data.empty()) { + std::cout << "data received: "; + for (int i = 0; i < data.size(); ++i) { + std::cout << std::hex << std::setfill('0') << std::setw(2) + << int(uint8_t(data.at(i))) << " "; + } + std::cout << std::endl; + } +} + int main(int argc, char **argv) { ros::init(argc, argv, "linktrack_parser"); ros::NodeHandle nh; @@ -17,6 +31,7 @@ int main(int argc, char **argv) { std::string str_received; if (available_bytes) { serial.read(str_received, available_bytes); + // printHexData(str_received); protocol_extraction.AddNewData(str_received); } ros::spinOnce(); diff --git a/src/linktrack/main_rviz_converter.cpp b/src/linktrack/main_rviz_converter.cpp index 4150da8..b9cd924 100644 --- a/src/linktrack/main_rviz_converter.cpp +++ b/src/linktrack/main_rviz_converter.cpp @@ -20,7 +20,7 @@ struct PosePair { geometry_msgs::PoseStamped msg; inline void publish() { publisher.publish(msg); } }; -} // namespace +} // namespace void Anchorframe0Callback(const nlink_parser::LinktrackAnchorframe0 &msg) { static ros::Publisher publisher; @@ -44,7 +44,7 @@ void Anchorframe0Callback(const nlink_parser::LinktrackAnchorframe0 &msg) { } auto &msg_pose = poses[id].msg; ++msg_pose.header.seq; - msg_pose.header.stamp = ros::Time(); // ros::Time(msg.system_time / 1000.0) + msg_pose.header.stamp = ros::Time(); // ros::Time(msg.system_time / 1000.0) msg_pose.pose.position.x = static_cast(node.pos_3d[0]); msg_pose.pose.position.y = static_cast(node.pos_3d[1]); msg_pose.pose.position.z = static_cast(node.pos_3d[2]); @@ -74,7 +74,7 @@ void Nodeframe1Callback(const nlink_parser::LinktrackNodeframe1 &msg) { } auto &msg_pose = poses[id].msg; ++msg_pose.header.seq; - msg_pose.header.stamp = ros::Time(); // ros::Time(msg.system_time / 1000.0) + msg_pose.header.stamp = ros::Time(); // ros::Time(msg.system_time / 1000.0) msg_pose.pose.position.x = static_cast(node.pos_3d[0]); msg_pose.pose.position.y = static_cast(node.pos_3d[1]); msg_pose.pose.position.z = static_cast(node.pos_3d[2]); @@ -94,7 +94,7 @@ void Tagframe0Callback(const nlink_parser::LinktrackTagframe0 &msg) { } auto &msg_pose = pose->msg; ++msg_pose.header.seq; - msg_pose.header.stamp = ros::Time(); // ros::Time(msg.system_time / 1000.0) + msg_pose.header.stamp = ros::Time(); // ros::Time(msg.system_time / 1000.0) msg_pose.pose.orientation.w = static_cast(msg.quaternion[0]); msg_pose.pose.orientation.x = static_cast(msg.quaternion[1]); msg_pose.pose.orientation.y = static_cast(msg.quaternion[2]); @@ -117,7 +117,7 @@ void Nodeframe2Callback(const nlink_parser::LinktrackNodeframe2 &msg) { } auto &msg_pose = pose->msg; ++msg_pose.header.seq; - msg_pose.header.stamp = ros::Time(); // ros::Time(msg.system_time / 1000.0) + msg_pose.header.stamp = ros::Time(); // ros::Time(msg.system_time / 1000.0) msg_pose.pose.orientation.w = static_cast(msg.quaternion[0]); msg_pose.pose.orientation.x = static_cast(msg.quaternion[1]); msg_pose.pose.orientation.y = static_cast(msg.quaternion[2]); diff --git a/src/linktrack/protocols.cpp b/src/linktrack/protocols.cpp index 3601283..dd0b5ea 100644 --- a/src/linktrack/protocols.cpp +++ b/src/linktrack/protocols.cpp @@ -58,3 +58,30 @@ NLT_ProtocolNodeFrame3::NLT_ProtocolNodeFrame3() void NLT_ProtocolNodeFrame3::UnpackFrameData(const uint8_t *data) { g_nlt_nodeframe3.UnpackData(data, length()); } + +NLT_ProtocolNodeFrame4::NLT_ProtocolNodeFrame4() + : NLinkProtocolVLength( + true, g_nlt_nodeframe4.fixed_part_size, + {g_nlt_nodeframe4.frame_header, g_nlt_nodeframe4.function_mark}) {} + +void NLT_ProtocolNodeFrame4::UnpackFrameData(const uint8_t *data) { + g_nlt_nodeframe4.UnpackData(data, length()); +} + +NLT_ProtocolNodeFrame5::NLT_ProtocolNodeFrame5() + : NLinkProtocolVLength( + true, g_nlt_nodeframe5.fixed_part_size, + {g_nlt_nodeframe5.frame_header, g_nlt_nodeframe5.function_mark}) {} + +void NLT_ProtocolNodeFrame5::UnpackFrameData(const uint8_t *data) { + g_nlt_nodeframe5.UnpackData(data, length()); +} + +NLT_ProtocolNodeFrame6::NLT_ProtocolNodeFrame6() + : NLinkProtocolVLength( + true, g_nlt_nodeframe6.fixed_part_size, + {g_nlt_nodeframe6.frame_header, g_nlt_nodeframe6.function_mark}) {} + +void NLT_ProtocolNodeFrame6::UnpackFrameData(const uint8_t *data) { + g_nlt_nodeframe6.UnpackData(data, length()); +} diff --git a/src/linktrack/protocols.h b/src/linktrack/protocols.h index b48bfd1..0775caa 100644 --- a/src/linktrack/protocols.h +++ b/src/linktrack/protocols.h @@ -7,55 +7,82 @@ #include "nlink_unpack/nlink_linktrack_nodeframe1.h" #include "nlink_unpack/nlink_linktrack_nodeframe2.h" #include "nlink_unpack/nlink_linktrack_nodeframe3.h" +#include "nlink_unpack/nlink_linktrack_nodeframe4.h" +#include "nlink_unpack/nlink_linktrack_nodeframe5.h" +#include "nlink_unpack/nlink_linktrack_nodeframe6.h" #include "nlink_unpack/nlink_linktrack_tagframe0.h" class NLT_ProtocolAnchorFrame0 : public NLinkProtocol { - public: +public: NLT_ProtocolAnchorFrame0(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; bool Verify(const uint8_t *data) override; }; class NLT_ProtocolTagFrame0 : public NLinkProtocol { - public: +public: NLT_ProtocolTagFrame0(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; class NLT_ProtocolNodeFrame0 : public NLinkProtocolVLength { - public: +public: NLT_ProtocolNodeFrame0(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; class NLT_ProtocolNodeFrame1 : public NLinkProtocolVLength { - public: +public: NLT_ProtocolNodeFrame1(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; class NLT_ProtocolNodeFrame2 : public NLinkProtocolVLength { - public: +public: NLT_ProtocolNodeFrame2(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; class NLT_ProtocolNodeFrame3 : public NLinkProtocolVLength { - public: +public: NLT_ProtocolNodeFrame3(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; -#endif // LINKTRACK_PROTOCOLS_H +class NLT_ProtocolNodeFrame4 : public NLinkProtocolVLength { +public: + NLT_ProtocolNodeFrame4(); + +protected: + void UnpackFrameData(const uint8_t *data) override; +}; + +class NLT_ProtocolNodeFrame5 : public NLinkProtocolVLength { +public: + NLT_ProtocolNodeFrame5(); + +protected: + void UnpackFrameData(const uint8_t *data) override; +}; + +class NLT_ProtocolNodeFrame6 : public NLinkProtocolVLength { +public: + NLT_ProtocolNodeFrame6(); + +protected: + void UnpackFrameData(const uint8_t *data) override; +}; + +#endif // LINKTRACK_PROTOCOLS_H diff --git a/src/linktrack_aoa/init.cpp b/src/linktrack_aoa/init.cpp index 96733e4..6cf3bc1 100644 --- a/src/linktrack_aoa/init.cpp +++ b/src/linktrack_aoa/init.cpp @@ -9,10 +9,10 @@ #include "nutils.h" class NLTAoa_ProtocolNodeFrame0 : public NLinkProtocolVLength { - public: +public: NLTAoa_ProtocolNodeFrame0(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; @@ -26,7 +26,6 @@ void NLTAoa_ProtocolNodeFrame0::UnpackFrameData(const uint8_t *data) { } namespace linktrack_aoa { - nlink_parser::LinktrackNodeframe0 g_msg_nodeframe0; nlink_parser::LinktrackAoaNodeframe0 g_msg_aoa_nodeframe0; @@ -34,21 +33,22 @@ static serial::Serial *g_serial; Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) { g_serial = serial; - InitDataTransmission(); - InitNodeFrame0(protocol_extraction); + initDataTransmission(); + initNodeFrame0(protocol_extraction); InitAoaNodeFrame0(protocol_extraction); } static void DTCallback(const std_msgs::String::ConstPtr &msg) { - if (g_serial) g_serial->write(msg->data); + if (g_serial) + g_serial->write(msg->data); } -void Init::InitDataTransmission() { +void Init::initDataTransmission() { dt_sub_ = nh_.subscribe("nlink_linktrack_data_transmission", 1000, DTCallback); } -void Init::InitNodeFrame0(NProtocolExtracter *protocol_extraction) { +void Init::initNodeFrame0(NProtocolExtracter *protocol_extraction) { auto protocol = new NLT_ProtocolNodeFrame0; protocol_extraction->AddProtocol(protocol); protocol->SetHandleDataCallback([=] { @@ -116,4 +116,4 @@ void Init::InitAoaNodeFrame0(NProtocolExtracter *protocol_extraction) { }); } -} // namespace linktrack_aoa +} // namespace linktrack_aoa diff --git a/src/linktrack_aoa/init.h b/src/linktrack_aoa/init.h index c745d61..5db46fa 100644 --- a/src/linktrack_aoa/init.h +++ b/src/linktrack_aoa/init.h @@ -12,18 +12,18 @@ namespace linktrack_aoa { class Init { - public: +public: explicit Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial); - private: - void InitDataTransmission(); - void InitNodeFrame0(NProtocolExtracter *protocol_extraction); +private: + void initDataTransmission(); + void initNodeFrame0(NProtocolExtracter *protocol_extraction); void InitAoaNodeFrame0(NProtocolExtracter *protocol_extraction); std::unordered_map publishers_; ros::NodeHandle nh_; ros::Subscriber dt_sub_; }; -} // namespace linktrack_aoa +} // namespace linktrack_aoa -#endif // LINKTRACKAOAINIT_H +#endif // LINKTRACKAOAINIT_H diff --git a/src/tofsense/init.cpp b/src/tofsense/init.cpp index 01021d0..969fb9e 100644 --- a/src/tofsense/init.cpp +++ b/src/tofsense/init.cpp @@ -7,10 +7,10 @@ #include "nutils.h" class NTS_ProtocolFrame0 : public NLinkProtocol { - public: +public: NTS_ProtocolFrame0(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; @@ -69,6 +69,7 @@ void Init::InitFrame0(NProtocolExtracter *protocol_extraction) { g_msg_frame0.dis = data.dis; g_msg_frame0.dis_status = data.dis_status; g_msg_frame0.signal_strength = data.signal_strength; + g_msg_frame0.range_precision = data.range_precision; if (is_inquire_mode_) { frame0_map_[data.id] = g_msg_frame0; @@ -110,4 +111,4 @@ void Init::InitFrame0(NProtocolExtracter *protocol_extraction) { } } -} // namespace tofsense +} // namespace tofsense diff --git a/src/tofsense/init.h b/src/tofsense/init.h index c2483df..a7ba9ae 100644 --- a/src/tofsense/init.h +++ b/src/tofsense/init.h @@ -13,11 +13,11 @@ namespace tofsense { class Init { - public: +public: explicit Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial); - private: +private: void InitFrame0(NProtocolExtracter *protocol_extraction); std::unordered_map publishers_; @@ -35,5 +35,5 @@ class Init { uint8_t node_index_ = 0; }; -} // namespace tofsense -#endif // TOFSENSEINIT_H +} // namespace tofsense +#endif // TOFSENSEINIT_H diff --git a/src/tofsense/main.cpp b/src/tofsense/main.cpp index 5a22f75..df1c651 100644 --- a/src/tofsense/main.cpp +++ b/src/tofsense/main.cpp @@ -12,7 +12,7 @@ int main(int argc, char **argv) { NProtocolExtracter extracter; tofsense::Init init(&extracter, &serial); - + ros::Rate loop_rate(1000); while (ros::ok()) { auto available_bytes = serial.available(); std::string str_received; @@ -21,6 +21,7 @@ int main(int argc, char **argv) { extracter.AddNewData(str_received); } ros::spinOnce(); + loop_rate.sleep(); } return EXIT_SUCCESS; } diff --git a/src/tofsensem/init.cpp b/src/tofsensem/init.cpp new file mode 100644 index 0000000..c00273c --- /dev/null +++ b/src/tofsensem/init.cpp @@ -0,0 +1,63 @@ + +#include "init.h" + +#include "nlink_protocol.h" +#include "nlink_unpack/nlink_tofsensem_frame0.h" +#include "nlink_unpack/nlink_utils.h" +#include "nutils.h" + +namespace { +class ProtocolFrame0 : public NLinkProtocolVLength { +public: + ProtocolFrame0() + : NLinkProtocolVLength( + true, g_ntsm_frame0.fixed_part_size, + {g_ntsm_frame0.frame_header, g_ntsm_frame0.function_mark}) {} + +protected: + bool UpdateLength(const uint8_t *data, size_t available_bytes) override { + if (available_bytes < g_ntsm_frame0.fixed_part_size) + return false; + return set_length(tofm_frame0_size(data)); + } + void UnpackFrameData(const uint8_t *data) override { + g_ntsm_frame0.UnpackData(data, length()); + } +}; +} // namespace + +namespace tofsensem { +nlink_parser::TofsenseMFrame0 g_msg_tofmframe0; + +Init::Init(NProtocolExtracter *protocol_extraction) { + InitFrame0(protocol_extraction); +} + +void Init::InitFrame0(NProtocolExtracter *protocol_extraction) { + static auto protocol = new ProtocolFrame0; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + ros::NodeHandle nh_; + auto topic = "nlink_tofsensem_frame0"; + publishers_[protocol] = + nh_.advertise(topic, 50); + TopicAdvertisedTip(topic); + } + + const auto &data = g_ntsm_frame0; + g_msg_tofmframe0.id = data.id; + g_msg_tofmframe0.system_time = data.system_time; + g_msg_tofmframe0.pixels.resize(data.pixel_count); + for (int i = 0; i < data.pixel_count; ++i) { + const auto &src_pixel = data.pixels[i]; + auto &pixel = g_msg_tofmframe0.pixels[i]; + pixel.dis = src_pixel.dis; + pixel.dis_status = src_pixel.dis_status; + pixel.signal_strength = src_pixel.signal_strength; + } + publishers_.at(protocol).publish(g_msg_tofmframe0); + }); +} + +} // namespace tofsensem diff --git a/src/tofsensem/init.h b/src/tofsensem/init.h new file mode 100644 index 0000000..ab47578 --- /dev/null +++ b/src/tofsensem/init.h @@ -0,0 +1,21 @@ +#ifndef TOFSENSEMINIT_H +#define TOFSENSEMINIT_H + +#include "protocol_extracter/nprotocol_extracter.h" +#include +#include +#include + +namespace tofsensem { +class Init { +public: + explicit Init(NProtocolExtracter *protocol_extraction); + +private: + void InitFrame0(NProtocolExtracter *protocol_extraction); + std::unordered_map publishers_; + ros::NodeHandle nh_; +}; + +} // namespace tofsensem +#endif // TOFSENSEMINIT_H diff --git a/src/tofsensem/main.cpp b/src/tofsensem/main.cpp new file mode 100644 index 0000000..d973d35 --- /dev/null +++ b/src/tofsensem/main.cpp @@ -0,0 +1,33 @@ +#include + +#include "init.h" +#include "init_serial.h" +#include "nlink_unpack/nlink_tofsensem_frame0.h" +#include "nlink_unpack/nlink_utils.h" +#include "protocol_extracter/nprotocol_extracter.h" +#include +#include + +int main(int argc, char **argv) { + ros::init(argc, argv, "tofsensem_parser"); + ros::NodeHandle nh; + serial::Serial serial; + initSerial(&serial); + + NProtocolExtracter extracter; + tofsensem::Init init(&extracter); + ros::Rate loop_rate(1000); + while (ros::ok()) { + auto available_bytes = serial.available(); + std::string str_received; + if (available_bytes) { + serial.read(str_received, available_bytes); + extracter.AddNewData(str_received); + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + ros::spinOnce(); + loop_rate.sleep(); + } + return EXIT_SUCCESS; +} diff --git a/src/utils/init_serial.h b/src/utils/init_serial.h index 634ff5b..bcb43ce 100644 --- a/src/utils/init_serial.h +++ b/src/utils/init_serial.h @@ -4,4 +4,4 @@ void initSerial(serial::Serial *serial); -#endif // INITSERIAL_H +#endif // INITSERIAL_H diff --git a/src/utils/nlink_protocol.cpp b/src/utils/nlink_protocol.cpp index 634f006..f5ab6b5 100644 --- a/src/utils/nlink_protocol.cpp +++ b/src/utils/nlink_protocol.cpp @@ -19,6 +19,7 @@ bool NLinkProtocol::Verify(const uint8_t *data) { bool NLinkProtocolVLength::UpdateLength(const uint8_t *data, size_t available_bytes) { - if (available_bytes < 4) return false; + if (available_bytes < 4) + return false; return set_length(static_cast(data[2] | data[3] << 8)); -} \ No newline at end of file +} diff --git a/src/utils/nlink_protocol.h b/src/utils/nlink_protocol.h index 3d97db6..4c6ede0 100644 --- a/src/utils/nlink_protocol.h +++ b/src/utils/nlink_protocol.h @@ -5,28 +5,28 @@ #include "protocol_extracter/nprotocol_base.h" class NLinkProtocol : public NProtocolBase { - public: +public: using NProtocolBase::NProtocolBase; void SetHandleDataCallback(std::function handle) { HandleDataCallback_ = handle; } - protected: +protected: void HandleData(const uint8_t *data) final; virtual void UnpackFrameData(const uint8_t *data) = 0; bool Verify(const uint8_t *data) override; - private: +private: std::function HandleDataCallback_; }; class NLinkProtocolVLength : public NLinkProtocol { - public: +public: using NLinkProtocol::NLinkProtocol; - protected: +protected: bool UpdateLength(const uint8_t *data, size_t available_bytes) override; }; -#endif // NLINK_PROTOCOL_H +#endif // NLINK_PROTOCOL_H diff --git a/src/utils/nlink_unpack b/src/utils/nlink_unpack index ec6155a..1b79045 160000 --- a/src/utils/nlink_unpack +++ b/src/utils/nlink_unpack @@ -1 +1 @@ -Subproject commit ec6155ae6f3a9dc78bd939d8815cde91fd7fb52a +Subproject commit 1b790459c62bd7f15c18d98d9c87d13471f6db47 diff --git a/src/utils/nutils.cpp b/src/utils/nutils.cpp index bb443bb..d2163b0 100644 --- a/src/utils/nutils.cpp +++ b/src/utils/nutils.cpp @@ -3,8 +3,7 @@ #include void TopicAdvertisedTip(const char *topic) { - ROS_INFO( - "%s has been advertised,use 'rostopic " - "echo /%s' to view the data", - topic, topic); + ROS_INFO("%s has been advertised,use 'rostopic " + "echo /%s' to view the data", + topic, topic); } diff --git a/src/utils/nutils.h b/src/utils/nutils.h index dcaa80d..aff4f3f 100644 --- a/src/utils/nutils.h +++ b/src/utils/nutils.h @@ -3,4 +3,4 @@ void TopicAdvertisedTip(const char *topic); -#endif // NUTILS_H +#endif // NUTILS_H diff --git a/src/utils/protocol_extracter b/src/utils/protocol_extracter index 718b0a7..a19abb7 160000 --- a/src/utils/protocol_extracter +++ b/src/utils/protocol_extracter @@ -1 +1 @@ -Subproject commit 718b0a7146d7c2cdf0950bfb37cad511bb978415 +Subproject commit a19abb76df85fa3e94f245089b258a4f36a7c404 diff --git a/test/test_nlink_parser.cpp b/test/test_nlink_parser.cpp index 95bc989..3d55440 100644 --- a/test/test_nlink_parser.cpp +++ b/test/test_nlink_parser.cpp @@ -1,27 +1,34 @@ // gtest +#include "../src/iot/init.h" +#include "../src/linktrack/init.h" +#include "../src/linktrack_aoa/init.h" +#include "../src/tofsense/init.h" +#include "../src/tofsensem/init.h" #include +#include #include #include #include #include #include #include +#include +#include +#include #include #include - -#include "../src/linktrack/init.h" -#include "../src/linktrack_aoa/init.h" -#include "../src/tofsense/init.h" +#include +#include static const double kAbsError = 0.001; -#define COMPARE_ARRAY(SRC, DEST, kAbsError) \ - { \ - const auto &ACTUAL = SRC; \ - const auto &EXPECTED = std::vector DEST; \ - for (size_t _INDEX = 0; _INDEX < EXPECTED.size(); ++_INDEX) { \ - EXPECT_NEAR(EXPECTED.at(_INDEX), ACTUAL.at(_INDEX), kAbsError); \ - } \ +#define COMPARE_ARRAY(SRC, DEST, kAbsError) \ + { \ + const auto &ACTUAL = SRC; \ + const auto &EXPECTED = std::vector DEST; \ + for (size_t _INDEX = 0; _INDEX < EXPECTED.size(); ++_INDEX) { \ + EXPECT_NEAR(EXPECTED.at(_INDEX), ACTUAL.at(_INDEX), kAbsError); \ + } \ } namespace linktrack { @@ -31,89 +38,91 @@ extern nlink_parser::LinktrackNodeframe0 g_msg_nodeframe0; extern nlink_parser::LinktrackNodeframe1 g_msg_nodeframe1; extern nlink_parser::LinktrackNodeframe2 g_msg_nodeframe2; extern nlink_parser::LinktrackNodeframe3 g_msg_nodeframe3; -} // namespace linktrack +extern nlink_parser::LinktrackNodeframe4 g_msg_nodeframe4; +extern nlink_parser::LinktrackNodeframe5 g_msg_nodeframe5; +extern nlink_parser::LinktrackNodeframe6 g_msg_nodeframe6; +} // namespace linktrack TEST(NLinkParser, linktrack) { NProtocolExtracter protocol_extraction; linktrack::Init init(&protocol_extraction, nullptr); uint8_t data[1024]; { - auto string = - "55 00 00 02 4f 0b 00 73 09 00 f9 fe ff 6c 01 4e 01 ea 01 ed " - "01 00 00 00 " - "00 00 00 00 00 ff 0b 00 8e 09 00 4a fe ff c9 37 8a 34 06 ee " - "37 3f aa 02 " - "02 7e 09 00 67 09 00 a3 02 02 83 09 00 5f 09 00 a3 fb ff 3e " - "01 2a 01 12 " - "02 13 02 00 00 00 00 00 00 00 00 ff ab 91 ef ea 45 09 e1 18 " - "5a 33 b4 f2 " - "08 40 da ca c8 fd df f7 44 d7 3c 3a ff a2 ff f6 12 0c 05 4c " - "2b 7b 7d 6b " - "49 41 1d 48 44 fd bc 96 21 43 16 45 97 ef c6 71 e5 ff f4 83 " - "77 fa 32 d3 " - "01 19 29 bd fd fb 60 ff 00 04 1a 12 38 ba 26 b7 9b 6d 7d 3a " - "ff 2f 5f da " - "82 64 40 42 86 fe df 72 08 1a 28 0b fb f7 77 f2 72 76 c4 38 " - "79 2a 9d ff " - "24 24 20 25 4e b7 9f 99 4d 70 56 80 d3 9e e2 7d 12 3a 85 c6 " - "8f bd c6 9f " - "81 8f ff f3 0c f4 c5 ce c9 91 51 d1 65 5f 30 3f 05 91 0c 86 " - "b0 07 7e 39 " - "d1 68 76 46 d3 ff 9f 6b 11 08 ce 06 b4 ac 59 e1 c4 33 ca 80 " - "1f be 13 bc " - "6d e2 32 8d ea 75 a5 b1 ff 06 95 4d f7 12 d6 61 45 8c 89 84 " - "36 64 96 e0 " - "f0 02 a2 84 6c 37 fb fa ac 18 a4 ff 79 6a 78 76 78 60 ba 00 " - "fb 7b d6 fb " - "f8 00 8a f9 b5 63 cf eb d0 45 56 43 23 e9 ff bb 73 25 0a 0b " - "cf b9 df cd " - "aa 62 1f 54 eb 1d f7 b8 c0 02 d8 8a c3 af 34 97 03 ff e2 88 " - "b6 c3 39 fa " - "89 5d 40 00 62 8b ce 4b c0 20 28 11 fd d6 ce bd 3c 60 7b 72 " - "ff 42 e4 ed " - "90 61 70 40 28 df c1 be 11 00 15 55 cb 95 56 2d 50 a3 5d 28 " - "32 ee bb ff " - "01 91 1d 67 35 f5 f8 bf 3c 10 01 1c 6d 1b f5 5e 89 1f 28 23 " - "60 31 2f 75 " - "44 02 ff 51 7b c7 af af f2 d0 0e ca bb 61 31 f6 ac e0 69 10 " - "34 b3 73 7f " - "30 14 18 01 7a ff 76 ed 42 37 66 0a 2b 55 7f f5 04 43 f4 40 " - "4e 3f 52 ee " - "c4 4f 09 1e b9 8d f6 3d ff 51 f2 20 d6 4b d2 7d 87 0c a8 15 " - "99 4b ee 2a " - "42 41 69 68 75 51 f3 bf 5d 4b 01 ff 8e 9e 1e 92 a8 a1 57 04 " - "f4 c4 3f eb " - "43 18 ac 03 58 0d 5d d4 51 c8 81 64 7e 6d ff 71 c1 18 2b 4f " - "b7 ef ec 7c " - "46 2a a4 8f 6c 94 cb 63 9e 6c 22 74 de 1f fc a3 62 ff 1f 01 " - "f9 61 71 50 " - "57 1a 83 b2 64 1e 3e 1c 85 22 2b 99 56 6d f9 bb b2 c1 a1 22 " - "ff ef 5c af " - "f7 4c f0 68 6d 4c 42 37 24 03 45 28 fd 1b a6 e2 04 7c a4 7c " - "e5 33 9b ff " - "6b 1a 03 14 77 5f 23 3d a8 6c 44 80 17 64 f1 a2 10 06 38 24 " - "06 d3 3c 3c " - "0c 68 ff 98 d2 ab 63 ca d0 f2 c6 2a 7e a3 57 dd 21 35 b1 60 " - "14 ee de 36 " - "15 d8 2b 08 b7 ff 17 6c 6b 21 ad da dc b3 31 dc ea 0c a6 cb " - "71 ff 4c e9 " - "8e 08 66 58 e2 3e 6d e6 ff 00 d8 00 1c db db d1 05 ac ee 06 " - "85 f6 de da " - "00 51 0c 2e ca ed 8b c7 16 7b 06 ff 17 ef f9 ef 7c 1e e0 d3 " - "3f 6d 6c 6f " - "eb 27 b9 65 53 c6 1f 2a 3e 11 3a 40 8f b1 ff 3e a2 12 20 c3 " - "d5 b7 68 be " - "18 14 5c a4 a1 7f bb 9f fa c4 54 80 38 65 4e ff 51 ff 9a 1b " - "d7 3f e7 96 " - "20 3b 1c 08 82 9b 3f e2 04 ae 16 80 b7 ca e3 07 83 60 50 f5 " - "ff 8a fd c1 " - "24 2d 84 35 b7 fe 0c 6d 9b d6 e8 8c ee 79 21 ef e1 10 82 90 " - "b7 92 c2 ff " - "57 04 da 00 0e bf 4e e7 c2 70 aa cc 95 85 ff 6b 30 03 83 41 " - "8b df ad e3 " - "8a 40 ff 02 ed cb 6f 7e 13 20 99 f1 f1 87 2a 21 82 00 00 0c " - "c7 e9 7a 83 " - "13 00 7d 00 00 00 03 ee"; + auto string = "55 00 00 02 4f 0b 00 73 09 00 f9 fe ff 6c 01 4e 01 ea 01 ed " + "01 00 00 00 " + "00 00 00 00 00 ff 0b 00 8e 09 00 4a fe ff c9 37 8a 34 06 ee " + "37 3f aa 02 " + "02 7e 09 00 67 09 00 a3 02 02 83 09 00 5f 09 00 a3 fb ff 3e " + "01 2a 01 12 " + "02 13 02 00 00 00 00 00 00 00 00 ff ab 91 ef ea 45 09 e1 18 " + "5a 33 b4 f2 " + "08 40 da ca c8 fd df f7 44 d7 3c 3a ff a2 ff f6 12 0c 05 4c " + "2b 7b 7d 6b " + "49 41 1d 48 44 fd bc 96 21 43 16 45 97 ef c6 71 e5 ff f4 83 " + "77 fa 32 d3 " + "01 19 29 bd fd fb 60 ff 00 04 1a 12 38 ba 26 b7 9b 6d 7d 3a " + "ff 2f 5f da " + "82 64 40 42 86 fe df 72 08 1a 28 0b fb f7 77 f2 72 76 c4 38 " + "79 2a 9d ff " + "24 24 20 25 4e b7 9f 99 4d 70 56 80 d3 9e e2 7d 12 3a 85 c6 " + "8f bd c6 9f " + "81 8f ff f3 0c f4 c5 ce c9 91 51 d1 65 5f 30 3f 05 91 0c 86 " + "b0 07 7e 39 " + "d1 68 76 46 d3 ff 9f 6b 11 08 ce 06 b4 ac 59 e1 c4 33 ca 80 " + "1f be 13 bc " + "6d e2 32 8d ea 75 a5 b1 ff 06 95 4d f7 12 d6 61 45 8c 89 84 " + "36 64 96 e0 " + "f0 02 a2 84 6c 37 fb fa ac 18 a4 ff 79 6a 78 76 78 60 ba 00 " + "fb 7b d6 fb " + "f8 00 8a f9 b5 63 cf eb d0 45 56 43 23 e9 ff bb 73 25 0a 0b " + "cf b9 df cd " + "aa 62 1f 54 eb 1d f7 b8 c0 02 d8 8a c3 af 34 97 03 ff e2 88 " + "b6 c3 39 fa " + "89 5d 40 00 62 8b ce 4b c0 20 28 11 fd d6 ce bd 3c 60 7b 72 " + "ff 42 e4 ed " + "90 61 70 40 28 df c1 be 11 00 15 55 cb 95 56 2d 50 a3 5d 28 " + "32 ee bb ff " + "01 91 1d 67 35 f5 f8 bf 3c 10 01 1c 6d 1b f5 5e 89 1f 28 23 " + "60 31 2f 75 " + "44 02 ff 51 7b c7 af af f2 d0 0e ca bb 61 31 f6 ac e0 69 10 " + "34 b3 73 7f " + "30 14 18 01 7a ff 76 ed 42 37 66 0a 2b 55 7f f5 04 43 f4 40 " + "4e 3f 52 ee " + "c4 4f 09 1e b9 8d f6 3d ff 51 f2 20 d6 4b d2 7d 87 0c a8 15 " + "99 4b ee 2a " + "42 41 69 68 75 51 f3 bf 5d 4b 01 ff 8e 9e 1e 92 a8 a1 57 04 " + "f4 c4 3f eb " + "43 18 ac 03 58 0d 5d d4 51 c8 81 64 7e 6d ff 71 c1 18 2b 4f " + "b7 ef ec 7c " + "46 2a a4 8f 6c 94 cb 63 9e 6c 22 74 de 1f fc a3 62 ff 1f 01 " + "f9 61 71 50 " + "57 1a 83 b2 64 1e 3e 1c 85 22 2b 99 56 6d f9 bb b2 c1 a1 22 " + "ff ef 5c af " + "f7 4c f0 68 6d 4c 42 37 24 03 45 28 fd 1b a6 e2 04 7c a4 7c " + "e5 33 9b ff " + "6b 1a 03 14 77 5f 23 3d a8 6c 44 80 17 64 f1 a2 10 06 38 24 " + "06 d3 3c 3c " + "0c 68 ff 98 d2 ab 63 ca d0 f2 c6 2a 7e a3 57 dd 21 35 b1 60 " + "14 ee de 36 " + "15 d8 2b 08 b7 ff 17 6c 6b 21 ad da dc b3 31 dc ea 0c a6 cb " + "71 ff 4c e9 " + "8e 08 66 58 e2 3e 6d e6 ff 00 d8 00 1c db db d1 05 ac ee 06 " + "85 f6 de da " + "00 51 0c 2e ca ed 8b c7 16 7b 06 ff 17 ef f9 ef 7c 1e e0 d3 " + "3f 6d 6c 6f " + "eb 27 b9 65 53 c6 1f 2a 3e 11 3a 40 8f b1 ff 3e a2 12 20 c3 " + "d5 b7 68 be " + "18 14 5c a4 a1 7f bb 9f fa c4 54 80 38 65 4e ff 51 ff 9a 1b " + "d7 3f e7 96 " + "20 3b 1c 08 82 9b 3f e2 04 ae 16 80 b7 ca e3 07 83 60 50 f5 " + "ff 8a fd c1 " + "24 2d 84 35 b7 fe 0c 6d 9b d6 e8 8c ee 79 21 ef e1 10 82 90 " + "b7 92 c2 ff " + "57 04 da 00 0e bf 4e e7 c2 70 aa cc 95 85 ff 6b 30 03 83 41 " + "8b df ad e3 " + "8a 40 ff 02 ed cb 6f 7e 13 20 99 f1 f1 87 2a 21 82 00 00 0c " + "c7 e9 7a 83 " + "13 00 7d 00 00 00 03 ee"; auto data_length = NLink_StringToHex(string, data); protocol_extraction.AddNewData(data, data_length); @@ -249,6 +258,98 @@ TEST(NLinkParser, linktrack) { EXPECT_NEAR(msgData.nodes[3].fp_rssi, -92, kAbsError); EXPECT_NEAR(msgData.nodes[3].rx_rssi, -80, kAbsError); } + { + auto string = + "55 06 40 00 01 03 8e 9d 01 00 8e 9d 01 00 ff ff 03 01 68 11 02 02 00 " + "00 58 04 00 8d 09 00 01 d4 06 00 02 f6 07 00 03 ad 06 00 05 00 00 46 " + "04 00 b8 0a 00 01 93 05 00 02 68 09 00 03 43 05 00 60 55 06 40 00 01 " + "03 c0 9d 01 00 c0 9d 01 00 ff ff 03 01 68 11 02 02 00 00 59 04 00 7d " + "09 00 01 d6 06 00 02 1f 08 00 03 0d 07 00 05 00 00 4d 04 00 d2 0a 00 " + "01 80 05 00 02 64 09 00 03 32 05 00 3b 55 06 40 00 01 03 f2 9d 01 00 " + "f2 9d 01 00 ff ff 03 01 58 11 02 02 00 00 59 04 00 6b 09 00 01 aa 06 " + "00 02 e6 07 00 03 ed 06 00 05 00 00 4d 04 00 ab 0a 00 01 82 05 00 02 " + "3d 09 00 03 45 05 00 bd 55 06 40 00 01 03 24 9e 01 00 24 9e 01 00 ff " + "ff 03 01 58 11 02 02 00 00 59 04 00 76 09 00 01 c1 06 00 02 3b 08 00 " + "03 e2 06 00 05 00 00 49 04 00 8d 0a 00 01 95 05 00 02 4a 09 00 03 32 " + "05 00 7b"; + + auto data_length = NLink_StringToHex(string, data); + protocol_extraction.AddNewData(data, data_length); + auto &msg_data = linktrack::g_msg_nodeframe4; + EXPECT_EQ(msg_data.local_time, 106020); + EXPECT_EQ(msg_data.system_time, 106020); + EXPECT_NEAR(msg_data.voltage, 4.44f, kAbsError); + { + const auto &tag = msg_data.tags[0]; + EXPECT_EQ(tag.id, 2); + EXPECT_NEAR(tag.voltage, 4.45f, kAbsError); + EXPECT_EQ(tag.anchors.size(), 4); + std::vector> datas = { + {0, 2.422f}, + {1, 1.729f}, + {2, 2.107f}, + {3, 1.762f}, + }; + for (int i = 0; i < tag.anchors.size(); ++i) { + const auto &anchor = tag.anchors[i]; + EXPECT_EQ(anchor.id, datas[i].first); + EXPECT_NEAR(anchor.dis, datas[i].second, kAbsError); + } + } + { + const auto &tag = msg_data.tags[1]; + EXPECT_EQ(tag.id, 5); + EXPECT_NEAR(tag.voltage, 3.65f, kAbsError); + EXPECT_EQ(tag.anchors.size(), 4); + std::vector> datas = { + {0, 2.701f}, + {1, 1.429f}, + {2, 2.378f}, + {3, 1.33f}, + }; + for (int i = 0; i < tag.anchors.size(); ++i) { + const auto &anchor = tag.anchors[i]; + EXPECT_EQ(anchor.id, datas[i].first); + EXPECT_NEAR(anchor.dis, datas[i].second, kAbsError); + } + } + } + { + auto string = + "55 08 41 00 02 01 00 00 00 f8 11 07 00 6f d0 6e 00 00 00 01 02 5a 13 " + "04 01 00 00 00 00 22 0b 00 b5 9f01 01 00 00 00 a3 17 00 b6 a0 01 02 " + "00 00 00 88 1c 00 aa 9f 01 03 00 00 00 e6 14 00 b8 a0 ac"; + auto data_length = NLink_StringToHex(string, data); + protocol_extraction.AddNewData(data, data_length); + auto &msgData = linktrack::g_msg_nodeframe5; + EXPECT_EQ(msgData.local_time, 463352); + EXPECT_EQ(msgData.system_time, 7262319); + EXPECT_NEAR(msgData.voltage, 4.954f, kAbsError); + EXPECT_NEAR(msgData.nodes[0].dis, 2.85f, kAbsError); + EXPECT_NEAR(msgData.nodes[0].fp_rssi, -90.5f, kAbsError); + EXPECT_NEAR(msgData.nodes[0].rx_rssi, -79.5f, kAbsError); + EXPECT_NEAR(msgData.nodes[1].dis, 6.051f, kAbsError); + EXPECT_NEAR(msgData.nodes[1].fp_rssi, -91, kAbsError); + EXPECT_NEAR(msgData.nodes[1].rx_rssi, -80, kAbsError); + EXPECT_NEAR(msgData.nodes[2].dis, 7.304f, kAbsError); + EXPECT_NEAR(msgData.nodes[2].fp_rssi, -85, kAbsError); + EXPECT_NEAR(msgData.nodes[2].rx_rssi, -79.5f, kAbsError); + EXPECT_NEAR(msgData.nodes[3].dis, 5.35f, kAbsError); + EXPECT_NEAR(msgData.nodes[3].fp_rssi, -92, kAbsError); + EXPECT_NEAR(msgData.nodes[3].rx_rssi, -80, kAbsError); + } + + { + auto string = "55 09 4b 00 01 00 00 00 00 d1 2c c3 88 02 02 00 00 00 00 09 " + "00 11 22 33 44 55 66 77 88 99 02 02 00 00 00 25 00 11 12 23 " + "22 32 44 34 54 55 65 67 76 67 87 77 99 aa a2 13 45 57 65 56 " + "56 56 56 57 78 43 33 34 44 44 44 44 46 76 1d"; + auto data_length = NLink_StringToHex(string, data); + protocol_extraction.AddNewData(data, data_length); + auto &msgData = linktrack::g_msg_nodeframe6; + EXPECT_EQ(msgData.nodes[0].data.size(), 9); + EXPECT_EQ(msgData.nodes[1].data.size(), 37); + } } namespace tofsense { @@ -270,6 +371,68 @@ TEST(NLinkParser, tofsense) { EXPECT_NEAR(msg.dis, 0.64, kAbsError); EXPECT_EQ(msg.dis_status, 0); EXPECT_EQ(msg.signal_strength, 8); + EXPECT_EQ(msg.range_precision, 255); +} + +namespace tofsensem { +extern nlink_parser::TofsenseMFrame0 g_msg_tofmframe0; +} + +TEST(NLinkParser, tofsensem) { + NProtocolExtracter protocol_extraction; + tofsensem::Init init(&protocol_extraction); + + uint8_t data[1024]; + auto string = + "57 01 ff 00 13 c9 00 00 40 40 36 11 05 2d 00 30 80 13 05 16 00 c0 62 14 " + "ff 09 00 98 21 19 ff 04 00 28 76 07 ff 08 00 38 20 07 ff 06 00 00 59 06 " + "ff 08 00 c8 c0 12 ff 1f 00 f8 3b 12 05 27 00 a8 e3 14 05 1c 00 58 08 18 " + "05 15 00 d8 34 1b 05 09 00 58 43 23 ff 0a 00 18 18 25 04 04 00 e0 04 07 " + "ff 06 00 90 41 06 ff 07 00 48 7c 13 05 25 00 98 33 16 05 15 00 78 cd 19 " + "05 0c 00 f0 9b 1e ff 09 00 e0 c1 26 ff 0b 00 80 d7 25 05 08 00 a0 31 24 " + "05 09 00 38 78 22 05 06 00 98 bc 14 05 1b 00 48 5e 18 05 13 00 c8 84 1c " + "05 08 00 08 68 26 05 07 00 58 b4 25 05 0b 00 80 dd 24 05 08 00 90 0a 24 " + "05 0d 00 e8 56 07 05 16 00 f0 d1 15 05 1b 00 18 dd 19 05 0e 00 e8 37 21 " + "04 07 00 d0 23 25 05 08 00 b0 52 25 05 08 00 70 39 24 05 0e 00 80 e9 22 " + "05 0c 00 d0 5a 07 05 31 00 c8 a8 16 05 0d 00 18 54 1b 05 0c 00 60 18 23 " + "0a 0a 00 f0 77 24 05 08 00 10 cc 23 05 0c 00 98 62 23 05 0f 00 f0 06 22 " + "05 11 00 a8 37 07 05 46 00 b8 69 1a 05 0b 00 30 c1 1d 0a 0d 00 68 64 24 " + "ff 0d 00 b0 58 24 05 08 00 28 4b 23 05 0d 00 b0 e7 21 05 12 00 38 01 21 " + "05 0f 00 c8 08 07 05 44 00 18 dd 19 09 0a 00 38 6c 24 05 08 00 58 c0 23 " + "05 0b 00 90 10 23 05 0c 00 a0 c0 21 05 0c 00 b8 3f 21 05 0c 00 a0 49 20 " + "05 10 00 e0 04 07 05 32 00 ff ff ff ff ff ff 67"; + std::vector vals{ + 0, 51475, 1128, 5, 45, 1278, 5, 22, 1336, 255, 9, 1647, + 255, 4, 489, 255, 8, 467, 255, 6, 416, 255, 8, 1229, + 255, 31, 1195, 5, 39, 1369, 5, 28, 1575, 5, 21, 1783, + 5, 9, 2311, 255, 10, 2431, 4, 4, 460, 255, 6, 410, + 255, 7, 1277, 5, 37, 1455, 5, 21, 1691, 5, 12, 2006, + 255, 9, 2540, 255, 11, 2480, 5, 8, 2372, 5, 9, 2259, + 5, 6, 1359, 5, 27, 1597, 5, 19, 1869, 5, 8, 2517, + 5, 7, 2471, 5, 11, 2416, 5, 8, 2362, 5, 13, 481, + 5, 22, 1430, 5, 27, 1695, 5, 14, 2177, 4, 7, 2434, + 5, 8, 2446, 5, 8, 2374, 5, 14, 2288, 5, 12, 482, + 5, 49, 1485, 5, 13, 1791, 5, 12, 2300, 10, 10, 2390, + 5, 8, 2346, 5, 12, 2319, 5, 15, 2230, 5, 17, 473, + 5, 70, 1731, 5, 11, 1950, 10, 13, 2385, 255, 13, 2382, + 5, 8, 2313, 5, 13, 2222, 5, 18, 2163, 5, 15, 461, + 5, 68, 1695, 9, 10, 2387, 5, 8, 2343, 5, 11, 2298, + 5, 12, 2212, 5, 12, 2179, 5, 12, 2116, 5, 16, 460, + 5, 50}; + int val_cnt = -1; + auto next_val = [&vals, &val_cnt]() { return vals[++val_cnt]; }; + auto data_length = NLink_StringToHex(string, data); + protocol_extraction.AddNewData(data, data_length); + + auto &msg = tofsensem::g_msg_tofmframe0; + EXPECT_EQ(msg.id, next_val()); + EXPECT_EQ(msg.system_time, next_val()); + for (int i = 0; i < msg.pixels.size(); ++i) { + const auto &pixel = msg.pixels.at(i); + EXPECT_NEAR(pixel.dis, next_val(), kAbsError); + EXPECT_NEAR(pixel.dis_status, next_val(), kAbsError); + EXPECT_NEAR(pixel.signal_strength, next_val(), kAbsError); + } } namespace linktrack_aoa { @@ -310,6 +473,34 @@ TEST(nlink_parser, linktrack_aoa) { EXPECT_NEAR(msg.nodes[3].angle, -49.67f, kAbsError); } +namespace iot { +extern nlink_parser::IotFrame0 g_msg_iotframe0; +} + +TEST(nlink_parser, iot) { + NProtocolExtracter protocol_extraction; + iot::Init init(&protocol_extraction); + + uint8_t data[128]; + auto string = "6a 00 1d 00 00 4f 00 30 fa db 0b 00 00 01 00 5b 00 2f 12 01 " + "00 a5 f0 3c 03 8f a5 00 8c"; + auto data_length = NLink_StringToHex(string, data); + protocol_extraction.AddNewData(data, data_length); + + auto &msg = iot::g_msg_iotframe0; + EXPECT_EQ(msg.uid, 0x30004f00); + EXPECT_EQ(msg.system_time, 0x000bdbfa); + EXPECT_EQ(msg.io_status, 0); + EXPECT_EQ(msg.nodes.size(), 1); + EXPECT_EQ(msg.nodes[0].uid, 0x2f005b00); + EXPECT_NEAR(msg.nodes[0].dis, 0.274f, kAbsError); + EXPECT_NEAR(msg.nodes[0].aoa_angle_horizontal, -39.31f, kAbsError); + EXPECT_NEAR(msg.nodes[0].aoa_angle_vertical, 8.28f, kAbsError); + EXPECT_NEAR(msg.nodes[0].fp_rssi, -71.5f, kAbsError); + EXPECT_NEAR(msg.nodes[0].rx_rssi, -82.5f, kAbsError); + EXPECT_EQ(msg.nodes[0].user_data.empty(), true); +} + // Run all the tests that were declared with TEST() int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv);