From 7796833021cb63407c8230a4dc4cc948a0506d67 Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Thu, 21 Jul 2022 09:13:25 +0900 Subject: [PATCH] latency logging, etc. --- src/cpm_application.cpp | 65 ++++++++++++++++++++++++++++------------- src/raw_socket_link.cpp | 5 +++- src/v2x_node.cpp | 48 +++++++++++++++++++++++++----- 3 files changed, 90 insertions(+), 28 deletions(-) diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index a906ba9..f832387 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -47,7 +47,8 @@ namespace v2x { include_all_persons_and_animals_(false), cpm_num_(0), received_cpm_num_(0), - cpm_object_id_(0) + cpm_object_id_(0), + use_dynamic_generation_rules_(true) { RCLCPP_INFO(node_->get_logger(), "CpmApplication started. is_sender: %d", is_sender_); set_interval(milliseconds(100)); @@ -95,6 +96,12 @@ namespace v2x { asn1::Cpm message = *cpm; ItsPduHeader_t &header = message->header; + std::chrono::milliseconds ms = std::chrono::duration_cast ( + std::chrono::system_clock::now().time_since_epoch() + ); + node_->latency_log_file << "T_received," << header.stationID << "," << ms.count() << std::endl; + + // Calculate GDT and get GDT from CPM and calculate the "Age of CPM" GenerationDeltaTime_t gdt_cpm = message->cpm.generationDeltaTime; const auto time_now = duration_cast (runtime_.now().time_since_epoch()); @@ -169,7 +176,7 @@ namespace v2x { receivedObjectsStack.push_back(object); } - node_->publishObjects(&receivedObjectsStack); + node_->publishObjects(&receivedObjectsStack, header.stationID); } else { // RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC"); } @@ -243,12 +250,13 @@ namespace v2x { void CpmApplication::updateObjectsList(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { updating_objects_list_ = true; - // if (sending_) { - // RCLCPP_INFO(node_->get_logger(), "updateObjectsStack Skipped..."); - // return; - // } else { - // // objectsList.clear(); - // } + // Flag all objects as NOT_SEND + if (objectsList.size() > 0) { + for (auto& object : objectsList) { + object.to_send = false; + object.to_send_trigger = -1; + } + } if (msg->objects.size() > 0) { for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) { @@ -303,7 +311,7 @@ namespace v2x { continue; } - object.to_send = false; + object.to_send = true; object.to_send_trigger = 0; object.timestamp = runtime_.now(); @@ -400,6 +408,12 @@ namespace v2x { found_object->timestamp = runtime_.now(); + // if use_dymanic_generation_rules_ == false, then always include object in CPM + if (!use_dynamic_generation_rules_) { + found_object->to_send = true; + found_object->to_send_trigger = 0; + } + } } } @@ -432,7 +446,7 @@ namespace v2x { sending_ = true; - // printObjectsList(cpm_num_); + printObjectsList(cpm_num_); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM..."); @@ -442,7 +456,7 @@ namespace v2x { ItsPduHeader_t &header = message->header; header.protocolVersion = 1; header.messageID = 14; - header.stationID = 1; + header.stationID = cpm_num_; CollectivePerceptionMessage_t &cpm = message->cpm; @@ -539,7 +553,8 @@ namespace v2x { ASN_SEQUENCE_ADD(poc, pObj); - object.to_send = false; + // object.to_send = false; + // object.to_send_trigger = -1; // RCLCPP_INFO(node_->get_logger(), "Sending object: %s", object.uuid.c_str()); ++perceivedObjectsCount; @@ -550,18 +565,21 @@ namespace v2x { } } - cpm.cpmParameters.numberOfPerceivedObjects =perceivedObjectsCount; + cpm.cpmParameters.numberOfPerceivedObjects = perceivedObjectsCount; + + if (perceivedObjectsCount == 0) { + cpm.cpmParameters.perceivedObjectContainer = NULL; + } } else { cpm.cpmParameters.perceivedObjectContainer = NULL; - // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Empty POC"); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Empty POC"); } - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM with %ld objects", objectsList.size()); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM with %d objects", perceivedObjectsCount); insertCpmToCpmTable(message, (char*) "cpm_sent"); - Application::DownPacketPtr packet{new DownPacket()}; std::unique_ptr payload{new geonet::DownPacket()}; payload->layer(OsiLayer::Application) = std::move(message); @@ -578,9 +596,14 @@ namespace v2x { } sending_ = false; - rclcpp::Time current_time = node_->now(); + // rclcpp::Time current_time = node_->now(); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds()); + std::chrono::milliseconds ms = std::chrono::duration_cast ( + std::chrono::system_clock::now().time_since_epoch() + ); + node_->latency_log_file << "T_depart," << cpm_num_ << "," << ms.count() << std::endl; + ++cpm_num_; } } @@ -601,7 +624,7 @@ namespace v2x { ret = sqlite3_exec(db, sql_command, NULL, NULL, &err); if (ret != SQLITE_OK) { - RCLCPP_INFO(node_->get_logger(), "DB Execution Error"); + RCLCPP_INFO(node_->get_logger(), "DB Execution Error (create table cpm_sent)"); sqlite3_close(db); sqlite3_free(err); return; @@ -611,7 +634,7 @@ namespace v2x { ret = sqlite3_exec(db, sql_command, NULL, NULL, &err); if (ret != SQLITE_OK) { - RCLCPP_INFO(node_->get_logger(), "DB Execution Error"); + RCLCPP_INFO(node_->get_logger(), "DB Execution Error (create table cpm_received)"); sqlite3_close(db); sqlite3_free(err); return; @@ -644,7 +667,9 @@ namespace v2x { ret = sqlite3_exec(db, sql_command.str().c_str(), NULL, NULL, &err); if (ret != SQLITE_OK) { - RCLCPP_INFO(node_->get_logger(), "DB Execution Error"); + RCLCPP_INFO(node_->get_logger(), "DB Execution Error (insertCpmToCpmTable)"); + RCLCPP_INFO(node_->get_logger(), sql_command.str().c_str()); + RCLCPP_INFO(node_->get_logger(), err); sqlite3_close(db); sqlite3_free(err); return; diff --git a/src/raw_socket_link.cpp b/src/raw_socket_link.cpp index 802ae62..516ac81 100644 --- a/src/raw_socket_link.cpp +++ b/src/raw_socket_link.cpp @@ -2,6 +2,7 @@ #include #include #include +#include using namespace vanetza; @@ -33,7 +34,9 @@ std::size_t RawSocketLink::transmit(std::unique_ptr packet) return socket_.send(const_buffers); } -void RawSocketLink::indicate(IndicationCallback callback) { callback_ = callback; } +void RawSocketLink::indicate(IndicationCallback callback) { + callback_ = callback; +} void RawSocketLink::do_receive() { diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index 7b17740..416fff9 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -18,6 +18,8 @@ #include #include "tf2/LinearMath/Quaternion.h" +#include +#include namespace gn = vanetza::geonet; @@ -44,8 +46,32 @@ namespace v2x RCLCPP_INFO(get_logger(), "V2X Node Launched"); - rclcpp::Time current_time = this->now(); - RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] [measure] T_R1 %ld", current_time.nanoseconds()); + //rclcpp::Time current_time = this->now(); + //RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] [measure] T_R1 %ld", current_time.nanoseconds()); + + time_t t = time(nullptr); + const tm* lt = localtime(&t); + std::stringstream s; + s<<"20"; + s<tm_year-100; //100を引くことで20xxのxxの部分になる + s<<"-"; + s<tm_mon+1; //月を0からカウントしているため + s<<"-"; + s<tm_mday; //そのまま + s<<"_"; + s<tm_hour; + s<<":"; + s<tm_min; + s<<":"; + s<tm_sec; + std::string timestamp = s.str(); + + char cur_dir[1024]; + getcwd(cur_dir, 1024); + std::string latency_log_filename = std::string(cur_dir) + "/latency_logs/latency_log_file_" + timestamp + ".csv"; + latency_log_file.open(latency_log_filename, std::ios::out); + + // latency_log_file << "test hello" << std::endl; } @@ -91,7 +117,7 @@ namespace v2x } - void V2XNode::publishObjects(std::vector *objectsStack) { + void V2XNode::publishObjects(std::vector *objectsStack, int cpm_num) { autoware_auto_perception_msgs::msg::PredictedObjects output_dynamic_object_msg; std_msgs::msg::Header header; rclcpp::Time current_time = this->now(); @@ -128,20 +154,28 @@ namespace v2x output_dynamic_object_msg.objects.push_back(object); } - current_time = this->now(); - // RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds()); + std::chrono::milliseconds ms = std::chrono::duration_cast ( + std::chrono::system_clock::now().time_since_epoch() + ); + latency_log_file << "T_publish," << cpm_num << "," << ms.count() << std::endl; publisher_->publish(output_dynamic_object_msg); } void V2XNode::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { - rclcpp::Time current_time = this->now(); + // rclcpp::Time current_time = this->now(); rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg. // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size()); // Measuring T_A1R1 // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj %ld", msg_time.nanoseconds()); - // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj_receive %ld", current_time.nanoseconds()); + // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_rosmsg %ld", current_time.nanoseconds()); + + std::chrono::milliseconds ms = std::chrono::duration_cast ( + std::chrono::system_clock::now().time_since_epoch() + ); + latency_log_file << "T_rosmsg,," << ms.count() << std::endl; + app->objectsCallback(msg); }