latency logging, etc.

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::milliseconds> (
+        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<milliseconds> (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_) {
+    // Flag all objects as NOT_SEND
-    //   RCLCPP_INFO(node_->get_logger(), "updateObjectsStack Skipped...");
+    if (objectsList.size() > 0) {
-    //   return;
+      for (auto& object : objectsList) {
-    // } else {
+        object.to_send = false;
-    //   // objectsList.clear();
+        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<geonet::DownPacket> 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::milliseconds> (
+        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;

src/raw_socket_link.cpp

@@ -2,6 +2,7 @@
 #include <iostream>
 #include <vanetza/access/data_request.hpp>
 #include <vanetza/net/ethernet_header.hpp>
+#include <chrono>
 
 using namespace vanetza;
 
@@ -33,7 +34,9 @@ std::size_t RawSocketLink::transmit(std::unique_ptr<ChunkPacket> packet)
   return socket_.send(const_buffers);
 }
 
-void RawSocketLink::indicate(IndicationCallback callback) { callback_ = callback; }
+void RawSocketLink::indicate(IndicationCallback callback) {
+  callback_ = callback;
+}
 
 void RawSocketLink::do_receive()
 {

src/v2x_node.cpp

@@ -18,6 +18,8 @@
 #include <boost/date_time/posix_time/posix_time.hpp>
 
 #include "tf2/LinearMath/Quaternion.h"
+#include <chrono>
+#include <iostream>
 
 namespace gn = vanetza::geonet;
 
@@ -44,8 +46,32 @@ namespace v2x
 
     RCLCPP_INFO(get_logger(), "V2X Node Launched");
 
-    rclcpp::Time current_time = this->now();
+    //rclcpp::Time current_time = this->now();
-    RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] [measure] T_R1 %ld", current_time.nanoseconds());
+    //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<<lt->tm_year-100; //100を引くことで20xxのxxの部分になる
+    s<<"-";
+    s<<lt->tm_mon+1; //月を0からカウントしているため
+    s<<"-";
+    s<<lt->tm_mday; //そのまま
+    s<<"_";
+    s<<lt->tm_hour;
+    s<<":";
+    s<<lt->tm_min;
+    s<<":";
+    s<<lt->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<CpmApplication::Object> *objectsStack) {
+  void V2XNode::publishObjects(std::vector<CpmApplication::Object> *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();
+    std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> (
-    // RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds());
+      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::milliseconds> (
+      std::chrono::system_clock::now().time_since_epoch()
+    );
+    latency_log_file << "T_rosmsg,," << ms.count() << std::endl;
+
     app->objectsCallback(msg);
   }