From 1e91d7fc7ae9978896d6cbd43139ab2a9bc223ca Mon Sep 17 00:00:00 2001 From: samuelhsu Date: Wed, 7 Apr 2021 21:19:22 +0800 Subject: [PATCH 01/15] - format files --- src/linktrack/init.cpp | 499 ++++++++++++++------------ src/linktrack/init.h | 40 ++- src/linktrack/main.cpp | 9 +- src/linktrack/main_rviz_converter.cpp | 63 ++-- src/linktrack/protocols.cpp | 45 ++- src/linktrack/protocols.h | 44 ++- src/linktrack_aoa/init.cpp | 213 ++++++----- src/linktrack_aoa/init.h | 32 +- src/linktrack_aoa/main.cpp | 9 +- src/tofsense/init.cpp | 181 +++++----- src/tofsense/init.h | 40 ++- src/tofsense/main.cpp | 9 +- src/utils/init_serial.cpp | 17 +- src/utils/init_serial.h | 2 +- src/utils/nlink_protocol.cpp | 12 +- src/utils/nlink_protocol.h | 21 +- src/utils/nlink_unpack | 2 +- src/utils/nutils.cpp | 10 +- src/utils/nutils.h | 2 +- src/utils/protocol_extracter | 2 +- test/test_nlink_parser.cpp | 205 ++++++----- 21 files changed, 795 insertions(+), 662 deletions(-) diff --git a/src/linktrack/init.cpp b/src/linktrack/init.cpp index 5c28f21..eb49435 100644 --- a/src/linktrack/init.cpp +++ b/src/linktrack/init.cpp @@ -12,243 +12,266 @@ #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 { -nlink_parser::LinktrackAnchorframe0 g_msg_anchorframe0; -nlink_parser::LinktrackTagframe0 g_msg_tagframe0; -nlink_parser::LinktrackNodeframe0 g_msg_nodeframe0; -nlink_parser::LinktrackNodeframe1 g_msg_nodeframe1; -nlink_parser::LinktrackNodeframe2 g_msg_nodeframe2; -nlink_parser::LinktrackNodeframe3 g_msg_nodeframe3; - -serial::Serial *serial_; - -Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) { - serial_ = serial; - InitDataTransmission(); - initAnchorFrame0(protocol_extraction); - initTagFrame0(protocol_extraction); - InitNodeFrame0(protocol_extraction); - initNodeFrame1(protocol_extraction); - initNodeFrame2(protocol_extraction); - initNodeFrame3(protocol_extraction); -} - -static void DTCallback(const std_msgs::String::ConstPtr &msg) { - if (serial_) serial_->write(msg->data); -} - -void Init::InitDataTransmission() { - dt_sub_ = - nh_.subscribe("nlink_linktrack_data_transmission", 1000, DTCallback); -} - -void Init::initAnchorFrame0(NProtocolExtracter *protocol_extraction) { - auto protocol = new NLT_ProtocolAnchorFrame0; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) { - auto topic = "nlink_linktrack_anchorframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - auto data = nlt_anchorframe0_.result; - g_msg_anchorframe0.role = data.role; - g_msg_anchorframe0.id = data.id; - g_msg_anchorframe0.voltage = data.voltage; - g_msg_anchorframe0.local_time = data.local_time; - g_msg_anchorframe0.system_time = data.system_time; - auto &msg_nodes = g_msg_anchorframe0.nodes; - msg_nodes.clear(); - decltype(g_msg_anchorframe0.nodes)::value_type msg_node; - for (size_t i = 0, icount = data.valid_node_count; i < icount; ++i) { - auto node = data.nodes[i]; - msg_node.role = node->role; - msg_node.id = node->id; - ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d) - ARRAY_ASSIGN(msg_node.dis_arr, node->dis_arr) - msg_nodes.push_back(msg_node); - } - publishers_.at(protocol).publish(g_msg_anchorframe0); - }); -} - -void Init::initTagFrame0(NProtocolExtracter *protocol_extraction) { - auto protocol = new NLT_ProtocolTagFrame0; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) { - auto topic = "nlink_linktrack_tagframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - - const auto &data = g_nlt_tagframe0.result; - auto &msg_data = g_msg_tagframe0; - - 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; - ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d) - ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d) - ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d) - ARRAY_ASSIGN(msg_data.dis_arr, data.dis_arr) - ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d) - ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d) - ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d) - ARRAY_ASSIGN(msg_data.quaternion, data.quaternion) - - publishers_.at(protocol).publish(msg_data); - }); -} - -void Init::InitNodeFrame0(NProtocolExtracter *protocol_extraction) { - auto protocol = new NLT_ProtocolNodeFrame0; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) { - auto topic = "nlink_linktrack_nodeframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - ; - } - const auto &data = g_nlt_nodeframe0.result; - auto &msg_data = g_msg_nodeframe0; - 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); - }); -} - -void Init::initNodeFrame1(NProtocolExtracter *protocol_extraction) { - auto protocol = new NLT_ProtocolNodeFrame1; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) { - auto topic = "nlink_linktrack_nodeframe1"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - const auto &data = g_nlt_nodeframe1.result; - auto &msg_data = g_msg_nodeframe1; - 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; - ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d) - } - - publishers_.at(protocol).publish(msg_data); - }); -} - -void Init::initNodeFrame2(NProtocolExtracter *protocol_extraction) { - auto protocol = new NLT_ProtocolNodeFrame2; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) { - auto topic = "nlink_linktrack_nodeframe2"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - const auto &data = g_nlt_nodeframe2.result; - auto &msg_data = g_msg_nodeframe2; - 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; - ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d) - ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d) - ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d) - ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d) - ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d) - ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d) - ARRAY_ASSIGN(msg_data.quaternion, data.quaternion) - - 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::initNodeFrame3(NProtocolExtracter *protocol_extraction) { - auto protocol = new NLT_ProtocolNodeFrame3; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) { - auto topic = "nlink_linktrack_nodeframe3"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - const auto &data = g_nlt_nodeframe3.result; - auto &msg_data = g_msg_nodeframe3; - 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); - }); -} -} // namespace linktrack +namespace linktrack +{ + nlink_parser::LinktrackAnchorframe0 g_msg_anchorframe0; + nlink_parser::LinktrackTagframe0 g_msg_tagframe0; + nlink_parser::LinktrackNodeframe0 g_msg_nodeframe0; + nlink_parser::LinktrackNodeframe1 g_msg_nodeframe1; + nlink_parser::LinktrackNodeframe2 g_msg_nodeframe2; + nlink_parser::LinktrackNodeframe3 g_msg_nodeframe3; + + serial::Serial *serial_; + + Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) + { + serial_ = serial; + InitDataTransmission(); + initAnchorFrame0(protocol_extraction); + initTagFrame0(protocol_extraction); + InitNodeFrame0(protocol_extraction); + initNodeFrame1(protocol_extraction); + initNodeFrame2(protocol_extraction); + initNodeFrame3(protocol_extraction); + } + + static void DTCallback(const std_msgs::String::ConstPtr &msg) + { + if (serial_) + serial_->write(msg->data); + } + + void Init::InitDataTransmission() + { + dt_sub_ = + nh_.subscribe("nlink_linktrack_data_transmission", 1000, DTCallback); + } + + void Init::initAnchorFrame0(NProtocolExtracter *protocol_extraction) + { + auto protocol = new NLT_ProtocolAnchorFrame0; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_anchorframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + auto data = nlt_anchorframe0_.result; + g_msg_anchorframe0.role = data.role; + g_msg_anchorframe0.id = data.id; + g_msg_anchorframe0.voltage = data.voltage; + g_msg_anchorframe0.local_time = data.local_time; + g_msg_anchorframe0.system_time = data.system_time; + auto &msg_nodes = g_msg_anchorframe0.nodes; + msg_nodes.clear(); + decltype(g_msg_anchorframe0.nodes)::value_type msg_node; + for (size_t i = 0, icount = data.valid_node_count; i < icount; ++i) + { + auto node = data.nodes[i]; + msg_node.role = node->role; + msg_node.id = node->id; + ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d) + ARRAY_ASSIGN(msg_node.dis_arr, node->dis_arr) + msg_nodes.push_back(msg_node); + } + publishers_.at(protocol).publish(g_msg_anchorframe0); + }); + } + + void Init::initTagFrame0(NProtocolExtracter *protocol_extraction) + { + auto protocol = new NLT_ProtocolTagFrame0; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_tagframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + + const auto &data = g_nlt_tagframe0.result; + auto &msg_data = g_msg_tagframe0; + + 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; + ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d) + ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d) + ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d) + ARRAY_ASSIGN(msg_data.dis_arr, data.dis_arr) + ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d) + ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d) + ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d) + ARRAY_ASSIGN(msg_data.quaternion, data.quaternion) + + publishers_.at(protocol).publish(msg_data); + }); + } + + void Init::InitNodeFrame0(NProtocolExtracter *protocol_extraction) + { + auto protocol = new NLT_ProtocolNodeFrame0; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_nodeframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + ; + } + const auto &data = g_nlt_nodeframe0.result; + auto &msg_data = g_msg_nodeframe0; + 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); + }); + } + + void Init::initNodeFrame1(NProtocolExtracter *protocol_extraction) + { + auto protocol = new NLT_ProtocolNodeFrame1; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_nodeframe1"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nlt_nodeframe1.result; + auto &msg_data = g_msg_nodeframe1; + 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; + ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d) + } + + publishers_.at(protocol).publish(msg_data); + }); + } + + void Init::initNodeFrame2(NProtocolExtracter *protocol_extraction) + { + auto protocol = new NLT_ProtocolNodeFrame2; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_nodeframe2"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nlt_nodeframe2.result; + auto &msg_data = g_msg_nodeframe2; + 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; + ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d) + ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d) + ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d) + ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d) + ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d) + ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d) + ARRAY_ASSIGN(msg_data.quaternion, data.quaternion) + + 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::initNodeFrame3(NProtocolExtracter *protocol_extraction) + { + auto protocol = new NLT_ProtocolNodeFrame3; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_nodeframe3"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nlt_nodeframe3.result; + auto &msg_data = g_msg_nodeframe3; + 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); + }); + } +} // namespace linktrack diff --git a/src/linktrack/init.h b/src/linktrack/init.h index 0bf5e07..9798e3d 100644 --- a/src/linktrack/init.h +++ b/src/linktrack/init.h @@ -10,25 +10,27 @@ #include "protocol_extracter/nprotocol_extracter.h" class NProtocolExtracter; -namespace linktrack { -class Init { - public: - explicit Init(NProtocolExtracter *protocol_extraction, - serial::Serial *serial); +namespace linktrack +{ + class Init + { + public: + explicit Init(NProtocolExtracter *protocol_extraction, + serial::Serial *serial); - private: - void InitDataTransmission(); - void initAnchorFrame0(NProtocolExtracter *protocol_extraction); - void initTagFrame0(NProtocolExtracter *protocol_extraction); - void InitNodeFrame0(NProtocolExtracter *protocol_extraction); - void initNodeFrame1(NProtocolExtracter *protocol_extraction); - void initNodeFrame2(NProtocolExtracter *protocol_extraction); - void initNodeFrame3(NProtocolExtracter *protocol_extraction); + private: + void InitDataTransmission(); + void initAnchorFrame0(NProtocolExtracter *protocol_extraction); + void initTagFrame0(NProtocolExtracter *protocol_extraction); + void InitNodeFrame0(NProtocolExtracter *protocol_extraction); + void initNodeFrame1(NProtocolExtracter *protocol_extraction); + void initNodeFrame2(NProtocolExtracter *protocol_extraction); + void initNodeFrame3(NProtocolExtracter *protocol_extraction); - std::unordered_map publishers_; - ros::NodeHandle nh_; - ros::Subscriber dt_sub_; -}; -} // namespace linktrack + std::unordered_map publishers_; + ros::NodeHandle nh_; + ros::Subscriber dt_sub_; + }; +} // namespace linktrack -#endif // LINKTRACKINIT_H +#endif // LINKTRACKINIT_H diff --git a/src/linktrack/main.cpp b/src/linktrack/main.cpp index 535fb48..4905bb0 100644 --- a/src/linktrack/main.cpp +++ b/src/linktrack/main.cpp @@ -4,7 +4,8 @@ #include "init_serial.h" #include "protocol_extracter/nprotocol_extracter.h" -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ ros::init(argc, argv, "linktrack_parser"); ros::NodeHandle nh; serial::Serial serial; @@ -12,10 +13,12 @@ int main(int argc, char **argv) { NProtocolExtracter protocol_extraction; linktrack::Init init(&protocol_extraction, &serial); ros::Rate loop_rate(1000); - while (ros::ok()) { + while (ros::ok()) + { auto available_bytes = serial.available(); std::string str_received; - if (available_bytes) { + if (available_bytes) + { serial.read(str_received, available_bytes); protocol_extraction.AddNewData(str_received); } diff --git a/src/linktrack/main_rviz_converter.cpp b/src/linktrack/main_rviz_converter.cpp index 4150da8..734a6b0 100644 --- a/src/linktrack/main_rviz_converter.cpp +++ b/src/linktrack/main_rviz_converter.cpp @@ -12,23 +12,28 @@ #include "nutils.h" -namespace { -std::string frameId; - -struct PosePair { - ros::Publisher publisher; - geometry_msgs::PoseStamped msg; - inline void publish() { publisher.publish(msg); } -}; -} // namespace - -void Anchorframe0Callback(const nlink_parser::LinktrackAnchorframe0 &msg) { +namespace +{ + std::string frameId; + + struct PosePair + { + ros::Publisher publisher; + geometry_msgs::PoseStamped msg; + inline void publish() { publisher.publish(msg); } + }; +} // namespace + +void Anchorframe0Callback(const nlink_parser::LinktrackAnchorframe0 &msg) +{ static ros::Publisher publisher; static std::map poses; - for (const auto &node : msg.nodes) { + for (const auto &node : msg.nodes) + { auto id = node.id; - if (!poses.count(id)) { + if (!poses.count(id)) + { std::ostringstream string_stream; string_stream << "nlt_anchorframe0_pose_node" << static_cast(id); auto topic = string_stream.str(); @@ -44,7 +49,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]); @@ -52,13 +57,16 @@ void Anchorframe0Callback(const nlink_parser::LinktrackAnchorframe0 &msg) { } } -void Nodeframe1Callback(const nlink_parser::LinktrackNodeframe1 &msg) { +void Nodeframe1Callback(const nlink_parser::LinktrackNodeframe1 &msg) +{ static ros::Publisher publisher; static std::map poses; - for (const auto &node : msg.nodes) { + for (const auto &node : msg.nodes) + { auto id = node.id; - if (!poses.count(id)) { + if (!poses.count(id)) + { std::ostringstream string_stream; string_stream << "nlt_nodeframe1_pose_node" << static_cast(id); auto topic = string_stream.str(); @@ -74,7 +82,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]); @@ -82,9 +90,11 @@ void Nodeframe1Callback(const nlink_parser::LinktrackNodeframe1 &msg) { } } -void Tagframe0Callback(const nlink_parser::LinktrackTagframe0 &msg) { +void Tagframe0Callback(const nlink_parser::LinktrackTagframe0 &msg) +{ static PosePair *pose = nullptr; - if (!pose) { + if (!pose) + { pose = new PosePair; auto topic = "nlt_tagframe0_pose"; pose->publisher = @@ -94,7 +104,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]); @@ -105,9 +115,11 @@ void Tagframe0Callback(const nlink_parser::LinktrackTagframe0 &msg) { pose->publish(); } -void Nodeframe2Callback(const nlink_parser::LinktrackNodeframe2 &msg) { +void Nodeframe2Callback(const nlink_parser::LinktrackNodeframe2 &msg) +{ static PosePair *pose = nullptr; - if (!pose) { + if (!pose) + { pose = new PosePair; auto topic = "nlt_nodeframe2_pose"; pose->publisher = @@ -117,7 +129,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]); @@ -128,7 +140,8 @@ void Nodeframe2Callback(const nlink_parser::LinktrackNodeframe2 &msg) { pose->publish(); } -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ ros::init(argc, argv, "linktrack_example"); ros::NodeHandle nh; diff --git a/src/linktrack/protocols.cpp b/src/linktrack/protocols.cpp index 3601283..682ce80 100644 --- a/src/linktrack/protocols.cpp +++ b/src/linktrack/protocols.cpp @@ -4,57 +4,76 @@ NLT_ProtocolAnchorFrame0::NLT_ProtocolAnchorFrame0() : NLinkProtocol( true, nlt_anchorframe0_.fixed_part_size, {nlt_anchorframe0_.frame_header, nlt_anchorframe0_.function_mark}, - {nlt_anchorframe0_.tail_check}) {} + {nlt_anchorframe0_.tail_check}) +{ +} -void NLT_ProtocolAnchorFrame0::UnpackFrameData(const uint8_t *data) { +void NLT_ProtocolAnchorFrame0::UnpackFrameData(const uint8_t *data) +{ nlt_anchorframe0_.UnpackData(data, length()); } -bool NLT_ProtocolAnchorFrame0::Verify(const uint8_t *data) { +bool NLT_ProtocolAnchorFrame0::Verify(const uint8_t *data) +{ return data[length() - 1] == nlt_anchorframe0_.tail_check; } NLT_ProtocolTagFrame0::NLT_ProtocolTagFrame0() : NLinkProtocol( true, g_nlt_tagframe0.fixed_part_size, - {g_nlt_tagframe0.frame_header, g_nlt_tagframe0.function_mark}) {} + {g_nlt_tagframe0.frame_header, g_nlt_tagframe0.function_mark}) +{ +} -void NLT_ProtocolTagFrame0::UnpackFrameData(const uint8_t *data) { +void NLT_ProtocolTagFrame0::UnpackFrameData(const uint8_t *data) +{ g_nlt_tagframe0.UnpackData(data, length()); } NLT_ProtocolNodeFrame0::NLT_ProtocolNodeFrame0() : NLinkProtocolVLength( true, g_nlt_nodeframe0.fixed_part_size, - {g_nlt_nodeframe0.frame_header, g_nlt_nodeframe0.function_mark}) {} + {g_nlt_nodeframe0.frame_header, g_nlt_nodeframe0.function_mark}) +{ +} -void NLT_ProtocolNodeFrame0::UnpackFrameData(const uint8_t *data) { +void NLT_ProtocolNodeFrame0::UnpackFrameData(const uint8_t *data) +{ g_nlt_nodeframe0.UnpackData(data, length()); } NLT_ProtocolNodeFrame1::NLT_ProtocolNodeFrame1() : NLinkProtocolVLength( true, g_nlt_nodeframe1.fixed_part_size, - {g_nlt_nodeframe1.frame_header, g_nlt_nodeframe1.function_mark}) {} + {g_nlt_nodeframe1.frame_header, g_nlt_nodeframe1.function_mark}) +{ +} -void NLT_ProtocolNodeFrame1::UnpackFrameData(const uint8_t *data) { +void NLT_ProtocolNodeFrame1::UnpackFrameData(const uint8_t *data) +{ g_nlt_nodeframe1.UnpackData(data, length()); } NLT_ProtocolNodeFrame2::NLT_ProtocolNodeFrame2() : NLinkProtocolVLength( true, g_nlt_nodeframe2.fixed_part_size, - {g_nlt_nodeframe2.frame_header, g_nlt_nodeframe2.function_mark}) {} + {g_nlt_nodeframe2.frame_header, g_nlt_nodeframe2.function_mark}) +{ +} -void NLT_ProtocolNodeFrame2::UnpackFrameData(const uint8_t *data) { +void NLT_ProtocolNodeFrame2::UnpackFrameData(const uint8_t *data) +{ g_nlt_nodeframe2.UnpackData(data, length()); } NLT_ProtocolNodeFrame3::NLT_ProtocolNodeFrame3() : NLinkProtocolVLength( true, g_nlt_nodeframe3.fixed_part_size, - {g_nlt_nodeframe3.frame_header, g_nlt_nodeframe3.function_mark}) {} + {g_nlt_nodeframe3.frame_header, g_nlt_nodeframe3.function_mark}) +{ +} -void NLT_ProtocolNodeFrame3::UnpackFrameData(const uint8_t *data) { +void NLT_ProtocolNodeFrame3::UnpackFrameData(const uint8_t *data) +{ g_nlt_nodeframe3.UnpackData(data, length()); } diff --git a/src/linktrack/protocols.h b/src/linktrack/protocols.h index b48bfd1..5d22149 100644 --- a/src/linktrack/protocols.h +++ b/src/linktrack/protocols.h @@ -9,53 +9,59 @@ #include "nlink_unpack/nlink_linktrack_nodeframe3.h" #include "nlink_unpack/nlink_linktrack_tagframe0.h" -class NLT_ProtocolAnchorFrame0 : public NLinkProtocol { - public: +class NLT_ProtocolAnchorFrame0 : public NLinkProtocol +{ +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: +class NLT_ProtocolTagFrame0 : public NLinkProtocol +{ +public: NLT_ProtocolTagFrame0(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; -class NLT_ProtocolNodeFrame0 : public NLinkProtocolVLength { - public: +class NLT_ProtocolNodeFrame0 : public NLinkProtocolVLength +{ +public: NLT_ProtocolNodeFrame0(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; -class NLT_ProtocolNodeFrame1 : public NLinkProtocolVLength { - public: +class NLT_ProtocolNodeFrame1 : public NLinkProtocolVLength +{ +public: NLT_ProtocolNodeFrame1(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; -class NLT_ProtocolNodeFrame2 : public NLinkProtocolVLength { - public: +class NLT_ProtocolNodeFrame2 : public NLinkProtocolVLength +{ +public: NLT_ProtocolNodeFrame2(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; -class NLT_ProtocolNodeFrame3 : public NLinkProtocolVLength { - public: +class NLT_ProtocolNodeFrame3 : public NLinkProtocolVLength +{ +public: NLT_ProtocolNodeFrame3(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; -#endif // LINKTRACK_PROTOCOLS_H +#endif // LINKTRACK_PROTOCOLS_H diff --git a/src/linktrack_aoa/init.cpp b/src/linktrack_aoa/init.cpp index 96733e4..73b92c2 100644 --- a/src/linktrack_aoa/init.cpp +++ b/src/linktrack_aoa/init.cpp @@ -8,112 +8,127 @@ #include "nlink_unpack/nlink_linktrack_nodeframe0.h" #include "nutils.h" -class NLTAoa_ProtocolNodeFrame0 : public NLinkProtocolVLength { - public: +class NLTAoa_ProtocolNodeFrame0 : public NLinkProtocolVLength +{ +public: NLTAoa_ProtocolNodeFrame0(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; NLTAoa_ProtocolNodeFrame0::NLTAoa_ProtocolNodeFrame0() - : NLinkProtocolVLength(true, g_nltaoa_nodeframe0.fixed_part_size, - {g_nltaoa_nodeframe0.frame_header, - g_nltaoa_nodeframe0.function_mark}) {} - -void NLTAoa_ProtocolNodeFrame0::UnpackFrameData(const uint8_t *data) { - g_nltaoa_nodeframe0.UnpackData(data, length()); -} - -namespace linktrack_aoa { - -nlink_parser::LinktrackNodeframe0 g_msg_nodeframe0; -nlink_parser::LinktrackAoaNodeframe0 g_msg_aoa_nodeframe0; - -static serial::Serial *g_serial; - -Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) { - g_serial = serial; - InitDataTransmission(); - InitNodeFrame0(protocol_extraction); - InitAoaNodeFrame0(protocol_extraction); + : NLinkProtocolVLength( + true, g_nltaoa_nodeframe0.fixed_part_size, + {g_nltaoa_nodeframe0.frame_header, g_nltaoa_nodeframe0.function_mark}) +{ } -static void DTCallback(const std_msgs::String::ConstPtr &msg) { - if (g_serial) g_serial->write(msg->data); -} - -void Init::InitDataTransmission() { - dt_sub_ = - nh_.subscribe("nlink_linktrack_data_transmission", 1000, DTCallback); -} - -void Init::InitNodeFrame0(NProtocolExtracter *protocol_extraction) { - auto protocol = new NLT_ProtocolNodeFrame0; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) { - auto topic = "nlink_linktrack_nodeframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - ; - } - const auto &data = g_nlt_nodeframe0.result; - auto &msg_data = g_msg_nodeframe0; - 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); - }); -} - -void Init::InitAoaNodeFrame0(NProtocolExtracter *protocol_extraction) { - auto protocol = new NLTAoa_ProtocolNodeFrame0; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) { - auto topic = "nlink_linktrack_aoa_nodeframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - const auto &data = g_nltaoa_nodeframe0.result; - auto &msg_data = g_msg_aoa_nodeframe0; - 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.angle = node->angle; - msg_node.fp_rssi = node->fp_rssi; - msg_node.rx_rssi = node->rx_rssi; - } - - publishers_.at(protocol).publish(msg_data); - }); +void NLTAoa_ProtocolNodeFrame0::UnpackFrameData(const uint8_t *data) +{ + g_nltaoa_nodeframe0.UnpackData(data, length()); } -} // namespace linktrack_aoa +namespace linktrack_aoa +{ + + nlink_parser::LinktrackNodeframe0 g_msg_nodeframe0; + nlink_parser::LinktrackAoaNodeframe0 g_msg_aoa_nodeframe0; + + static serial::Serial *g_serial; + + Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) + { + g_serial = serial; + InitDataTransmission(); + InitNodeFrame0(protocol_extraction); + InitAoaNodeFrame0(protocol_extraction); + } + + static void DTCallback(const std_msgs::String::ConstPtr &msg) + { + if (g_serial) + g_serial->write(msg->data); + } + + void Init::InitDataTransmission() + { + dt_sub_ = + nh_.subscribe("nlink_linktrack_data_transmission", 1000, DTCallback); + } + + void Init::InitNodeFrame0(NProtocolExtracter *protocol_extraction) + { + auto protocol = new NLT_ProtocolNodeFrame0; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_nodeframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + ; + } + const auto &data = g_nlt_nodeframe0.result; + auto &msg_data = g_msg_nodeframe0; + 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); + }); + } + + void Init::InitAoaNodeFrame0(NProtocolExtracter *protocol_extraction) + { + auto protocol = new NLTAoa_ProtocolNodeFrame0; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_aoa_nodeframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nltaoa_nodeframe0.result; + auto &msg_data = g_msg_aoa_nodeframe0; + 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.angle = node->angle; + msg_node.fp_rssi = node->fp_rssi; + msg_node.rx_rssi = node->rx_rssi; + } + + publishers_.at(protocol).publish(msg_data); + }); + } + +} // namespace linktrack_aoa diff --git a/src/linktrack_aoa/init.h b/src/linktrack_aoa/init.h index c745d61..ff7aaea 100644 --- a/src/linktrack_aoa/init.h +++ b/src/linktrack_aoa/init.h @@ -10,20 +10,22 @@ #include "protocol_extracter/nprotocol_extracter.h" -namespace linktrack_aoa { -class Init { - public: - explicit Init(NProtocolExtracter *protocol_extraction, - serial::Serial *serial); +namespace linktrack_aoa +{ + class Init + { + public: + explicit Init(NProtocolExtracter *protocol_extraction, + serial::Serial *serial); - 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 + 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 -#endif // LINKTRACKAOAINIT_H +#endif // LINKTRACKAOAINIT_H diff --git a/src/linktrack_aoa/main.cpp b/src/linktrack_aoa/main.cpp index 0957ab1..dac4df0 100644 --- a/src/linktrack_aoa/main.cpp +++ b/src/linktrack_aoa/main.cpp @@ -3,7 +3,8 @@ #include "init.h" #include "init_serial.h" -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ ros::init(argc, argv, "linktrack_aoa"); ros::NodeHandle nh; serial::Serial serial; @@ -11,10 +12,12 @@ int main(int argc, char **argv) { NProtocolExtracter protocol_extraction; linktrack_aoa::Init aoaInit(&protocol_extraction, &serial); ros::Rate loop_rate(1000); - while (ros::ok()) { + while (ros::ok()) + { auto available_bytes = serial.available(); std::string str_received; - if (available_bytes) { + if (available_bytes) + { serial.read(str_received, available_bytes); protocol_extraction.AddNewData(str_received); } diff --git a/src/tofsense/init.cpp b/src/tofsense/init.cpp index 01021d0..b99a30a 100644 --- a/src/tofsense/init.cpp +++ b/src/tofsense/init.cpp @@ -6,108 +6,129 @@ #include "nlink_unpack/nlink_utils.h" #include "nutils.h" -class NTS_ProtocolFrame0 : public NLinkProtocol { - public: +class NTS_ProtocolFrame0 : public NLinkProtocol +{ +public: NTS_ProtocolFrame0(); - protected: +protected: void UnpackFrameData(const uint8_t *data) override; }; NTS_ProtocolFrame0::NTS_ProtocolFrame0() : NLinkProtocol(true, g_nts_frame0.fixed_part_size, - {g_nts_frame0.frame_header, g_nts_frame0.function_mark}) {} + {g_nts_frame0.frame_header, g_nts_frame0.function_mark}) +{ +} -void NTS_ProtocolFrame0::UnpackFrameData(const uint8_t *data) { +void NTS_ProtocolFrame0::UnpackFrameData(const uint8_t *data) +{ g_nts_frame0.UnpackData(data, length()); } -namespace tofsense { -nlink_parser::TofsenseFrame0 g_msg_frame0; +namespace tofsense +{ + nlink_parser::TofsenseFrame0 g_msg_frame0; #pragma pack(push, 1) -struct { - char header[2]{0x57, 0x10}; - uint8_t reserved0[2]{0xff, 0xff}; - uint8_t id{}; - uint8_t reserved1[2]{0xff, 0xff}; - uint8_t checkSum{}; -} g_command_read; + struct + { + char header[2]{0x57, 0x10}; + uint8_t reserved0[2]{0xff, 0xff}; + uint8_t id{}; + uint8_t reserved1[2]{0xff, 0xff}; + uint8_t checkSum{}; + } g_command_read; #pragma pack(pop) -Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) - : serial_(serial) { - is_inquire_mode_ = - serial_ ? ros::param::param("~inquire_mode", true) : false; + Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) + : serial_(serial) + { + is_inquire_mode_ = + serial_ ? ros::param::param("~inquire_mode", true) : false; - InitFrame0(protocol_extraction); -} + InitFrame0(protocol_extraction); + } -void Init::InitFrame0(NProtocolExtracter *protocol_extraction) { - static auto protocol_frame0_ = new NTS_ProtocolFrame0; - protocol_extraction->AddProtocol(protocol_frame0_); - protocol_frame0_->SetHandleDataCallback([=] { - if (!publishers_[protocol_frame0_]) { - ros::NodeHandle nh_; - if (is_inquire_mode_) { - auto topic = "nlink_tofsense_cascade"; - publishers_[protocol_frame0_] = - nh_.advertise(topic, 50); - TopicAdvertisedTip(topic); - } else { - auto topic = "nlink_tofsense_frame0"; - publishers_[protocol_frame0_] = - nh_.advertise(topic, 50); - TopicAdvertisedTip(topic); + void Init::InitFrame0(NProtocolExtracter *protocol_extraction) + { + static auto protocol_frame0_ = new NTS_ProtocolFrame0; + protocol_extraction->AddProtocol(protocol_frame0_); + protocol_frame0_->SetHandleDataCallback([=] { + if (!publishers_[protocol_frame0_]) + { + ros::NodeHandle nh_; + if (is_inquire_mode_) + { + auto topic = "nlink_tofsense_cascade"; + publishers_[protocol_frame0_] = + nh_.advertise(topic, 50); + TopicAdvertisedTip(topic); + } + else + { + auto topic = "nlink_tofsense_frame0"; + publishers_[protocol_frame0_] = + nh_.advertise(topic, 50); + TopicAdvertisedTip(topic); + } } - } - const auto &data = g_nts_frame0.result; + const auto &data = g_nts_frame0.result; - g_msg_frame0.id = data.id; - g_msg_frame0.system_time = data.system_time; - 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.id = data.id; + g_msg_frame0.system_time = data.system_time; + g_msg_frame0.dis = data.dis; + g_msg_frame0.dis_status = data.dis_status; + g_msg_frame0.signal_strength = data.signal_strength; - if (is_inquire_mode_) { - frame0_map_[data.id] = g_msg_frame0; - } else { - publishers_.at(protocol_frame0_).publish(g_msg_frame0); - } - }); - - if (is_inquire_mode_) { - timer_scan_ = nh_.createTimer( - ros::Duration(1.0 / frequency_), - [=](const ros::TimerEvent &) { - frame0_map_.clear(); - node_index_ = 0; - timer_read_.start(); - }, - false, true); - timer_read_ = nh_.createTimer( - ros::Duration(0.006), - [=](const ros::TimerEvent &) { - if (node_index_ >= 8) { - if (!frame0_map_.empty()) { - nlink_parser::TofsenseCascade msg_cascade; - for (const auto &msg : frame0_map_) { - msg_cascade.nodes.push_back(msg.second); + if (is_inquire_mode_) + { + frame0_map_[data.id] = g_msg_frame0; + } + else + { + publishers_.at(protocol_frame0_).publish(g_msg_frame0); + } + }); + + if (is_inquire_mode_) + { + timer_scan_ = nh_.createTimer( + ros::Duration(1.0 / frequency_), + [=](const ros::TimerEvent &) { + frame0_map_.clear(); + node_index_ = 0; + timer_read_.start(); + }, + false, true); + timer_read_ = nh_.createTimer( + ros::Duration(0.006), + [=](const ros::TimerEvent &) { + if (node_index_ >= 8) + { + if (!frame0_map_.empty()) + { + nlink_parser::TofsenseCascade msg_cascade; + for (const auto &msg : frame0_map_) + { + msg_cascade.nodes.push_back(msg.second); + } + publishers_.at(protocol_frame0_).publish(msg_cascade); } - publishers_.at(protocol_frame0_).publish(msg_cascade); + timer_read_.stop(); + } + else + { + g_command_read.id = node_index_; + auto data = reinterpret_cast(&g_command_read); + NLink_UpdateCheckSum(data, sizeof(g_command_read)); + serial_->write(data, sizeof(g_command_read)); + ++node_index_; } - timer_read_.stop(); - } else { - g_command_read.id = node_index_; - auto data = reinterpret_cast(&g_command_read); - NLink_UpdateCheckSum(data, sizeof(g_command_read)); - serial_->write(data, sizeof(g_command_read)); - ++node_index_; - } - }, - false, false); + }, + false, false); + } } -} -} // namespace tofsense +} // namespace tofsense diff --git a/src/tofsense/init.h b/src/tofsense/init.h index c2483df..9192fa0 100644 --- a/src/tofsense/init.h +++ b/src/tofsense/init.h @@ -11,29 +11,31 @@ #include "protocol_extracter/nprotocol_extracter.h" -namespace tofsense { -class Init { - public: - explicit Init(NProtocolExtracter *protocol_extraction, - serial::Serial *serial); +namespace tofsense +{ + class Init + { + public: + explicit Init(NProtocolExtracter *protocol_extraction, + serial::Serial *serial); - private: - void InitFrame0(NProtocolExtracter *protocol_extraction); + private: + void InitFrame0(NProtocolExtracter *protocol_extraction); - std::unordered_map publishers_; + std::unordered_map publishers_; - std::map frame0_map_; + std::map frame0_map_; - serial::Serial *serial_; + serial::Serial *serial_; - const int frequency_ = 10; - bool is_inquire_mode_ = true; + const int frequency_ = 10; + bool is_inquire_mode_ = true; - ros::NodeHandle nh_; - ros::Timer timer_scan_; - ros::Timer timer_read_; - uint8_t node_index_ = 0; -}; + ros::NodeHandle nh_; + ros::Timer timer_scan_; + ros::Timer timer_read_; + 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..3ee085e 100644 --- a/src/tofsense/main.cpp +++ b/src/tofsense/main.cpp @@ -4,7 +4,8 @@ #include "init_serial.h" #include "protocol_extracter/nprotocol_extracter.h" -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ ros::init(argc, argv, "tofsense_parser"); ros::NodeHandle nh; serial::Serial serial; @@ -13,10 +14,12 @@ int main(int argc, char **argv) { NProtocolExtracter extracter; tofsense::Init init(&extracter, &serial); - while (ros::ok()) { + while (ros::ok()) + { auto available_bytes = serial.available(); std::string str_received; - if (available_bytes) { + if (available_bytes) + { serial.read(str_received, available_bytes); extracter.AddNewData(str_received); } diff --git a/src/utils/init_serial.cpp b/src/utils/init_serial.cpp index 44478bd..b37cdf5 100644 --- a/src/utils/init_serial.cpp +++ b/src/utils/init_serial.cpp @@ -18,8 +18,10 @@ void enumerate_ports() { } */ -void initSerial(serial::Serial *serial) { - try { +void initSerial(serial::Serial *serial) +{ + try + { auto port_name = ros::param::param("~port_name", "/dev/ttyUSB0"); auto baud_rate = ros::param::param("~baud_rate", 921600); @@ -33,13 +35,18 @@ void initSerial(serial::Serial *serial) { serial->setTimeout(timeout); serial->open(); - if (serial->isOpen()) { + if (serial->isOpen()) + { ROS_INFO("Serial port opened successfully, waiting for data."); - } else { + } + else + { ROS_ERROR("Failed to open serial port, please check and retry."); exit(EXIT_FAILURE); } - } catch (const std::exception &e) { + } + catch (const std::exception &e) + { ROS_ERROR("Unhandled Exception: %s", e.what()); exit(EXIT_FAILURE); } 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..4a9dc7c 100644 --- a/src/utils/nlink_protocol.cpp +++ b/src/utils/nlink_protocol.cpp @@ -5,20 +5,24 @@ #include #include -void NLinkProtocol::HandleData(const uint8_t *data) { +void NLinkProtocol::HandleData(const uint8_t *data) +{ UnpackFrameData(data); assert(HandleDataCallback_); HandleDataCallback_(); } -bool NLinkProtocol::Verify(const uint8_t *data) { +bool NLinkProtocol::Verify(const uint8_t *data) +{ uint8_t sum = 0; return data[length() - 1] == std::accumulate(data, data + length() - sizeof(sum), sum); } bool NLinkProtocolVLength::UpdateLength(const uint8_t *data, - size_t available_bytes) { - if (available_bytes < 4) return false; + size_t available_bytes) +{ + 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..a6a099a 100644 --- a/src/utils/nlink_protocol.h +++ b/src/utils/nlink_protocol.h @@ -4,29 +4,32 @@ #include "protocol_extracter/nprotocol_base.h" -class NLinkProtocol : public NProtocolBase { - public: +class NLinkProtocol : public NProtocolBase +{ +public: using NProtocolBase::NProtocolBase; - void SetHandleDataCallback(std::function handle) { + 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: +class NLinkProtocolVLength : public NLinkProtocol +{ +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..9ed9df2 160000 --- a/src/utils/nlink_unpack +++ b/src/utils/nlink_unpack @@ -1 +1 @@ -Subproject commit ec6155ae6f3a9dc78bd939d8815cde91fd7fb52a +Subproject commit 9ed9df23f654d5babc6d7055d62e9c34efad970d diff --git a/src/utils/nutils.cpp b/src/utils/nutils.cpp index bb443bb..ebaebe4 100644 --- a/src/utils/nutils.cpp +++ b/src/utils/nutils.cpp @@ -2,9 +2,9 @@ #include -void TopicAdvertisedTip(const char *topic) { - ROS_INFO( - "%s has been advertised,use 'rostopic " - "echo /%s' to view the data", - topic, topic); +void TopicAdvertisedTip(const char *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..5134cf7 160000 --- a/src/utils/protocol_extracter +++ b/src/utils/protocol_extracter @@ -1 +1 @@ -Subproject commit 718b0a7146d7c2cdf0950bfb37cad511bb978415 +Subproject commit 5134cf7acf46d0b66eb53f9eab398e3d24516fbf diff --git a/test/test_nlink_parser.cpp b/test/test_nlink_parser.cpp index 95bc989..8d4def1 100644 --- a/test/test_nlink_parser.cpp +++ b/test/test_nlink_parser.cpp @@ -15,105 +15,107 @@ 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 { -extern nlink_parser::LinktrackAnchorframe0 g_msg_anchorframe0; -extern nlink_parser::LinktrackTagframe0 g_msg_tagframe0; -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 +namespace linktrack +{ + extern nlink_parser::LinktrackAnchorframe0 g_msg_anchorframe0; + extern nlink_parser::LinktrackTagframe0 g_msg_tagframe0; + 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 -TEST(NLinkParser, 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); @@ -251,11 +253,13 @@ TEST(NLinkParser, linktrack) { } } -namespace tofsense { -extern nlink_parser::TofsenseFrame0 g_msg_frame0; +namespace tofsense +{ + extern nlink_parser::TofsenseFrame0 g_msg_frame0; } -TEST(NLinkParser, tofsense) { +TEST(NLinkParser, tofsense) +{ NProtocolExtracter protocol_extraction; tofsense::Init init(&protocol_extraction, nullptr); @@ -272,11 +276,13 @@ TEST(NLinkParser, tofsense) { EXPECT_EQ(msg.signal_strength, 8); } -namespace linktrack_aoa { -extern nlink_parser::LinktrackAoaNodeframe0 g_msg_aoa_nodeframe0; +namespace linktrack_aoa +{ + extern nlink_parser::LinktrackAoaNodeframe0 g_msg_aoa_nodeframe0; } -TEST(nlink_parser, linktrack_aoa) { +TEST(nlink_parser, linktrack_aoa) +{ NProtocolExtracter protocol_extraction; linktrack_aoa::Init init(&protocol_extraction, nullptr); @@ -311,7 +317,8 @@ TEST(nlink_parser, linktrack_aoa) { } // Run all the tests that were declared with TEST() -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_nlink_parser"); ros::NodeHandle nh; From 68817ddd8ea041946330bbb21bb29003821579f4 Mon Sep 17 00:00:00 2001 From: samuelhsu Date: Wed, 7 Apr 2021 23:20:22 +0800 Subject: [PATCH 02/15] - add nodeframe5 nodeframe6 --- CMakeLists.txt | 7 +++ README.en.md | 2 + README.md | 2 + msg/LinktrackNode5.msg | 5 +++ msg/LinktrackNode6.msg | 3 ++ msg/LinktrackNodeframe5.msg | 6 +++ msg/LinktrackNodeframe6.msg | 3 ++ src/linktrack/init.cpp | 88 +++++++++++++++++++++++++++++++++++-- src/linktrack/init.h | 6 ++- src/linktrack/protocols.cpp | 24 ++++++++++ src/linktrack/protocols.h | 20 +++++++++ src/linktrack_aoa/init.cpp | 8 ++-- src/linktrack_aoa/init.h | 4 +- src/utils/nlink_unpack | 2 +- test/test_nlink_parser.cpp | 41 +++++++++++++++++ 15 files changed, 208 insertions(+), 13 deletions(-) create mode 100644 msg/LinktrackNode5.msg create mode 100644 msg/LinktrackNode6.msg create mode 100644 msg/LinktrackNodeframe5.msg create mode 100644 msg/LinktrackNodeframe6.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index c659564..f0beba4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,6 +31,11 @@ add_message_files( TofsenseFrame0.msg LinktrackAoaNode0.msg LinktrackAoaNodeframe0.msg + + LinktrackNode5.msg + LinktrackNodeframe5.msg + LinktrackNode6.msg + LinktrackNodeframe6.msg ) ## Generate services in the 'srv' folder @@ -83,6 +88,8 @@ 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_nodeframe5.c + src/utils/nlink_unpack/nlink_linktrack_nodeframe6.c src/utils/nlink_unpack/nlink_tofsense_frame0.c src/utils/nlink_unpack/nlink_linktrack_aoa_nodeframe0.c src/utils/init_serial.cpp diff --git a/README.en.md b/README.en.md index c94eda5..29eb994 100644 --- a/README.en.md +++ b/README.en.md @@ -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` diff --git a/README.md b/README.md index 2681a3a..60fedd9 100644 --- a/README.md +++ b/README.md @@ -105,6 +105,8 @@ - **`/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]) 如果收到来自其他节点的数传数据，则 `/nlink_linktrack_nodeframe0` 话题将会发布消息 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/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/src/linktrack/init.cpp b/src/linktrack/init.cpp index eb49435..f6ce293 100644 --- a/src/linktrack/init.cpp +++ b/src/linktrack/init.cpp @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include #include #include @@ -26,19 +28,23 @@ namespace linktrack nlink_parser::LinktrackNodeframe1 g_msg_nodeframe1; nlink_parser::LinktrackNodeframe2 g_msg_nodeframe2; nlink_parser::LinktrackNodeframe3 g_msg_nodeframe3; + 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); + initNodeFrame5(protocol_extraction); + initNodeFrame6(protocol_extraction); } static void DTCallback(const std_msgs::String::ConstPtr &msg) @@ -47,7 +53,7 @@ namespace linktrack serial_->write(msg->data); } - void Init::InitDataTransmission() + void Init::initDataTransmission() { dt_sub_ = nh_.subscribe("nlink_linktrack_data_transmission", 1000, DTCallback); @@ -121,7 +127,7 @@ namespace linktrack }); } - void Init::InitNodeFrame0(NProtocolExtracter *protocol_extraction) + void Init::initNodeFrame0(NProtocolExtracter *protocol_extraction) { auto protocol = new NLT_ProtocolNodeFrame0; protocol_extraction->AddProtocol(protocol); @@ -274,4 +280,78 @@ namespace linktrack 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 9798e3d..f40c4e4 100644 --- a/src/linktrack/init.h +++ b/src/linktrack/init.h @@ -19,13 +19,15 @@ namespace linktrack serial::Serial *serial); private: - void InitDataTransmission(); + 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 initNodeFrame5(NProtocolExtracter *protocol_extraction); + void initNodeFrame6(NProtocolExtracter *protocol_extraction); std::unordered_map publishers_; ros::NodeHandle nh_; diff --git a/src/linktrack/protocols.cpp b/src/linktrack/protocols.cpp index 682ce80..39d82b1 100644 --- a/src/linktrack/protocols.cpp +++ b/src/linktrack/protocols.cpp @@ -77,3 +77,27 @@ void NLT_ProtocolNodeFrame3::UnpackFrameData(const uint8_t *data) { g_nlt_nodeframe3.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()); +} \ No newline at end of file diff --git a/src/linktrack/protocols.h b/src/linktrack/protocols.h index 5d22149..5645d53 100644 --- a/src/linktrack/protocols.h +++ b/src/linktrack/protocols.h @@ -7,6 +7,8 @@ #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_nodeframe5.h" +#include "nlink_unpack/nlink_linktrack_nodeframe6.h" #include "nlink_unpack/nlink_linktrack_tagframe0.h" class NLT_ProtocolAnchorFrame0 : public NLinkProtocol @@ -64,4 +66,22 @@ class NLT_ProtocolNodeFrame3 : public NLinkProtocolVLength 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 73b92c2..c891acd 100644 --- a/src/linktrack_aoa/init.cpp +++ b/src/linktrack_aoa/init.cpp @@ -40,8 +40,8 @@ namespace linktrack_aoa Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) { g_serial = serial; - InitDataTransmission(); - InitNodeFrame0(protocol_extraction); + initDataTransmission(); + initNodeFrame0(protocol_extraction); InitAoaNodeFrame0(protocol_extraction); } @@ -51,13 +51,13 @@ namespace linktrack_aoa 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); diff --git a/src/linktrack_aoa/init.h b/src/linktrack_aoa/init.h index ff7aaea..d7cc055 100644 --- a/src/linktrack_aoa/init.h +++ b/src/linktrack_aoa/init.h @@ -19,8 +19,8 @@ namespace linktrack_aoa serial::Serial *serial); private: - void InitDataTransmission(); - void InitNodeFrame0(NProtocolExtracter *protocol_extraction); + void initDataTransmission(); + void initNodeFrame0(NProtocolExtracter *protocol_extraction); void InitAoaNodeFrame0(NProtocolExtracter *protocol_extraction); std::unordered_map publishers_; ros::NodeHandle nh_; diff --git a/src/utils/nlink_unpack b/src/utils/nlink_unpack index 9ed9df2..2f3343f 160000 --- a/src/utils/nlink_unpack +++ b/src/utils/nlink_unpack @@ -1 +1 @@ -Subproject commit 9ed9df23f654d5babc6d7055d62e9c34efad970d +Subproject commit 2f3343fc7e5e5d5f417f3ee59fbb47ab8268baf5 diff --git a/test/test_nlink_parser.cpp b/test/test_nlink_parser.cpp index 8d4def1..14bd93d 100644 --- a/test/test_nlink_parser.cpp +++ b/test/test_nlink_parser.cpp @@ -6,6 +6,8 @@ #include #include #include +#include +#include #include #include @@ -33,6 +35,8 @@ namespace linktrack extern nlink_parser::LinktrackNodeframe1 g_msg_nodeframe1; extern nlink_parser::LinktrackNodeframe2 g_msg_nodeframe2; extern nlink_parser::LinktrackNodeframe3 g_msg_nodeframe3; + extern nlink_parser::LinktrackNodeframe5 g_msg_nodeframe5; + extern nlink_parser::LinktrackNodeframe6 g_msg_nodeframe6; } // namespace linktrack TEST(NLinkParser, linktrack) @@ -251,6 +255,43 @@ TEST(NLinkParser, linktrack) EXPECT_NEAR(msgData.nodes[3].fp_rssi, -92, kAbsError); EXPECT_NEAR(msgData.nodes[3].rx_rssi, -80, 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 From 8d619de95ba4f4ebb3ee00175252ab35e7701ec0 Mon Sep 17 00:00:00 2001 From: SamuelHsu Date: Fri, 18 Jun 2021 10:30:28 +0800 Subject: [PATCH 03/15] add raw data print --- src/linktrack/main.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/linktrack/main.cpp b/src/linktrack/main.cpp index 4905bb0..141a7ee 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"); @@ -20,6 +34,7 @@ int main(int argc, char **argv) if (available_bytes) { serial.read(str_received, available_bytes); + //printHexData(str_received); protocol_extraction.AddNewData(str_received); } ros::spinOnce(); From 8e2999ab2e0a8ddca5bcd7e23d45908f4523f7d0 Mon Sep 17 00:00:00 2001 From: SamuelHsu Date: Sun, 27 Feb 2022 13:23:51 +0800 Subject: [PATCH 04/15] - format --- .gitignore | 2 ++ src/linktrack/main.cpp | 23 +++++++++++++---------- src/utils/nlink_unpack | 2 +- 3 files changed, 16 insertions(+), 11 deletions(-) diff --git a/.gitignore b/.gitignore index 202644c..5b3799d 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ *.txt.user +.vscode +build \ No newline at end of file diff --git a/src/linktrack/main.cpp b/src/linktrack/main.cpp index 141a7ee..2e8f5eb 100644 --- a/src/linktrack/main.cpp +++ b/src/linktrack/main.cpp @@ -4,18 +4,21 @@ #include "init_serial.h" #include "protocol_extracter/nprotocol_extracter.h" -#include #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; - } +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) diff --git a/src/utils/nlink_unpack b/src/utils/nlink_unpack index 2f3343f..995cf68 160000 --- a/src/utils/nlink_unpack +++ b/src/utils/nlink_unpack @@ -1 +1 @@ -Subproject commit 2f3343fc7e5e5d5f417f3ee59fbb47ab8268baf5 +Subproject commit 995cf6864899c0fb045e46686ce1de10d54fcd70 From c77ae1fd665b173edcc13634542ed1f40e30e07b Mon Sep 17 00:00:00 2001 From: SamuelHsu Date: Sun, 27 Feb 2022 14:07:05 +0800 Subject: [PATCH 05/15] =?UTF-8?q?-=20=E4=B8=BAtofsens-f=E6=B7=BB=E5=8A=A0r?= =?UTF-8?q?ange=5Fprecision?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- msg/TofsenseFrame0.msg | 1 + src/tofsense/init.cpp | 81 ++++++++++++++++++++------------------ src/utils/nlink_unpack | 2 +- test/test_nlink_parser.cpp | 1 + 4 files changed, 46 insertions(+), 39 deletions(-) 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/src/tofsense/init.cpp b/src/tofsense/init.cpp index b99a30a..4e40a54 100644 --- a/src/tofsense/init.cpp +++ b/src/tofsense/init.cpp @@ -54,49 +54,53 @@ namespace tofsense { static auto protocol_frame0_ = new NTS_ProtocolFrame0; protocol_extraction->AddProtocol(protocol_frame0_); - protocol_frame0_->SetHandleDataCallback([=] { - if (!publishers_[protocol_frame0_]) - { - ros::NodeHandle nh_; - if (is_inquire_mode_) + protocol_frame0_->SetHandleDataCallback( + [=] { - auto topic = "nlink_tofsense_cascade"; - publishers_[protocol_frame0_] = - nh_.advertise(topic, 50); - TopicAdvertisedTip(topic); - } - else - { - auto topic = "nlink_tofsense_frame0"; - publishers_[protocol_frame0_] = - nh_.advertise(topic, 50); - TopicAdvertisedTip(topic); - } - } - - const auto &data = g_nts_frame0.result; - - g_msg_frame0.id = data.id; - g_msg_frame0.system_time = data.system_time; - g_msg_frame0.dis = data.dis; - g_msg_frame0.dis_status = data.dis_status; - g_msg_frame0.signal_strength = data.signal_strength; - - if (is_inquire_mode_) - { - frame0_map_[data.id] = g_msg_frame0; - } - else - { - publishers_.at(protocol_frame0_).publish(g_msg_frame0); - } - }); + if (!publishers_[protocol_frame0_]) + { + ros::NodeHandle nh_; + if (is_inquire_mode_) + { + auto topic = "nlink_tofsense_cascade"; + publishers_[protocol_frame0_] = + nh_.advertise(topic, 50); + TopicAdvertisedTip(topic); + } + else + { + auto topic = "nlink_tofsense_frame0"; + publishers_[protocol_frame0_] = + nh_.advertise(topic, 50); + TopicAdvertisedTip(topic); + } + } + + const auto &data = g_nts_frame0.result; + + g_msg_frame0.id = data.id; + g_msg_frame0.system_time = data.system_time; + 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; + } + else + { + publishers_.at(protocol_frame0_).publish(g_msg_frame0); + } + }); if (is_inquire_mode_) { timer_scan_ = nh_.createTimer( ros::Duration(1.0 / frequency_), - [=](const ros::TimerEvent &) { + [=](const ros::TimerEvent &) + { frame0_map_.clear(); node_index_ = 0; timer_read_.start(); @@ -104,7 +108,8 @@ namespace tofsense false, true); timer_read_ = nh_.createTimer( ros::Duration(0.006), - [=](const ros::TimerEvent &) { + [=](const ros::TimerEvent &) + { if (node_index_ >= 8) { if (!frame0_map_.empty()) diff --git a/src/utils/nlink_unpack b/src/utils/nlink_unpack index 995cf68..1bd6ee2 160000 --- a/src/utils/nlink_unpack +++ b/src/utils/nlink_unpack @@ -1 +1 @@ -Subproject commit 995cf6864899c0fb045e46686ce1de10d54fcd70 +Subproject commit 1bd6ee24ba4ff96eee660a1efc2cf4240405a9fc diff --git a/test/test_nlink_parser.cpp b/test/test_nlink_parser.cpp index 14bd93d..355254b 100644 --- a/test/test_nlink_parser.cpp +++ b/test/test_nlink_parser.cpp @@ -315,6 +315,7 @@ 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 linktrack_aoa From badce6a3db25b00154f8b296f68191f0e488f35e Mon Sep 17 00:00:00 2001 From: SamuelHsu Date: Sun, 27 Feb 2022 17:02:29 +0800 Subject: [PATCH 06/15] =?UTF-8?q?-=20=E6=B7=BB=E5=8A=A0nodeframe4=E6=94=AF?= =?UTF-8?q?=E6=8C=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 11 +- msg/LinktrackNode4Anchor.msg | 2 + msg/LinktrackNode4Tag.msg | 3 + msg/LinktrackNodeframe4.msg | 6 + src/linktrack/init.cpp | 555 +++++++++++++++++++---------------- src/linktrack/init.h | 1 + src/linktrack/protocols.cpp | 14 +- src/linktrack/protocols.h | 10 + src/utils/nlink_unpack | 2 +- test/test_nlink_parser.cpp | 67 ++++- 10 files changed, 414 insertions(+), 257 deletions(-) create mode 100644 msg/LinktrackNode4Anchor.msg create mode 100644 msg/LinktrackNode4Tag.msg create mode 100644 msg/LinktrackNodeframe4.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index f0beba4..21dad6c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,7 +31,9 @@ add_message_files( TofsenseFrame0.msg LinktrackAoaNode0.msg LinktrackAoaNodeframe0.msg - + LinktrackNode4Anchor.msg + LinktrackNode4Tag.msg + LinktrackNodeframe4.msg LinktrackNode5.msg LinktrackNodeframe5.msg LinktrackNode6.msg @@ -88,6 +90,7 @@ 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 @@ -153,7 +156,7 @@ 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 + catkin_add_gtest(${PROJECT_NAME}_test src/linktrack/init.cpp src/linktrack/protocols.cpp src/tofsense/init.cpp @@ -162,6 +165,6 @@ if(${CATKIN_ENABLE_TESTING}) ) 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/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/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/src/linktrack/init.cpp b/src/linktrack/init.cpp index f6ce293..9a61d2c 100644 --- a/src/linktrack/init.cpp +++ b/src/linktrack/init.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -28,6 +29,7 @@ namespace linktrack 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; @@ -43,6 +45,7 @@ namespace linktrack initNodeFrame1(protocol_extraction); initNodeFrame2(protocol_extraction); initNodeFrame3(protocol_extraction); + initNodeFrame4(protocol_extraction); initNodeFrame5(protocol_extraction); initNodeFrame6(protocol_extraction); } @@ -63,295 +66,353 @@ namespace linktrack { auto protocol = new NLT_ProtocolAnchorFrame0; protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_anchorframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - auto data = nlt_anchorframe0_.result; - g_msg_anchorframe0.role = data.role; - g_msg_anchorframe0.id = data.id; - g_msg_anchorframe0.voltage = data.voltage; - g_msg_anchorframe0.local_time = data.local_time; - g_msg_anchorframe0.system_time = data.system_time; - auto &msg_nodes = g_msg_anchorframe0.nodes; - msg_nodes.clear(); - decltype(g_msg_anchorframe0.nodes)::value_type msg_node; - for (size_t i = 0, icount = data.valid_node_count; i < icount; ++i) - { - auto node = data.nodes[i]; - msg_node.role = node->role; - msg_node.id = node->id; - ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d) - ARRAY_ASSIGN(msg_node.dis_arr, node->dis_arr) - msg_nodes.push_back(msg_node); - } - publishers_.at(protocol).publish(g_msg_anchorframe0); - }); + protocol->SetHandleDataCallback( + [=] + { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_anchorframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + auto data = nlt_anchorframe0_.result; + g_msg_anchorframe0.role = data.role; + g_msg_anchorframe0.id = data.id; + g_msg_anchorframe0.voltage = data.voltage; + g_msg_anchorframe0.local_time = data.local_time; + g_msg_anchorframe0.system_time = data.system_time; + auto &msg_nodes = g_msg_anchorframe0.nodes; + msg_nodes.clear(); + decltype(g_msg_anchorframe0.nodes)::value_type msg_node; + for (size_t i = 0, icount = data.valid_node_count; i < icount; ++i) + { + auto node = data.nodes[i]; + msg_node.role = node->role; + msg_node.id = node->id; + ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d) + ARRAY_ASSIGN(msg_node.dis_arr, node->dis_arr) + msg_nodes.push_back(msg_node); + } + publishers_.at(protocol).publish(g_msg_anchorframe0); + }); } void Init::initTagFrame0(NProtocolExtracter *protocol_extraction) { auto protocol = new NLT_ProtocolTagFrame0; protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_tagframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - - const auto &data = g_nlt_tagframe0.result; - auto &msg_data = g_msg_tagframe0; - - 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; - ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d) - ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d) - ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d) - ARRAY_ASSIGN(msg_data.dis_arr, data.dis_arr) - ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d) - ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d) - ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d) - ARRAY_ASSIGN(msg_data.quaternion, data.quaternion) - - publishers_.at(protocol).publish(msg_data); - }); + protocol->SetHandleDataCallback( + [=] + { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_tagframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + + const auto &data = g_nlt_tagframe0.result; + auto &msg_data = g_msg_tagframe0; + + 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; + ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d) + ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d) + ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d) + ARRAY_ASSIGN(msg_data.dis_arr, data.dis_arr) + ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d) + ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d) + ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d) + ARRAY_ASSIGN(msg_data.quaternion, data.quaternion) + + publishers_.at(protocol).publish(msg_data); + }); } void Init::initNodeFrame0(NProtocolExtracter *protocol_extraction) { auto protocol = new NLT_ProtocolNodeFrame0; protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_nodeframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - ; - } - const auto &data = g_nlt_nodeframe0.result; - auto &msg_data = g_msg_nodeframe0; - 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); - }); + protocol->SetHandleDataCallback( + [=] + { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_nodeframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + ; + } + const auto &data = g_nlt_nodeframe0.result; + auto &msg_data = g_msg_nodeframe0; + 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); + }); } void Init::initNodeFrame1(NProtocolExtracter *protocol_extraction) { auto protocol = new NLT_ProtocolNodeFrame1; protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_nodeframe1"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - const auto &data = g_nlt_nodeframe1.result; - auto &msg_data = g_msg_nodeframe1; - 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; - ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d) - } - - publishers_.at(protocol).publish(msg_data); - }); + protocol->SetHandleDataCallback( + [=] + { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_nodeframe1"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nlt_nodeframe1.result; + auto &msg_data = g_msg_nodeframe1; + 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; + ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d) + } + + publishers_.at(protocol).publish(msg_data); + }); } void Init::initNodeFrame2(NProtocolExtracter *protocol_extraction) { auto protocol = new NLT_ProtocolNodeFrame2; protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_nodeframe2"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - const auto &data = g_nlt_nodeframe2.result; - auto &msg_data = g_msg_nodeframe2; - 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; - ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d) - ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d) - ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d) - ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d) - ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d) - ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d) - ARRAY_ASSIGN(msg_data.quaternion, data.quaternion) - - 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); - }); + protocol->SetHandleDataCallback( + [=] + { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_nodeframe2"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nlt_nodeframe2.result; + auto &msg_data = g_msg_nodeframe2; + 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; + ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d) + ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d) + ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d) + ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d) + ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d) + ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d) + ARRAY_ASSIGN(msg_data.quaternion, data.quaternion) + + 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::initNodeFrame3(NProtocolExtracter *protocol_extraction) { auto protocol = new NLT_ProtocolNodeFrame3; protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_nodeframe3"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - const auto &data = g_nlt_nodeframe3.result; - auto &msg_data = g_msg_nodeframe3; - 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); - }); + protocol->SetHandleDataCallback( + [=] + { + if (!publishers_[protocol]) + { + auto topic = "nlink_linktrack_nodeframe3"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nlt_nodeframe3.result; + auto &msg_data = g_msg_nodeframe3; + 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::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); - }); + 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); - }); + 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 f40c4e4..67fd8d9 100644 --- a/src/linktrack/init.h +++ b/src/linktrack/init.h @@ -26,6 +26,7 @@ namespace linktrack 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); diff --git a/src/linktrack/protocols.cpp b/src/linktrack/protocols.cpp index 39d82b1..1ada1b1 100644 --- a/src/linktrack/protocols.cpp +++ b/src/linktrack/protocols.cpp @@ -78,6 +78,18 @@ 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, @@ -100,4 +112,4 @@ NLT_ProtocolNodeFrame6::NLT_ProtocolNodeFrame6() void NLT_ProtocolNodeFrame6::UnpackFrameData(const uint8_t *data) { g_nlt_nodeframe6.UnpackData(data, length()); -} \ No newline at end of file +} diff --git a/src/linktrack/protocols.h b/src/linktrack/protocols.h index 5645d53..2597472 100644 --- a/src/linktrack/protocols.h +++ b/src/linktrack/protocols.h @@ -7,6 +7,7 @@ #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" @@ -66,6 +67,15 @@ class NLT_ProtocolNodeFrame3 : public NLinkProtocolVLength void UnpackFrameData(const uint8_t *data) override; }; +class NLT_ProtocolNodeFrame4 : public NLinkProtocolVLength +{ +public: + NLT_ProtocolNodeFrame4(); + +protected: + void UnpackFrameData(const uint8_t *data) override; +}; + class NLT_ProtocolNodeFrame5 : public NLinkProtocolVLength { public: diff --git a/src/utils/nlink_unpack b/src/utils/nlink_unpack index 1bd6ee2..0f9acd2 160000 --- a/src/utils/nlink_unpack +++ b/src/utils/nlink_unpack @@ -1 +1 @@ -Subproject commit 1bd6ee24ba4ff96eee660a1efc2cf4240405a9fc +Subproject commit 0f9acd2f39804d24320fb613760d9138fdbef13c diff --git a/test/test_nlink_parser.cpp b/test/test_nlink_parser.cpp index 355254b..42a3815 100644 --- a/test/test_nlink_parser.cpp +++ b/test/test_nlink_parser.cpp @@ -1,4 +1,7 @@ // gtest +#include "../src/linktrack/init.h" +#include "../src/linktrack_aoa/init.h" +#include "../src/tofsense/init.h" #include #include #include @@ -6,14 +9,12 @@ #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 static const double kAbsError = 0.001; @@ -35,6 +36,7 @@ namespace linktrack extern nlink_parser::LinktrackNodeframe1 g_msg_nodeframe1; extern nlink_parser::LinktrackNodeframe2 g_msg_nodeframe2; extern nlink_parser::LinktrackNodeframe3 g_msg_nodeframe3; + extern nlink_parser::LinktrackNodeframe4 g_msg_nodeframe4; extern nlink_parser::LinktrackNodeframe5 g_msg_nodeframe5; extern nlink_parser::LinktrackNodeframe6 g_msg_nodeframe6; } // namespace linktrack @@ -255,7 +257,64 @@ 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 " From cbef8589a6c45e0655496f352e9a57220911d495 Mon Sep 17 00:00:00 2001 From: SamuelHsu Date: Sun, 27 Feb 2022 19:56:42 +0800 Subject: [PATCH 07/15] =?UTF-8?q?-=20=E6=B7=BB=E5=8A=A0tofsensem=5Fframe0?= =?UTF-8?q?=E5=8D=8F=E8=AE=AE=E6=94=AF=E6=8C=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 14 +++++++ launch/tofsensem.launch | 6 +++ msg/TofsenseMFrame0.msg | 4 ++ msg/TofsenseMFrame0Pixel.msg | 3 ++ src/tofsensem/init.cpp | 76 ++++++++++++++++++++++++++++++++++++ src/tofsensem/init.h | 23 +++++++++++ src/tofsensem/main.cpp | 37 ++++++++++++++++++ src/utils/nlink_unpack | 2 +- test/test_nlink_parser.cpp | 66 +++++++++++++++++++++++++++++++ 9 files changed, 230 insertions(+), 1 deletion(-) create mode 100644 launch/tofsensem.launch create mode 100644 msg/TofsenseMFrame0.msg create mode 100644 msg/TofsenseMFrame0Pixel.msg create mode 100644 src/tofsensem/init.cpp create mode 100644 src/tofsensem/init.h create mode 100644 src/tofsensem/main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 21dad6c..434a8f7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,6 +38,8 @@ add_message_files( LinktrackNodeframe5.msg LinktrackNode6.msg LinktrackNodeframe6.msg + TofsenseMFrame0.msg + TofsenseMFrame0Pixel.msg ) ## Generate services in the 'srv' folder @@ -94,6 +96,7 @@ add_library(${nutils} 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/init_serial.cpp src/utils/nlink_protocol.cpp @@ -136,6 +139,16 @@ target_link_libraries(${TOFSENSE_NAME} ) add_dependencies(${TOFSENSE_NAME} ${PROJECT_NAME}_generate_messages_cpp) +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(LINKTRACK_AOA_NAME "linktrack_aoa") add_executable(${LINKTRACK_AOA_NAME} src/linktrack_aoa/init.cpp @@ -162,6 +175,7 @@ if(${CATKIN_ENABLE_TESTING}) src/tofsense/init.cpp src/linktrack_aoa/init.cpp test/test_nlink_parser.cpp + src/tofsensem/init.cpp ) endif() 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/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/tofsensem/init.cpp b/src/tofsensem/init.cpp new file mode 100644 index 0000000..a65c14c --- /dev/null +++ b/src/tofsensem/init.cpp @@ -0,0 +1,76 @@ + +#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..cc22c62 --- /dev/null +++ b/src/tofsensem/init.h @@ -0,0 +1,23 @@ +#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..10b0bca --- /dev/null +++ b/src/tofsensem/main.cpp @@ -0,0 +1,37 @@ +#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); + + 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(); + } + return EXIT_SUCCESS; +} diff --git a/src/utils/nlink_unpack b/src/utils/nlink_unpack index 0f9acd2..ac1dffd 160000 --- a/src/utils/nlink_unpack +++ b/src/utils/nlink_unpack @@ -1 +1 @@ -Subproject commit 0f9acd2f39804d24320fb613760d9138fdbef13c +Subproject commit ac1dffde840c9b1252bc2ed3a768c66cca6cc82c diff --git a/test/test_nlink_parser.cpp b/test/test_nlink_parser.cpp index 42a3815..187cbfb 100644 --- a/test/test_nlink_parser.cpp +++ b/test/test_nlink_parser.cpp @@ -2,6 +2,7 @@ #include "../src/linktrack/init.h" #include "../src/linktrack_aoa/init.h" #include "../src/tofsense/init.h" +#include "../src/tofsensem/init.h" #include #include #include @@ -14,6 +15,7 @@ #include #include #include +#include #include static const double kAbsError = 0.001; @@ -377,6 +379,70 @@ TEST(NLinkParser, tofsense) 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 { extern nlink_parser::LinktrackAoaNodeframe0 g_msg_aoa_nodeframe0; From 649989c992c4466bb2c26afea3a6edb32a570807 Mon Sep 17 00:00:00 2001 From: SamuelHsu Date: Sun, 27 Feb 2022 21:22:56 +0800 Subject: [PATCH 08/15] =?UTF-8?q?-=20=E6=B7=BB=E5=8A=A0iot=5Fframe0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 22 +++++++++--- launch/iot.launch | 6 ++++ msg/IotFrame0.msg | 2 ++ msg/IotFrame0Node.msg | 5 +++ src/iot/init.cpp | 72 ++++++++++++++++++++++++++++++++++++++ src/iot/init.h | 24 +++++++++++++ src/iot/main.cpp | 35 ++++++++++++++++++ src/utils/nlink_unpack | 2 +- test/test_nlink_parser.cpp | 45 ++++++++++++++++++++++++ 9 files changed, 208 insertions(+), 5 deletions(-) create mode 100644 launch/iot.launch create mode 100644 msg/IotFrame0.msg create mode 100644 msg/IotFrame0Node.msg create mode 100644 src/iot/init.cpp create mode 100644 src/iot/init.h create mode 100644 src/iot/main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 434a8f7..e8e0202 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,6 +40,8 @@ add_message_files( LinktrackNodeframe6.msg TofsenseMFrame0.msg TofsenseMFrame0Pixel.msg + IotFrame0.msg + IotFrame0Node.msg ) ## Generate services in the 'srv' folder @@ -98,6 +100,7 @@ add_library(${nutils} 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 @@ -149,17 +152,27 @@ target_link_libraries(${EXE_NAME} ) add_dependencies(${EXE_NAME} ${PROJECT_NAME}_generate_messages_cpp) -set(LINKTRACK_AOA_NAME "linktrack_aoa") -add_executable(${LINKTRACK_AOA_NAME} +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} +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) ############# @@ -176,6 +189,7 @@ if(${CATKIN_ENABLE_TESTING}) src/linktrack_aoa/init.cpp test/test_nlink_parser.cpp src/tofsensem/init.cpp + src/iot/init.cpp ) endif() 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/msg/IotFrame0.msg b/msg/IotFrame0.msg new file mode 100644 index 0000000..19e3845 --- /dev/null +++ b/msg/IotFrame0.msg @@ -0,0 +1,2 @@ +uint32 uid +IotFrame0Node[] nodes diff --git a/msg/IotFrame0Node.msg b/msg/IotFrame0Node.msg new file mode 100644 index 0000000..1685aaf --- /dev/null +++ b/msg/IotFrame0Node.msg @@ -0,0 +1,5 @@ +uint32 uid +uint8 cnt +float32 dis +float32 aoa_angle_horizontal +float32 aoa_angle_vertical diff --git a/src/iot/init.cpp b/src/iot/init.cpp new file mode 100644 index 0000000..96b65ab --- /dev/null +++ b/src/iot/init.cpp @@ -0,0 +1,72 @@ + +#include "init.h" + +#include "nlink_protocol.h" +#include "nlink_unpack/nlink_iot_frame0.h" +#include "nlink_unpack/nlink_utils.h" +#include "nutils.h" + +namespace +{ + class ProtocolFrame0 : public NLinkProtocol + { + public: + ProtocolFrame0() + : NLinkProtocol(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.nodes.resize(IOT_FRAME0_NODE_COUNT); + for (int i = 0; i < IOT_FRAME0_NODE_COUNT; ++i) + { + g_msg_iotframe0.nodes[i].uid = data.nodes[i].uid; + g_msg_iotframe0.nodes[i].cnt = data.nodes[i].cnt; + g_msg_iotframe0.nodes[i].dis = data.nodes[i].dis; + g_msg_iotframe0.nodes[i].aoa_angle_horizontal = + data.nodes[i].aoa_angle_horizontal; + g_msg_iotframe0.nodes[i].aoa_angle_vertical = + data.nodes[i].aoa_angle_vertical; + } + + 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..94a9e1e --- /dev/null +++ b/src/iot/init.h @@ -0,0 +1,24 @@ +#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..0277ba2 --- /dev/null +++ b/src/iot/main.cpp @@ -0,0 +1,35 @@ +#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); + + 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(); + } + return EXIT_SUCCESS; +} diff --git a/src/utils/nlink_unpack b/src/utils/nlink_unpack index ac1dffd..2e9ef3e 160000 --- a/src/utils/nlink_unpack +++ b/src/utils/nlink_unpack @@ -1 +1 @@ -Subproject commit ac1dffde840c9b1252bc2ed3a768c66cca6cc82c +Subproject commit 2e9ef3e937b013978925678ac3f508db38bf3eb6 diff --git a/test/test_nlink_parser.cpp b/test/test_nlink_parser.cpp index 187cbfb..7bc5b17 100644 --- a/test/test_nlink_parser.cpp +++ b/test/test_nlink_parser.cpp @@ -1,9 +1,11 @@ // 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 @@ -443,6 +445,49 @@ TEST(NLinkParser, tofsensem) } } +namespace iot +{ + extern nlink_parser::IotFrame0 g_msg_iotframe0; +} + +TEST(NLinkParser, iot) +{ + NProtocolExtracter protocol_extraction; + iot::Init init(&protocol_extraction); + + uint8_t data[1024]; + auto string = + "55 01 52 00 6b 00 4a 00 62 00 03 60 00 00 2b ef ff 00 00 00 47 00 48 00 " + "ac 52 00 00 6e da 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 " + "00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 " + "00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 " + "00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 " + "00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 " + "00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 10"; + std::vector vals{ + 7012434, 6422602, 3, 0.096, -4.309, 0, 4718663, 172, 0.082, 55.918, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0}; + 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 = iot::g_msg_iotframe0; + EXPECT_NEAR(msg.uid, next_val(), kAbsError); + for (int i = 0; i < msg.nodes.size(); ++i) + { + const auto &node = msg.nodes.at(i); + EXPECT_NEAR(node.uid, next_val(), kAbsError); + EXPECT_NEAR(node.cnt, next_val(), kAbsError); + EXPECT_NEAR(node.dis, next_val(), kAbsError); + EXPECT_NEAR(node.aoa_angle_horizontal, next_val(), kAbsError); + EXPECT_NEAR(node.aoa_angle_vertical, next_val(), kAbsError); + } +} + namespace linktrack_aoa { extern nlink_parser::LinktrackAoaNodeframe0 g_msg_aoa_nodeframe0; From 07e393c45c49df5e912c646c615d5602e8814677 Mon Sep 17 00:00:00 2001 From: SamuelHsu Date: Sun, 27 Feb 2022 21:23:14 +0800 Subject: [PATCH 09/15] - update readme --- README.md | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/README.md b/README.md index 60fedd9..75bbd50 100644 --- a/README.md +++ b/README.md @@ -35,6 +35,8 @@ - [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) @@ -105,6 +107,7 @@ - **`/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]) @@ -150,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 From 27a9dc65d57c29bebb736776ce0a307034c4ac4b Mon Sep 17 00:00:00 2001 From: SamuelHsu Date: Thu, 7 Apr 2022 10:35:46 +0800 Subject: [PATCH 10/15] - make serial lib a submodule --- .gitmodules | 3 +++ CMakeLists.txt | 3 +++ extern/serial | 1 + 3 files changed, 7 insertions(+) create mode 160000 extern/serial diff --git a/.gitmodules b/.gitmodules index 7df1396..ef61ac8 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/nooploop-dev/serial.git diff --git a/CMakeLists.txt b/CMakeLists.txt index e8e0202..b540e8e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,9 @@ find_package(catkin REQUIRED COMPONENTS message_generation ) +add_subdirectory(extern/serial) +# find_package(serial CONFIG REQUIRED) + ## Generate messages in the 'msg' folder add_message_files( FILES diff --git a/extern/serial b/extern/serial new file mode 160000 index 0000000..fa1141a --- /dev/null +++ b/extern/serial @@ -0,0 +1 @@ +Subproject commit fa1141a6ecbf4fd495560ad7acbaad844e158e41 From f338ea555c85105e35860fb3801c6e30300625e5 Mon Sep 17 00:00:00 2001 From: samuelyhsu Date: Sat, 10 Jun 2023 17:09:01 +0800 Subject: [PATCH 11/15] - format with clang-format llvm --- .gitignore | 2 +- CMakeLists.txt | 112 ++-- README.en.md | 8 +- README.md | 6 +- msg/LinktrackAnchorframe0.msg | 1 - msg/LinktrackTagframe0.msg | 1 - src/iot/init.cpp | 100 ++-- src/iot/init.h | 20 +- src/iot/main.cpp | 13 +- src/linktrack/init.cpp | 740 ++++++++++++-------------- src/linktrack/init.h | 42 +- src/linktrack/main.cpp | 20 +- src/linktrack/main_rviz_converter.cpp | 51 +- src/linktrack/protocols.cpp | 66 +-- src/linktrack/protocols.h | 27 +- src/linktrack_aoa/init.cpp | 205 ++++--- src/linktrack_aoa/init.h | 28 +- src/linktrack_aoa/main.cpp | 9 +- src/tofsense/init.cpp | 183 +++---- src/tofsense/init.h | 36 +- src/tofsense/main.cpp | 9 +- src/tofsensem/init.cpp | 113 ++-- src/tofsensem/init.h | 20 +- src/tofsensem/main.cpp | 13 +- src/utils/init_serial.cpp | 17 +- src/utils/nlink_protocol.cpp | 11 +- src/utils/nlink_protocol.h | 9 +- src/utils/nlink_unpack | 2 +- src/utils/nutils.cpp | 3 +- src/utils/protocol_extracter | 2 +- test/test_nlink_parser.cpp | 74 +-- 31 files changed, 860 insertions(+), 1083 deletions(-) diff --git a/.gitignore b/.gitignore index 5b3799d..ef3c35a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,3 @@ *.txt.user .vscode -build \ No newline at end of file +build diff --git a/CMakeLists.txt b/CMakeLists.txt index b540e8e..2a08f0e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,23 +1,24 @@ 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) + # find_package(serial CONFIG REQUIRED) -## Generate messages in the 'msg' folder +# # Generate messages in the 'msg' folder add_message_files( FILES LinktrackAnchorframe0.msg @@ -45,46 +46,48 @@ add_message_files( 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 +) + +# ########## +# # Build ## +# ########## -## Specify additional locations of header files -## Your package locations should be listed before other locations +# # 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} @@ -109,8 +112,8 @@ add_library(${nutils} src/utils/nutils.cpp ) target_link_libraries(${nutils} - ${catkin_LIBRARIES} - serial + ${catkin_LIBRARIES} + serial ) set(LINKTRACK_NAME "linktrack") @@ -118,41 +121,43 @@ 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(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") @@ -160,31 +165,32 @@ add_executable(${EXE_NAME} src/linktrack_aoa/init.cpp src/linktrack_aoa/main.cpp src/linktrack/protocols.cpp - ) +) target_link_libraries(${EXE_NAME} -# ${catkin_LIBRARIES} + + # ${catkin_LIBRARIES} ${nutils} - ) +) 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 + + # 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 @@ -193,9 +199,9 @@ if(${CATKIN_ENABLE_TESTING}) 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}) + target_link_libraries(${PROJECT_NAME}_test ${nutils}) endif() diff --git a/README.en.md b/README.en.md index 29eb994..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 @@ -185,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 75bbd50..2af5f73 100644 --- a/README.md +++ b/README.md @@ -39,10 +39,10 @@ - [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) 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/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/src/iot/init.cpp b/src/iot/init.cpp index 96b65ab..47bbf49 100644 --- a/src/iot/init.cpp +++ b/src/iot/init.cpp @@ -6,67 +6,55 @@ #include "nlink_unpack/nlink_utils.h" #include "nutils.h" -namespace -{ - class ProtocolFrame0 : public NLinkProtocol - { - public: - ProtocolFrame0() - : NLinkProtocol(true, g_iot_frame0.fixed_part_size, - {g_iot_frame0.frame_header, g_iot_frame0.function_mark}) - { - } +namespace { +class ProtocolFrame0 : public NLinkProtocol { +public: + ProtocolFrame0() + : NLinkProtocol(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()); - } - }; +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); - } +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.nodes.resize(IOT_FRAME0_NODE_COUNT); - for (int i = 0; i < IOT_FRAME0_NODE_COUNT; ++i) - { - g_msg_iotframe0.nodes[i].uid = data.nodes[i].uid; - g_msg_iotframe0.nodes[i].cnt = data.nodes[i].cnt; - g_msg_iotframe0.nodes[i].dis = data.nodes[i].dis; - g_msg_iotframe0.nodes[i].aoa_angle_horizontal = - data.nodes[i].aoa_angle_horizontal; - g_msg_iotframe0.nodes[i].aoa_angle_vertical = - data.nodes[i].aoa_angle_vertical; - } + const auto &data = g_iot_frame0; + g_msg_iotframe0.uid = data.uid; + g_msg_iotframe0.nodes.resize(IOT_FRAME0_NODE_COUNT); + for (int i = 0; i < IOT_FRAME0_NODE_COUNT; ++i) { + g_msg_iotframe0.nodes[i].uid = data.nodes[i].uid; + g_msg_iotframe0.nodes[i].cnt = data.nodes[i].cnt; + g_msg_iotframe0.nodes[i].dis = data.nodes[i].dis; + g_msg_iotframe0.nodes[i].aoa_angle_horizontal = + data.nodes[i].aoa_angle_horizontal; + g_msg_iotframe0.nodes[i].aoa_angle_vertical = + data.nodes[i].aoa_angle_vertical; + } - publishers_.at(protocol).publish(g_msg_iotframe0); - }); - } + publishers_.at(protocol).publish(g_msg_iotframe0); + }); +} } // namespace iot diff --git a/src/iot/init.h b/src/iot/init.h index 94a9e1e..b80d0bb 100644 --- a/src/iot/init.h +++ b/src/iot/init.h @@ -7,18 +7,16 @@ #include #include -namespace iot -{ - class Init - { - public: - explicit Init(NProtocolExtracter *protocol_extraction); +namespace iot { +class Init { +public: + explicit Init(NProtocolExtracter *protocol_extraction); - private: - void InitFrame0(NProtocolExtracter *protocol_extraction); - std::unordered_map publishers_; - ros::NodeHandle nh_; - }; +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 index 0277ba2..d3b29f4 100644 --- a/src/iot/main.cpp +++ b/src/iot/main.cpp @@ -6,8 +6,7 @@ #include #include -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "iot_parser"); ros::NodeHandle nh; serial::Serial serial; @@ -16,17 +15,13 @@ int main(int argc, char **argv) NProtocolExtracter extracter; iot::Init init(&extracter); - while (ros::ok()) - { + while (ros::ok()) { auto available_bytes = serial.available(); std::string str_received; - if (available_bytes) - { + if (available_bytes) { serial.read(str_received, available_bytes); extracter.AddNewData(str_received); - } - else - { + } else { std::this_thread::sleep_for(std::chrono::milliseconds(10)); } ros::spinOnce(); diff --git a/src/linktrack/init.cpp b/src/linktrack/init.cpp index 9a61d2c..bbe259d 100644 --- a/src/linktrack/init.cpp +++ b/src/linktrack/init.cpp @@ -16,403 +16,353 @@ #include "protocols.h" #define ARRAY_ASSIGN(DEST, SRC) \ - for (size_t _CNT = 0; _CNT < sizeof(SRC) / sizeof(SRC[0]); ++_CNT) \ - { \ + for (size_t _CNT = 0; _CNT < sizeof(SRC) / sizeof(SRC[0]); ++_CNT) { \ DEST[_CNT] = SRC[_CNT]; \ } -namespace linktrack -{ - nlink_parser::LinktrackAnchorframe0 g_msg_anchorframe0; - nlink_parser::LinktrackTagframe0 g_msg_tagframe0; - 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(); - initAnchorFrame0(protocol_extraction); - initTagFrame0(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); - } - - void Init::initDataTransmission() - { - dt_sub_ = - nh_.subscribe("nlink_linktrack_data_transmission", 1000, DTCallback); - } - - void Init::initAnchorFrame0(NProtocolExtracter *protocol_extraction) - { - auto protocol = new NLT_ProtocolAnchorFrame0; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback( - [=] - { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_anchorframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - auto data = nlt_anchorframe0_.result; - g_msg_anchorframe0.role = data.role; - g_msg_anchorframe0.id = data.id; - g_msg_anchorframe0.voltage = data.voltage; - g_msg_anchorframe0.local_time = data.local_time; - g_msg_anchorframe0.system_time = data.system_time; - auto &msg_nodes = g_msg_anchorframe0.nodes; - msg_nodes.clear(); - decltype(g_msg_anchorframe0.nodes)::value_type msg_node; - for (size_t i = 0, icount = data.valid_node_count; i < icount; ++i) - { - auto node = data.nodes[i]; - msg_node.role = node->role; - msg_node.id = node->id; - ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d) - ARRAY_ASSIGN(msg_node.dis_arr, node->dis_arr) - msg_nodes.push_back(msg_node); - } - publishers_.at(protocol).publish(g_msg_anchorframe0); - }); - } - - void Init::initTagFrame0(NProtocolExtracter *protocol_extraction) - { - auto protocol = new NLT_ProtocolTagFrame0; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback( - [=] - { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_tagframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - - const auto &data = g_nlt_tagframe0.result; - auto &msg_data = g_msg_tagframe0; - - 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; - ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d) - ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d) - ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d) - ARRAY_ASSIGN(msg_data.dis_arr, data.dis_arr) - ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d) - ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d) - ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d) - ARRAY_ASSIGN(msg_data.quaternion, data.quaternion) - - publishers_.at(protocol).publish(msg_data); - }); - } - - void Init::initNodeFrame0(NProtocolExtracter *protocol_extraction) - { - auto protocol = new NLT_ProtocolNodeFrame0; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback( - [=] - { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_nodeframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - ; - } - const auto &data = g_nlt_nodeframe0.result; - auto &msg_data = g_msg_nodeframe0; - 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); - }); - } - - void Init::initNodeFrame1(NProtocolExtracter *protocol_extraction) - { - auto protocol = new NLT_ProtocolNodeFrame1; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback( - [=] - { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_nodeframe1"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - const auto &data = g_nlt_nodeframe1.result; - auto &msg_data = g_msg_nodeframe1; - 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; - ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d) - } - - publishers_.at(protocol).publish(msg_data); - }); - } - - void Init::initNodeFrame2(NProtocolExtracter *protocol_extraction) - { - auto protocol = new NLT_ProtocolNodeFrame2; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback( - [=] - { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_nodeframe2"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - const auto &data = g_nlt_nodeframe2.result; - auto &msg_data = g_msg_nodeframe2; - 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; - ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d) - ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d) - ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d) - ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d) - ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d) - ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d) - ARRAY_ASSIGN(msg_data.quaternion, data.quaternion) - - 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::initNodeFrame3(NProtocolExtracter *protocol_extraction) - { - auto protocol = new NLT_ProtocolNodeFrame3; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback( - [=] - { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_nodeframe3"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - const auto &data = g_nlt_nodeframe3.result; - auto &msg_data = g_msg_nodeframe3; - 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::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 { +nlink_parser::LinktrackAnchorframe0 g_msg_anchorframe0; +nlink_parser::LinktrackTagframe0 g_msg_tagframe0; +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(); + initAnchorFrame0(protocol_extraction); + initTagFrame0(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); +} + +void Init::initDataTransmission() { + dt_sub_ = + nh_.subscribe("nlink_linktrack_data_transmission", 1000, DTCallback); +} + +void Init::initAnchorFrame0(NProtocolExtracter *protocol_extraction) { + auto protocol = new NLT_ProtocolAnchorFrame0; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + auto topic = "nlink_linktrack_anchorframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + auto data = nlt_anchorframe0_.result; + g_msg_anchorframe0.role = data.role; + g_msg_anchorframe0.id = data.id; + g_msg_anchorframe0.voltage = data.voltage; + g_msg_anchorframe0.local_time = data.local_time; + g_msg_anchorframe0.system_time = data.system_time; + auto &msg_nodes = g_msg_anchorframe0.nodes; + msg_nodes.clear(); + decltype(g_msg_anchorframe0.nodes)::value_type msg_node; + for (size_t i = 0, icount = data.valid_node_count; i < icount; ++i) { + auto node = data.nodes[i]; + msg_node.role = node->role; + msg_node.id = node->id; + ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d) + ARRAY_ASSIGN(msg_node.dis_arr, node->dis_arr) + msg_nodes.push_back(msg_node); + } + publishers_.at(protocol).publish(g_msg_anchorframe0); + }); +} + +void Init::initTagFrame0(NProtocolExtracter *protocol_extraction) { + auto protocol = new NLT_ProtocolTagFrame0; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + auto topic = "nlink_linktrack_tagframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + + const auto &data = g_nlt_tagframe0.result; + auto &msg_data = g_msg_tagframe0; + + 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; + ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d) + ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d) + ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d) + ARRAY_ASSIGN(msg_data.dis_arr, data.dis_arr) + ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d) + ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d) + ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d) + ARRAY_ASSIGN(msg_data.quaternion, data.quaternion) + + publishers_.at(protocol).publish(msg_data); + }); +} + +void Init::initNodeFrame0(NProtocolExtracter *protocol_extraction) { + auto protocol = new NLT_ProtocolNodeFrame0; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + auto topic = "nlink_linktrack_nodeframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + ; + } + const auto &data = g_nlt_nodeframe0.result; + auto &msg_data = g_msg_nodeframe0; + 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); + }); +} + +void Init::initNodeFrame1(NProtocolExtracter *protocol_extraction) { + auto protocol = new NLT_ProtocolNodeFrame1; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + auto topic = "nlink_linktrack_nodeframe1"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nlt_nodeframe1.result; + auto &msg_data = g_msg_nodeframe1; + 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; + ARRAY_ASSIGN(msg_node.pos_3d, node->pos_3d) + } + + publishers_.at(protocol).publish(msg_data); + }); +} + +void Init::initNodeFrame2(NProtocolExtracter *protocol_extraction) { + auto protocol = new NLT_ProtocolNodeFrame2; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + auto topic = "nlink_linktrack_nodeframe2"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nlt_nodeframe2.result; + auto &msg_data = g_msg_nodeframe2; + 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; + ARRAY_ASSIGN(msg_data.pos_3d, data.pos_3d) + ARRAY_ASSIGN(msg_data.eop_3d, data.eop_3d) + ARRAY_ASSIGN(msg_data.vel_3d, data.vel_3d) + ARRAY_ASSIGN(msg_data.imu_gyro_3d, data.imu_gyro_3d) + ARRAY_ASSIGN(msg_data.imu_acc_3d, data.imu_acc_3d) + ARRAY_ASSIGN(msg_data.angle_3d, data.angle_3d) + ARRAY_ASSIGN(msg_data.quaternion, data.quaternion) + + 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::initNodeFrame3(NProtocolExtracter *protocol_extraction) { + auto protocol = new NLT_ProtocolNodeFrame3; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + auto topic = "nlink_linktrack_nodeframe3"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nlt_nodeframe3.result; + auto &msg_data = g_msg_nodeframe3; + 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::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 67fd8d9..d68e3b6 100644 --- a/src/linktrack/init.h +++ b/src/linktrack/init.h @@ -10,30 +10,28 @@ #include "protocol_extracter/nprotocol_extracter.h" class NProtocolExtracter; -namespace linktrack -{ - class Init - { - public: - explicit Init(NProtocolExtracter *protocol_extraction, - serial::Serial *serial); +namespace linktrack { +class Init { +public: + explicit Init(NProtocolExtracter *protocol_extraction, + serial::Serial *serial); - private: - void initDataTransmission(); - void initAnchorFrame0(NProtocolExtracter *protocol_extraction); - void initTagFrame0(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); +private: + void initDataTransmission(); + void initAnchorFrame0(NProtocolExtracter *protocol_extraction); + void initTagFrame0(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_; - }; + std::unordered_map publishers_; + ros::NodeHandle nh_; + ros::Subscriber dt_sub_; +}; } // namespace linktrack #endif // LINKTRACKINIT_H diff --git a/src/linktrack/main.cpp b/src/linktrack/main.cpp index 2e8f5eb..899900c 100644 --- a/src/linktrack/main.cpp +++ b/src/linktrack/main.cpp @@ -7,13 +7,10 @@ #include #include -void printHexData(const std::string &data) -{ - if (!data.empty()) - { +void printHexData(const std::string &data) { + if (!data.empty()) { std::cout << "data received: "; - for (int i = 0; i < data.size(); ++i) - { + for (int i = 0; i < data.size(); ++i) { std::cout << std::hex << std::setfill('0') << std::setw(2) << int(uint8_t(data.at(i))) << " "; } @@ -21,8 +18,7 @@ void printHexData(const std::string &data) } } -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "linktrack_parser"); ros::NodeHandle nh; serial::Serial serial; @@ -30,14 +26,12 @@ int main(int argc, char **argv) NProtocolExtracter protocol_extraction; linktrack::Init init(&protocol_extraction, &serial); ros::Rate loop_rate(1000); - while (ros::ok()) - { + while (ros::ok()) { auto available_bytes = serial.available(); std::string str_received; - if (available_bytes) - { + if (available_bytes) { serial.read(str_received, available_bytes); - //printHexData(str_received); + // 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 734a6b0..b9cd924 100644 --- a/src/linktrack/main_rviz_converter.cpp +++ b/src/linktrack/main_rviz_converter.cpp @@ -12,28 +12,23 @@ #include "nutils.h" -namespace -{ - std::string frameId; - - struct PosePair - { - ros::Publisher publisher; - geometry_msgs::PoseStamped msg; - inline void publish() { publisher.publish(msg); } - }; +namespace { +std::string frameId; + +struct PosePair { + ros::Publisher publisher; + geometry_msgs::PoseStamped msg; + inline void publish() { publisher.publish(msg); } +}; } // namespace -void Anchorframe0Callback(const nlink_parser::LinktrackAnchorframe0 &msg) -{ +void Anchorframe0Callback(const nlink_parser::LinktrackAnchorframe0 &msg) { static ros::Publisher publisher; static std::map poses; - for (const auto &node : msg.nodes) - { + for (const auto &node : msg.nodes) { auto id = node.id; - if (!poses.count(id)) - { + if (!poses.count(id)) { std::ostringstream string_stream; string_stream << "nlt_anchorframe0_pose_node" << static_cast(id); auto topic = string_stream.str(); @@ -57,16 +52,13 @@ void Anchorframe0Callback(const nlink_parser::LinktrackAnchorframe0 &msg) } } -void Nodeframe1Callback(const nlink_parser::LinktrackNodeframe1 &msg) -{ +void Nodeframe1Callback(const nlink_parser::LinktrackNodeframe1 &msg) { static ros::Publisher publisher; static std::map poses; - for (const auto &node : msg.nodes) - { + for (const auto &node : msg.nodes) { auto id = node.id; - if (!poses.count(id)) - { + if (!poses.count(id)) { std::ostringstream string_stream; string_stream << "nlt_nodeframe1_pose_node" << static_cast(id); auto topic = string_stream.str(); @@ -90,11 +82,9 @@ void Nodeframe1Callback(const nlink_parser::LinktrackNodeframe1 &msg) } } -void Tagframe0Callback(const nlink_parser::LinktrackTagframe0 &msg) -{ +void Tagframe0Callback(const nlink_parser::LinktrackTagframe0 &msg) { static PosePair *pose = nullptr; - if (!pose) - { + if (!pose) { pose = new PosePair; auto topic = "nlt_tagframe0_pose"; pose->publisher = @@ -115,11 +105,9 @@ void Tagframe0Callback(const nlink_parser::LinktrackTagframe0 &msg) pose->publish(); } -void Nodeframe2Callback(const nlink_parser::LinktrackNodeframe2 &msg) -{ +void Nodeframe2Callback(const nlink_parser::LinktrackNodeframe2 &msg) { static PosePair *pose = nullptr; - if (!pose) - { + if (!pose) { pose = new PosePair; auto topic = "nlt_nodeframe2_pose"; pose->publisher = @@ -140,8 +128,7 @@ void Nodeframe2Callback(const nlink_parser::LinktrackNodeframe2 &msg) pose->publish(); } -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "linktrack_example"); ros::NodeHandle nh; diff --git a/src/linktrack/protocols.cpp b/src/linktrack/protocols.cpp index 1ada1b1..dd0b5ea 100644 --- a/src/linktrack/protocols.cpp +++ b/src/linktrack/protocols.cpp @@ -4,112 +4,84 @@ NLT_ProtocolAnchorFrame0::NLT_ProtocolAnchorFrame0() : NLinkProtocol( true, nlt_anchorframe0_.fixed_part_size, {nlt_anchorframe0_.frame_header, nlt_anchorframe0_.function_mark}, - {nlt_anchorframe0_.tail_check}) -{ -} + {nlt_anchorframe0_.tail_check}) {} -void NLT_ProtocolAnchorFrame0::UnpackFrameData(const uint8_t *data) -{ +void NLT_ProtocolAnchorFrame0::UnpackFrameData(const uint8_t *data) { nlt_anchorframe0_.UnpackData(data, length()); } -bool NLT_ProtocolAnchorFrame0::Verify(const uint8_t *data) -{ +bool NLT_ProtocolAnchorFrame0::Verify(const uint8_t *data) { return data[length() - 1] == nlt_anchorframe0_.tail_check; } NLT_ProtocolTagFrame0::NLT_ProtocolTagFrame0() : NLinkProtocol( true, g_nlt_tagframe0.fixed_part_size, - {g_nlt_tagframe0.frame_header, g_nlt_tagframe0.function_mark}) -{ -} + {g_nlt_tagframe0.frame_header, g_nlt_tagframe0.function_mark}) {} -void NLT_ProtocolTagFrame0::UnpackFrameData(const uint8_t *data) -{ +void NLT_ProtocolTagFrame0::UnpackFrameData(const uint8_t *data) { g_nlt_tagframe0.UnpackData(data, length()); } NLT_ProtocolNodeFrame0::NLT_ProtocolNodeFrame0() : NLinkProtocolVLength( true, g_nlt_nodeframe0.fixed_part_size, - {g_nlt_nodeframe0.frame_header, g_nlt_nodeframe0.function_mark}) -{ -} + {g_nlt_nodeframe0.frame_header, g_nlt_nodeframe0.function_mark}) {} -void NLT_ProtocolNodeFrame0::UnpackFrameData(const uint8_t *data) -{ +void NLT_ProtocolNodeFrame0::UnpackFrameData(const uint8_t *data) { g_nlt_nodeframe0.UnpackData(data, length()); } NLT_ProtocolNodeFrame1::NLT_ProtocolNodeFrame1() : NLinkProtocolVLength( true, g_nlt_nodeframe1.fixed_part_size, - {g_nlt_nodeframe1.frame_header, g_nlt_nodeframe1.function_mark}) -{ -} + {g_nlt_nodeframe1.frame_header, g_nlt_nodeframe1.function_mark}) {} -void NLT_ProtocolNodeFrame1::UnpackFrameData(const uint8_t *data) -{ +void NLT_ProtocolNodeFrame1::UnpackFrameData(const uint8_t *data) { g_nlt_nodeframe1.UnpackData(data, length()); } NLT_ProtocolNodeFrame2::NLT_ProtocolNodeFrame2() : NLinkProtocolVLength( true, g_nlt_nodeframe2.fixed_part_size, - {g_nlt_nodeframe2.frame_header, g_nlt_nodeframe2.function_mark}) -{ -} + {g_nlt_nodeframe2.frame_header, g_nlt_nodeframe2.function_mark}) {} -void NLT_ProtocolNodeFrame2::UnpackFrameData(const uint8_t *data) -{ +void NLT_ProtocolNodeFrame2::UnpackFrameData(const uint8_t *data) { g_nlt_nodeframe2.UnpackData(data, length()); } NLT_ProtocolNodeFrame3::NLT_ProtocolNodeFrame3() : NLinkProtocolVLength( true, g_nlt_nodeframe3.fixed_part_size, - {g_nlt_nodeframe3.frame_header, g_nlt_nodeframe3.function_mark}) -{ -} + {g_nlt_nodeframe3.frame_header, g_nlt_nodeframe3.function_mark}) {} -void NLT_ProtocolNodeFrame3::UnpackFrameData(const uint8_t *data) -{ +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}) -{ -} + {g_nlt_nodeframe4.frame_header, g_nlt_nodeframe4.function_mark}) {} -void NLT_ProtocolNodeFrame4::UnpackFrameData(const uint8_t *data) -{ +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}) -{ -} + {g_nlt_nodeframe5.frame_header, g_nlt_nodeframe5.function_mark}) {} -void NLT_ProtocolNodeFrame5::UnpackFrameData(const uint8_t *data) -{ +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}) -{ -} + {g_nlt_nodeframe6.frame_header, g_nlt_nodeframe6.function_mark}) {} -void NLT_ProtocolNodeFrame6::UnpackFrameData(const uint8_t *data) -{ +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 2597472..0775caa 100644 --- a/src/linktrack/protocols.h +++ b/src/linktrack/protocols.h @@ -12,8 +12,7 @@ #include "nlink_unpack/nlink_linktrack_nodeframe6.h" #include "nlink_unpack/nlink_linktrack_tagframe0.h" -class NLT_ProtocolAnchorFrame0 : public NLinkProtocol -{ +class NLT_ProtocolAnchorFrame0 : public NLinkProtocol { public: NLT_ProtocolAnchorFrame0(); @@ -22,8 +21,7 @@ class NLT_ProtocolAnchorFrame0 : public NLinkProtocol bool Verify(const uint8_t *data) override; }; -class NLT_ProtocolTagFrame0 : public NLinkProtocol -{ +class NLT_ProtocolTagFrame0 : public NLinkProtocol { public: NLT_ProtocolTagFrame0(); @@ -31,8 +29,7 @@ class NLT_ProtocolTagFrame0 : public NLinkProtocol void UnpackFrameData(const uint8_t *data) override; }; -class NLT_ProtocolNodeFrame0 : public NLinkProtocolVLength -{ +class NLT_ProtocolNodeFrame0 : public NLinkProtocolVLength { public: NLT_ProtocolNodeFrame0(); @@ -40,8 +37,7 @@ class NLT_ProtocolNodeFrame0 : public NLinkProtocolVLength void UnpackFrameData(const uint8_t *data) override; }; -class NLT_ProtocolNodeFrame1 : public NLinkProtocolVLength -{ +class NLT_ProtocolNodeFrame1 : public NLinkProtocolVLength { public: NLT_ProtocolNodeFrame1(); @@ -49,8 +45,7 @@ class NLT_ProtocolNodeFrame1 : public NLinkProtocolVLength void UnpackFrameData(const uint8_t *data) override; }; -class NLT_ProtocolNodeFrame2 : public NLinkProtocolVLength -{ +class NLT_ProtocolNodeFrame2 : public NLinkProtocolVLength { public: NLT_ProtocolNodeFrame2(); @@ -58,8 +53,7 @@ class NLT_ProtocolNodeFrame2 : public NLinkProtocolVLength void UnpackFrameData(const uint8_t *data) override; }; -class NLT_ProtocolNodeFrame3 : public NLinkProtocolVLength -{ +class NLT_ProtocolNodeFrame3 : public NLinkProtocolVLength { public: NLT_ProtocolNodeFrame3(); @@ -67,8 +61,7 @@ class NLT_ProtocolNodeFrame3 : public NLinkProtocolVLength void UnpackFrameData(const uint8_t *data) override; }; -class NLT_ProtocolNodeFrame4 : public NLinkProtocolVLength -{ +class NLT_ProtocolNodeFrame4 : public NLinkProtocolVLength { public: NLT_ProtocolNodeFrame4(); @@ -76,8 +69,7 @@ class NLT_ProtocolNodeFrame4 : public NLinkProtocolVLength void UnpackFrameData(const uint8_t *data) override; }; -class NLT_ProtocolNodeFrame5 : public NLinkProtocolVLength -{ +class NLT_ProtocolNodeFrame5 : public NLinkProtocolVLength { public: NLT_ProtocolNodeFrame5(); @@ -85,8 +77,7 @@ class NLT_ProtocolNodeFrame5 : public NLinkProtocolVLength void UnpackFrameData(const uint8_t *data) override; }; -class NLT_ProtocolNodeFrame6 : public NLinkProtocolVLength -{ +class NLT_ProtocolNodeFrame6 : public NLinkProtocolVLength { public: NLT_ProtocolNodeFrame6(); diff --git a/src/linktrack_aoa/init.cpp b/src/linktrack_aoa/init.cpp index c891acd..6cf3bc1 100644 --- a/src/linktrack_aoa/init.cpp +++ b/src/linktrack_aoa/init.cpp @@ -8,8 +8,7 @@ #include "nlink_unpack/nlink_linktrack_nodeframe0.h" #include "nutils.h" -class NLTAoa_ProtocolNodeFrame0 : public NLinkProtocolVLength -{ +class NLTAoa_ProtocolNodeFrame0 : public NLinkProtocolVLength { public: NLTAoa_ProtocolNodeFrame0(); @@ -18,117 +17,103 @@ class NLTAoa_ProtocolNodeFrame0 : public NLinkProtocolVLength }; NLTAoa_ProtocolNodeFrame0::NLTAoa_ProtocolNodeFrame0() - : NLinkProtocolVLength( - true, g_nltaoa_nodeframe0.fixed_part_size, - {g_nltaoa_nodeframe0.frame_header, g_nltaoa_nodeframe0.function_mark}) -{ -} + : NLinkProtocolVLength(true, g_nltaoa_nodeframe0.fixed_part_size, + {g_nltaoa_nodeframe0.frame_header, + g_nltaoa_nodeframe0.function_mark}) {} -void NLTAoa_ProtocolNodeFrame0::UnpackFrameData(const uint8_t *data) -{ +void NLTAoa_ProtocolNodeFrame0::UnpackFrameData(const uint8_t *data) { g_nltaoa_nodeframe0.UnpackData(data, length()); } -namespace linktrack_aoa -{ - - nlink_parser::LinktrackNodeframe0 g_msg_nodeframe0; - nlink_parser::LinktrackAoaNodeframe0 g_msg_aoa_nodeframe0; - - static serial::Serial *g_serial; - - Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) - { - g_serial = serial; - initDataTransmission(); - initNodeFrame0(protocol_extraction); - InitAoaNodeFrame0(protocol_extraction); - } - - static void DTCallback(const std_msgs::String::ConstPtr &msg) - { - if (g_serial) - g_serial->write(msg->data); - } - - void Init::initDataTransmission() - { - dt_sub_ = - nh_.subscribe("nlink_linktrack_data_transmission", 1000, DTCallback); - } - - void Init::initNodeFrame0(NProtocolExtracter *protocol_extraction) - { - auto protocol = new NLT_ProtocolNodeFrame0; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_nodeframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - ; - } - const auto &data = g_nlt_nodeframe0.result; - auto &msg_data = g_msg_nodeframe0; - 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); - }); - } - - void Init::InitAoaNodeFrame0(NProtocolExtracter *protocol_extraction) - { - auto protocol = new NLTAoa_ProtocolNodeFrame0; - protocol_extraction->AddProtocol(protocol); - protocol->SetHandleDataCallback([=] { - if (!publishers_[protocol]) - { - auto topic = "nlink_linktrack_aoa_nodeframe0"; - publishers_[protocol] = - nh_.advertise(topic, 200); - TopicAdvertisedTip(topic); - } - const auto &data = g_nltaoa_nodeframe0.result; - auto &msg_data = g_msg_aoa_nodeframe0; - 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.angle = node->angle; - msg_node.fp_rssi = node->fp_rssi; - msg_node.rx_rssi = node->rx_rssi; - } - - publishers_.at(protocol).publish(msg_data); - }); - } +namespace linktrack_aoa { +nlink_parser::LinktrackNodeframe0 g_msg_nodeframe0; +nlink_parser::LinktrackAoaNodeframe0 g_msg_aoa_nodeframe0; + +static serial::Serial *g_serial; + +Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) { + g_serial = serial; + initDataTransmission(); + initNodeFrame0(protocol_extraction); + InitAoaNodeFrame0(protocol_extraction); +} + +static void DTCallback(const std_msgs::String::ConstPtr &msg) { + if (g_serial) + g_serial->write(msg->data); +} + +void Init::initDataTransmission() { + dt_sub_ = + nh_.subscribe("nlink_linktrack_data_transmission", 1000, DTCallback); +} + +void Init::initNodeFrame0(NProtocolExtracter *protocol_extraction) { + auto protocol = new NLT_ProtocolNodeFrame0; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + auto topic = "nlink_linktrack_nodeframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + ; + } + const auto &data = g_nlt_nodeframe0.result; + auto &msg_data = g_msg_nodeframe0; + 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); + }); +} + +void Init::InitAoaNodeFrame0(NProtocolExtracter *protocol_extraction) { + auto protocol = new NLTAoa_ProtocolNodeFrame0; + protocol_extraction->AddProtocol(protocol); + protocol->SetHandleDataCallback([=] { + if (!publishers_[protocol]) { + auto topic = "nlink_linktrack_aoa_nodeframe0"; + publishers_[protocol] = + nh_.advertise(topic, 200); + TopicAdvertisedTip(topic); + } + const auto &data = g_nltaoa_nodeframe0.result; + auto &msg_data = g_msg_aoa_nodeframe0; + 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.angle = node->angle; + msg_node.fp_rssi = node->fp_rssi; + msg_node.rx_rssi = node->rx_rssi; + } + + publishers_.at(protocol).publish(msg_data); + }); +} } // namespace linktrack_aoa diff --git a/src/linktrack_aoa/init.h b/src/linktrack_aoa/init.h index d7cc055..5db46fa 100644 --- a/src/linktrack_aoa/init.h +++ b/src/linktrack_aoa/init.h @@ -10,22 +10,20 @@ #include "protocol_extracter/nprotocol_extracter.h" -namespace linktrack_aoa -{ - class Init - { - public: - explicit Init(NProtocolExtracter *protocol_extraction, - serial::Serial *serial); +namespace linktrack_aoa { +class Init { +public: + explicit Init(NProtocolExtracter *protocol_extraction, + serial::Serial *serial); - private: - void initDataTransmission(); - void initNodeFrame0(NProtocolExtracter *protocol_extraction); - void InitAoaNodeFrame0(NProtocolExtracter *protocol_extraction); - std::unordered_map publishers_; - ros::NodeHandle nh_; - ros::Subscriber dt_sub_; - }; +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 #endif // LINKTRACKAOAINIT_H diff --git a/src/linktrack_aoa/main.cpp b/src/linktrack_aoa/main.cpp index dac4df0..0957ab1 100644 --- a/src/linktrack_aoa/main.cpp +++ b/src/linktrack_aoa/main.cpp @@ -3,8 +3,7 @@ #include "init.h" #include "init_serial.h" -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "linktrack_aoa"); ros::NodeHandle nh; serial::Serial serial; @@ -12,12 +11,10 @@ int main(int argc, char **argv) NProtocolExtracter protocol_extraction; linktrack_aoa::Init aoaInit(&protocol_extraction, &serial); ros::Rate loop_rate(1000); - while (ros::ok()) - { + while (ros::ok()) { auto available_bytes = serial.available(); std::string str_received; - if (available_bytes) - { + if (available_bytes) { serial.read(str_received, available_bytes); protocol_extraction.AddNewData(str_received); } diff --git a/src/tofsense/init.cpp b/src/tofsense/init.cpp index 4e40a54..969fb9e 100644 --- a/src/tofsense/init.cpp +++ b/src/tofsense/init.cpp @@ -6,8 +6,7 @@ #include "nlink_unpack/nlink_utils.h" #include "nutils.h" -class NTS_ProtocolFrame0 : public NLinkProtocol -{ +class NTS_ProtocolFrame0 : public NLinkProtocol { public: NTS_ProtocolFrame0(); @@ -17,123 +16,99 @@ class NTS_ProtocolFrame0 : public NLinkProtocol NTS_ProtocolFrame0::NTS_ProtocolFrame0() : NLinkProtocol(true, g_nts_frame0.fixed_part_size, - {g_nts_frame0.frame_header, g_nts_frame0.function_mark}) -{ -} + {g_nts_frame0.frame_header, g_nts_frame0.function_mark}) {} -void NTS_ProtocolFrame0::UnpackFrameData(const uint8_t *data) -{ +void NTS_ProtocolFrame0::UnpackFrameData(const uint8_t *data) { g_nts_frame0.UnpackData(data, length()); } -namespace tofsense -{ - nlink_parser::TofsenseFrame0 g_msg_frame0; +namespace tofsense { +nlink_parser::TofsenseFrame0 g_msg_frame0; #pragma pack(push, 1) - struct - { - char header[2]{0x57, 0x10}; - uint8_t reserved0[2]{0xff, 0xff}; - uint8_t id{}; - uint8_t reserved1[2]{0xff, 0xff}; - uint8_t checkSum{}; - } g_command_read; +struct { + char header[2]{0x57, 0x10}; + uint8_t reserved0[2]{0xff, 0xff}; + uint8_t id{}; + uint8_t reserved1[2]{0xff, 0xff}; + uint8_t checkSum{}; +} g_command_read; #pragma pack(pop) - Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) - : serial_(serial) - { - is_inquire_mode_ = - serial_ ? ros::param::param("~inquire_mode", true) : false; +Init::Init(NProtocolExtracter *protocol_extraction, serial::Serial *serial) + : serial_(serial) { + is_inquire_mode_ = + serial_ ? ros::param::param("~inquire_mode", true) : false; - InitFrame0(protocol_extraction); - } + InitFrame0(protocol_extraction); +} - void Init::InitFrame0(NProtocolExtracter *protocol_extraction) - { - static auto protocol_frame0_ = new NTS_ProtocolFrame0; - protocol_extraction->AddProtocol(protocol_frame0_); - protocol_frame0_->SetHandleDataCallback( - [=] - { - if (!publishers_[protocol_frame0_]) - { - ros::NodeHandle nh_; - if (is_inquire_mode_) - { - auto topic = "nlink_tofsense_cascade"; - publishers_[protocol_frame0_] = - nh_.advertise(topic, 50); - TopicAdvertisedTip(topic); - } - else - { - auto topic = "nlink_tofsense_frame0"; - publishers_[protocol_frame0_] = - nh_.advertise(topic, 50); - TopicAdvertisedTip(topic); - } - } +void Init::InitFrame0(NProtocolExtracter *protocol_extraction) { + static auto protocol_frame0_ = new NTS_ProtocolFrame0; + protocol_extraction->AddProtocol(protocol_frame0_); + protocol_frame0_->SetHandleDataCallback([=] { + if (!publishers_[protocol_frame0_]) { + ros::NodeHandle nh_; + if (is_inquire_mode_) { + auto topic = "nlink_tofsense_cascade"; + publishers_[protocol_frame0_] = + nh_.advertise(topic, 50); + TopicAdvertisedTip(topic); + } else { + auto topic = "nlink_tofsense_frame0"; + publishers_[protocol_frame0_] = + nh_.advertise(topic, 50); + TopicAdvertisedTip(topic); + } + } - const auto &data = g_nts_frame0.result; + const auto &data = g_nts_frame0.result; - g_msg_frame0.id = data.id; - g_msg_frame0.system_time = data.system_time; - 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; + g_msg_frame0.id = data.id; + g_msg_frame0.system_time = data.system_time; + 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; - } - else - { - publishers_.at(protocol_frame0_).publish(g_msg_frame0); - } - }); - - if (is_inquire_mode_) - { - timer_scan_ = nh_.createTimer( - ros::Duration(1.0 / frequency_), - [=](const ros::TimerEvent &) - { - frame0_map_.clear(); - node_index_ = 0; - timer_read_.start(); - }, - false, true); - timer_read_ = nh_.createTimer( - ros::Duration(0.006), - [=](const ros::TimerEvent &) - { - if (node_index_ >= 8) - { - if (!frame0_map_.empty()) - { - nlink_parser::TofsenseCascade msg_cascade; - for (const auto &msg : frame0_map_) - { - msg_cascade.nodes.push_back(msg.second); - } - publishers_.at(protocol_frame0_).publish(msg_cascade); + if (is_inquire_mode_) { + frame0_map_[data.id] = g_msg_frame0; + } else { + publishers_.at(protocol_frame0_).publish(g_msg_frame0); + } + }); + + if (is_inquire_mode_) { + timer_scan_ = nh_.createTimer( + ros::Duration(1.0 / frequency_), + [=](const ros::TimerEvent &) { + frame0_map_.clear(); + node_index_ = 0; + timer_read_.start(); + }, + false, true); + timer_read_ = nh_.createTimer( + ros::Duration(0.006), + [=](const ros::TimerEvent &) { + if (node_index_ >= 8) { + if (!frame0_map_.empty()) { + nlink_parser::TofsenseCascade msg_cascade; + for (const auto &msg : frame0_map_) { + msg_cascade.nodes.push_back(msg.second); } - timer_read_.stop(); - } - else - { - g_command_read.id = node_index_; - auto data = reinterpret_cast(&g_command_read); - NLink_UpdateCheckSum(data, sizeof(g_command_read)); - serial_->write(data, sizeof(g_command_read)); - ++node_index_; + publishers_.at(protocol_frame0_).publish(msg_cascade); } - }, - false, false); - } + timer_read_.stop(); + } else { + g_command_read.id = node_index_; + auto data = reinterpret_cast(&g_command_read); + NLink_UpdateCheckSum(data, sizeof(g_command_read)); + serial_->write(data, sizeof(g_command_read)); + ++node_index_; + } + }, + false, false); } +} } // namespace tofsense diff --git a/src/tofsense/init.h b/src/tofsense/init.h index 9192fa0..a7ba9ae 100644 --- a/src/tofsense/init.h +++ b/src/tofsense/init.h @@ -11,31 +11,29 @@ #include "protocol_extracter/nprotocol_extracter.h" -namespace tofsense -{ - class Init - { - public: - explicit Init(NProtocolExtracter *protocol_extraction, - serial::Serial *serial); +namespace tofsense { +class Init { +public: + explicit Init(NProtocolExtracter *protocol_extraction, + serial::Serial *serial); - private: - void InitFrame0(NProtocolExtracter *protocol_extraction); +private: + void InitFrame0(NProtocolExtracter *protocol_extraction); - std::unordered_map publishers_; + std::unordered_map publishers_; - std::map frame0_map_; + std::map frame0_map_; - serial::Serial *serial_; + serial::Serial *serial_; - const int frequency_ = 10; - bool is_inquire_mode_ = true; + const int frequency_ = 10; + bool is_inquire_mode_ = true; - ros::NodeHandle nh_; - ros::Timer timer_scan_; - ros::Timer timer_read_; - uint8_t node_index_ = 0; - }; + ros::NodeHandle nh_; + ros::Timer timer_scan_; + ros::Timer timer_read_; + uint8_t node_index_ = 0; +}; } // namespace tofsense #endif // TOFSENSEINIT_H diff --git a/src/tofsense/main.cpp b/src/tofsense/main.cpp index 3ee085e..5a22f75 100644 --- a/src/tofsense/main.cpp +++ b/src/tofsense/main.cpp @@ -4,8 +4,7 @@ #include "init_serial.h" #include "protocol_extracter/nprotocol_extracter.h" -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "tofsense_parser"); ros::NodeHandle nh; serial::Serial serial; @@ -14,12 +13,10 @@ int main(int argc, char **argv) NProtocolExtracter extracter; tofsense::Init init(&extracter, &serial); - while (ros::ok()) - { + while (ros::ok()) { auto available_bytes = serial.available(); std::string str_received; - if (available_bytes) - { + if (available_bytes) { serial.read(str_received, available_bytes); extracter.AddNewData(str_received); } diff --git a/src/tofsensem/init.cpp b/src/tofsensem/init.cpp index a65c14c..c00273c 100644 --- a/src/tofsensem/init.cpp +++ b/src/tofsensem/init.cpp @@ -6,71 +6,58 @@ #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); +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 - 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); - } +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); - }); - } + 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 index cc22c62..ab47578 100644 --- a/src/tofsensem/init.h +++ b/src/tofsensem/init.h @@ -6,18 +6,16 @@ #include #include -namespace tofsensem -{ - class Init - { - public: - explicit Init(NProtocolExtracter *protocol_extraction); +namespace tofsensem { +class Init { +public: + explicit Init(NProtocolExtracter *protocol_extraction); - private: - void InitFrame0(NProtocolExtracter *protocol_extraction); - std::unordered_map publishers_; - ros::NodeHandle nh_; - }; +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 index 10b0bca..64073c1 100644 --- a/src/tofsensem/main.cpp +++ b/src/tofsensem/main.cpp @@ -8,8 +8,7 @@ #include #include -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "tofsensem_parser"); ros::NodeHandle nh; serial::Serial serial; @@ -18,17 +17,13 @@ int main(int argc, char **argv) NProtocolExtracter extracter; tofsensem::Init init(&extracter); - while (ros::ok()) - { + while (ros::ok()) { auto available_bytes = serial.available(); std::string str_received; - if (available_bytes) - { + if (available_bytes) { serial.read(str_received, available_bytes); extracter.AddNewData(str_received); - } - else - { + } else { std::this_thread::sleep_for(std::chrono::milliseconds(10)); } ros::spinOnce(); diff --git a/src/utils/init_serial.cpp b/src/utils/init_serial.cpp index b37cdf5..44478bd 100644 --- a/src/utils/init_serial.cpp +++ b/src/utils/init_serial.cpp @@ -18,10 +18,8 @@ void enumerate_ports() { } */ -void initSerial(serial::Serial *serial) -{ - try - { +void initSerial(serial::Serial *serial) { + try { auto port_name = ros::param::param("~port_name", "/dev/ttyUSB0"); auto baud_rate = ros::param::param("~baud_rate", 921600); @@ -35,18 +33,13 @@ void initSerial(serial::Serial *serial) serial->setTimeout(timeout); serial->open(); - if (serial->isOpen()) - { + if (serial->isOpen()) { ROS_INFO("Serial port opened successfully, waiting for data."); - } - else - { + } else { ROS_ERROR("Failed to open serial port, please check and retry."); exit(EXIT_FAILURE); } - } - catch (const std::exception &e) - { + } catch (const std::exception &e) { ROS_ERROR("Unhandled Exception: %s", e.what()); exit(EXIT_FAILURE); } diff --git a/src/utils/nlink_protocol.cpp b/src/utils/nlink_protocol.cpp index 4a9dc7c..f5ab6b5 100644 --- a/src/utils/nlink_protocol.cpp +++ b/src/utils/nlink_protocol.cpp @@ -5,24 +5,21 @@ #include #include -void NLinkProtocol::HandleData(const uint8_t *data) -{ +void NLinkProtocol::HandleData(const uint8_t *data) { UnpackFrameData(data); assert(HandleDataCallback_); HandleDataCallback_(); } -bool NLinkProtocol::Verify(const uint8_t *data) -{ +bool NLinkProtocol::Verify(const uint8_t *data) { uint8_t sum = 0; return data[length() - 1] == std::accumulate(data, data + length() - sizeof(sum), sum); } bool NLinkProtocolVLength::UpdateLength(const uint8_t *data, - size_t available_bytes) -{ + size_t available_bytes) { 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 a6a099a..4c6ede0 100644 --- a/src/utils/nlink_protocol.h +++ b/src/utils/nlink_protocol.h @@ -4,13 +4,11 @@ #include "protocol_extracter/nprotocol_base.h" -class NLinkProtocol : public NProtocolBase -{ +class NLinkProtocol : public NProtocolBase { public: using NProtocolBase::NProtocolBase; - void SetHandleDataCallback(std::function handle) - { + void SetHandleDataCallback(std::function handle) { HandleDataCallback_ = handle; } @@ -23,8 +21,7 @@ class NLinkProtocol : public NProtocolBase std::function HandleDataCallback_; }; -class NLinkProtocolVLength : public NLinkProtocol -{ +class NLinkProtocolVLength : public NLinkProtocol { public: using NLinkProtocol::NLinkProtocol; diff --git a/src/utils/nlink_unpack b/src/utils/nlink_unpack index 2e9ef3e..05acaf9 160000 --- a/src/utils/nlink_unpack +++ b/src/utils/nlink_unpack @@ -1 +1 @@ -Subproject commit 2e9ef3e937b013978925678ac3f508db38bf3eb6 +Subproject commit 05acaf97b8cd3998e4bd6a1acbac0177b4e0c099 diff --git a/src/utils/nutils.cpp b/src/utils/nutils.cpp index ebaebe4..d2163b0 100644 --- a/src/utils/nutils.cpp +++ b/src/utils/nutils.cpp @@ -2,8 +2,7 @@ #include -void TopicAdvertisedTip(const char *topic) -{ +void TopicAdvertisedTip(const char *topic) { ROS_INFO("%s has been advertised,use 'rostopic " "echo /%s' to view the data", topic, topic); diff --git a/src/utils/protocol_extracter b/src/utils/protocol_extracter index 5134cf7..a19abb7 160000 --- a/src/utils/protocol_extracter +++ b/src/utils/protocol_extracter @@ -1 +1 @@ -Subproject commit 5134cf7acf46d0b66eb53f9eab398e3d24516fbf +Subproject commit a19abb76df85fa3e94f245089b258a4f36a7c404 diff --git a/test/test_nlink_parser.cpp b/test/test_nlink_parser.cpp index 7bc5b17..6e7c25a 100644 --- a/test/test_nlink_parser.cpp +++ b/test/test_nlink_parser.cpp @@ -26,27 +26,24 @@ static const double kAbsError = 0.001; { \ const auto &ACTUAL = SRC; \ const auto &EXPECTED = std::vector DEST; \ - for (size_t _INDEX = 0; _INDEX < EXPECTED.size(); ++_INDEX) \ - { \ + for (size_t _INDEX = 0; _INDEX < EXPECTED.size(); ++_INDEX) { \ EXPECT_NEAR(EXPECTED.at(_INDEX), ACTUAL.at(_INDEX), kAbsError); \ } \ } -namespace linktrack -{ - extern nlink_parser::LinktrackAnchorframe0 g_msg_anchorframe0; - extern nlink_parser::LinktrackTagframe0 g_msg_tagframe0; - 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; - extern nlink_parser::LinktrackNodeframe4 g_msg_nodeframe4; - extern nlink_parser::LinktrackNodeframe5 g_msg_nodeframe5; - extern nlink_parser::LinktrackNodeframe6 g_msg_nodeframe6; +namespace linktrack { +extern nlink_parser::LinktrackAnchorframe0 g_msg_anchorframe0; +extern nlink_parser::LinktrackTagframe0 g_msg_tagframe0; +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; +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) -{ +TEST(NLinkParser, linktrack) { NProtocolExtracter protocol_extraction; linktrack::Init init(&protocol_extraction, nullptr); uint8_t data[1024]; @@ -293,8 +290,7 @@ TEST(NLinkParser, linktrack) {2, 2.107f}, {3, 1.762f}, }; - for (int i = 0; i < tag.anchors.size(); ++i) - { + 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); @@ -311,8 +307,7 @@ TEST(NLinkParser, linktrack) {2, 2.378f}, {3, 1.33f}, }; - for (int i = 0; i < tag.anchors.size(); ++i) - { + 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); @@ -357,13 +352,11 @@ TEST(NLinkParser, linktrack) } } -namespace tofsense -{ - extern nlink_parser::TofsenseFrame0 g_msg_frame0; +namespace tofsense { +extern nlink_parser::TofsenseFrame0 g_msg_frame0; } -TEST(NLinkParser, tofsense) -{ +TEST(NLinkParser, tofsense) { NProtocolExtracter protocol_extraction; tofsense::Init init(&protocol_extraction, nullptr); @@ -381,13 +374,11 @@ TEST(NLinkParser, tofsense) EXPECT_EQ(msg.range_precision, 255); } -namespace tofsensem -{ - extern nlink_parser::TofsenseMFrame0 g_msg_tofmframe0; +namespace tofsensem { +extern nlink_parser::TofsenseMFrame0 g_msg_tofmframe0; } -TEST(NLinkParser, tofsensem) -{ +TEST(NLinkParser, tofsensem) { NProtocolExtracter protocol_extraction; tofsensem::Init init(&protocol_extraction); @@ -436,8 +427,7 @@ TEST(NLinkParser, tofsensem) 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) - { + 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); @@ -445,13 +435,11 @@ TEST(NLinkParser, tofsensem) } } -namespace iot -{ - extern nlink_parser::IotFrame0 g_msg_iotframe0; +namespace iot { +extern nlink_parser::IotFrame0 g_msg_iotframe0; } -TEST(NLinkParser, iot) -{ +TEST(NLinkParser, iot) { NProtocolExtracter protocol_extraction; iot::Init init(&protocol_extraction); @@ -477,8 +465,7 @@ TEST(NLinkParser, iot) auto &msg = iot::g_msg_iotframe0; EXPECT_NEAR(msg.uid, next_val(), kAbsError); - for (int i = 0; i < msg.nodes.size(); ++i) - { + for (int i = 0; i < msg.nodes.size(); ++i) { const auto &node = msg.nodes.at(i); EXPECT_NEAR(node.uid, next_val(), kAbsError); EXPECT_NEAR(node.cnt, next_val(), kAbsError); @@ -488,13 +475,11 @@ TEST(NLinkParser, iot) } } -namespace linktrack_aoa -{ - extern nlink_parser::LinktrackAoaNodeframe0 g_msg_aoa_nodeframe0; +namespace linktrack_aoa { +extern nlink_parser::LinktrackAoaNodeframe0 g_msg_aoa_nodeframe0; } -TEST(nlink_parser, linktrack_aoa) -{ +TEST(nlink_parser, linktrack_aoa) { NProtocolExtracter protocol_extraction; linktrack_aoa::Init init(&protocol_extraction, nullptr); @@ -529,8 +514,7 @@ TEST(nlink_parser, linktrack_aoa) } // Run all the tests that were declared with TEST() -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_nlink_parser"); ros::NodeHandle nh; From 8477be644d05578ae4c51eece83880096f947786 Mon Sep 17 00:00:00 2001 From: samuelyhsu Date: Sat, 10 Jun 2023 17:22:28 +0800 Subject: [PATCH 12/15] - use origin serial repository --- .gitmodules | 2 +- CMakeLists.txt | 1 + extern/serial | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index ef61ac8..907d543 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,4 +6,4 @@ url = https://github.com/nooploop-dev/nlink_unpack.git [submodule "extern/serial"] path = extern/serial - url = https://github.com/nooploop-dev/serial.git + url = https://github.com/wjwwood/serial.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a08f0e..ec9923e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS ) add_subdirectory(extern/serial) +include_directories(extern/serial/include) # find_package(serial CONFIG REQUIRED) diff --git a/extern/serial b/extern/serial index fa1141a..69e0372 160000 --- a/extern/serial +++ b/extern/serial @@ -1 +1 @@ -Subproject commit fa1141a6ecbf4fd495560ad7acbaad844e158e41 +Subproject commit 69e0372cf0d3796e84ce9a09aff1d74496f68720 From c4ec0511d8a74deb9477d59410c06ce0fd7eae91 Mon Sep 17 00:00:00 2001 From: samuelyhsu Date: Sat, 10 Jun 2023 17:41:43 +0800 Subject: [PATCH 13/15] - fix full cpu usage --- src/iot/main.cpp | 5 ++--- src/tofsense/main.cpp | 3 ++- src/tofsensem/main.cpp | 3 ++- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/iot/main.cpp b/src/iot/main.cpp index d3b29f4..5f8ac65 100644 --- a/src/iot/main.cpp +++ b/src/iot/main.cpp @@ -14,17 +14,16 @@ int main(int argc, char **argv) { 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); - } else { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); } ros::spinOnce(); + loop_rate.sleep(); } return EXIT_SUCCESS; } 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/main.cpp b/src/tofsensem/main.cpp index 64073c1..d973d35 100644 --- a/src/tofsensem/main.cpp +++ b/src/tofsensem/main.cpp @@ -16,7 +16,7 @@ int main(int argc, char **argv) { NProtocolExtracter extracter; tofsensem::Init init(&extracter); - + ros::Rate loop_rate(1000); while (ros::ok()) { auto available_bytes = serial.available(); std::string str_received; @@ -27,6 +27,7 @@ int main(int argc, char **argv) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); } ros::spinOnce(); + loop_rate.sleep(); } return EXIT_SUCCESS; } From 7efe7ac02efe1c7d5a4e4ab4f2e79691e1efdf46 Mon Sep 17 00:00:00 2001 From: samuelyhsu Date: Thu, 17 Aug 2023 11:38:55 +0800 Subject: [PATCH 14/15] - update iot protocol --- msg/IotFrame0.msg | 2 ++ msg/IotFrame0Node.msg | 4 +++- src/iot/init.cpp | 25 +++++++++++++++--------- src/utils/nlink_unpack | 2 +- test/test_nlink_parser.cpp | 40 -------------------------------------- 5 files changed, 22 insertions(+), 51 deletions(-) diff --git a/msg/IotFrame0.msg b/msg/IotFrame0.msg index 19e3845..d0d6a5a 100644 --- a/msg/IotFrame0.msg +++ b/msg/IotFrame0.msg @@ -1,2 +1,4 @@ uint32 uid +uint32 system_time +uint8 io_status IotFrame0Node[] nodes diff --git a/msg/IotFrame0Node.msg b/msg/IotFrame0Node.msg index 1685aaf..73d14a7 100644 --- a/msg/IotFrame0Node.msg +++ b/msg/IotFrame0Node.msg @@ -1,5 +1,7 @@ uint32 uid -uint8 cnt float32 dis float32 aoa_angle_horizontal float32 aoa_angle_vertical +float32 fp_rssi +float32 rx_rssi +string user_data diff --git a/src/iot/init.cpp b/src/iot/init.cpp index 47bbf49..11037e9 100644 --- a/src/iot/init.cpp +++ b/src/iot/init.cpp @@ -5,6 +5,7 @@ #include "nlink_unpack/nlink_iot_frame0.h" #include "nlink_unpack/nlink_utils.h" #include "nutils.h" +#include namespace { class ProtocolFrame0 : public NLinkProtocol { @@ -42,15 +43,21 @@ void Init::InitFrame0(NProtocolExtracter *protocol_extraction) { const auto &data = g_iot_frame0; g_msg_iotframe0.uid = data.uid; - g_msg_iotframe0.nodes.resize(IOT_FRAME0_NODE_COUNT); - for (int i = 0; i < IOT_FRAME0_NODE_COUNT; ++i) { - g_msg_iotframe0.nodes[i].uid = data.nodes[i].uid; - g_msg_iotframe0.nodes[i].cnt = data.nodes[i].cnt; - g_msg_iotframe0.nodes[i].dis = data.nodes[i].dis; - g_msg_iotframe0.nodes[i].aoa_angle_horizontal = - data.nodes[i].aoa_angle_horizontal; - g_msg_iotframe0.nodes[i].aoa_angle_vertical = - data.nodes[i].aoa_angle_vertical; + 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); diff --git a/src/utils/nlink_unpack b/src/utils/nlink_unpack index 05acaf9..1b79045 160000 --- a/src/utils/nlink_unpack +++ b/src/utils/nlink_unpack @@ -1 +1 @@ -Subproject commit 05acaf97b8cd3998e4bd6a1acbac0177b4e0c099 +Subproject commit 1b790459c62bd7f15c18d98d9c87d13471f6db47 diff --git a/test/test_nlink_parser.cpp b/test/test_nlink_parser.cpp index 6e7c25a..fd1f6d2 100644 --- a/test/test_nlink_parser.cpp +++ b/test/test_nlink_parser.cpp @@ -435,46 +435,6 @@ TEST(NLinkParser, tofsensem) { } } -namespace iot { -extern nlink_parser::IotFrame0 g_msg_iotframe0; -} - -TEST(NLinkParser, iot) { - NProtocolExtracter protocol_extraction; - iot::Init init(&protocol_extraction); - - uint8_t data[1024]; - auto string = - "55 01 52 00 6b 00 4a 00 62 00 03 60 00 00 2b ef ff 00 00 00 47 00 48 00 " - "ac 52 00 00 6e da 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 " - "00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 " - "00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 " - "00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 " - "00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 " - "00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 10"; - std::vector vals{ - 7012434, 6422602, 3, 0.096, -4.309, 0, 4718663, 172, 0.082, 55.918, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0}; - 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 = iot::g_msg_iotframe0; - EXPECT_NEAR(msg.uid, next_val(), kAbsError); - for (int i = 0; i < msg.nodes.size(); ++i) { - const auto &node = msg.nodes.at(i); - EXPECT_NEAR(node.uid, next_val(), kAbsError); - EXPECT_NEAR(node.cnt, next_val(), kAbsError); - EXPECT_NEAR(node.dis, next_val(), kAbsError); - EXPECT_NEAR(node.aoa_angle_horizontal, next_val(), kAbsError); - EXPECT_NEAR(node.aoa_angle_vertical, next_val(), kAbsError); - } -} - namespace linktrack_aoa { extern nlink_parser::LinktrackAoaNodeframe0 g_msg_aoa_nodeframe0; } From 77a3238a5b73e7545a032570606492e2b7642b78 Mon Sep 17 00:00:00 2001 From: samuelyhsu Date: Wed, 27 Dec 2023 19:44:46 +0800 Subject: [PATCH 15/15] - fix iot::ProtocolFrame0 - add iot test --- src/iot/init.cpp | 8 ++++---- test/test_nlink_parser.cpp | 28 ++++++++++++++++++++++++++++ 2 files changed, 32 insertions(+), 4 deletions(-) diff --git a/src/iot/init.cpp b/src/iot/init.cpp index 11037e9..15f7656 100644 --- a/src/iot/init.cpp +++ b/src/iot/init.cpp @@ -8,12 +8,12 @@ #include namespace { -class ProtocolFrame0 : public NLinkProtocol { +class ProtocolFrame0 : public NLinkProtocolVLength { public: ProtocolFrame0() - : NLinkProtocol(true, g_iot_frame0.fixed_part_size, - {g_iot_frame0.frame_header, g_iot_frame0.function_mark}) { - } + : 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 { diff --git a/test/test_nlink_parser.cpp b/test/test_nlink_parser.cpp index fd1f6d2..3d55440 100644 --- a/test/test_nlink_parser.cpp +++ b/test/test_nlink_parser.cpp @@ -473,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);