Add measurement loggings

src/cpm_application.cpp

@@ -70,12 +70,15 @@ namespace v2x
     return btp::ports::CPM;
   }
 
-  void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet)
+  void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet) {
-  {
     asn1::PacketVisitor<asn1::Cpm> visitor;
     std::shared_ptr<const asn1::Cpm> cpm = boost::apply_visitor(visitor, *packet);
     if (cpm) {
       RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received decodable CPM content");
+
+      rclcpp::Time current_time = node_->now();
+      RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r2 %ld", current_time.nanoseconds());
+
       asn1::Cpm message = *cpm;
       ItsPduHeader_t &header = message->header;
 
@@ -172,8 +175,7 @@ namespace v2x
   void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp)
   {
     generationDeltaTime_ = *gdt;
-    gdt_timestamp_ = *gdt_timestamp;
+    gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
-    // RCLCPP_INFO(node_->get_logger(), "[updateGDT] %d %lu", generationDeltaTime_, gdt_timestamp_);
   }
 
   void CpmApplication::updateHeading(double *yaw)
@@ -230,10 +232,15 @@ namespace v2x
           object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600
         }
         
-        long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec;
+        long long msg_timestamp_sec = msg->header.stamp.sec;
-        object.timeOfMeasurement = (gdt_timestamp_ - timestamp) / 1e6;
+        long long msg_timestamp_nsec = msg->header.stamp.nanosec;
+        msg_timestamp_sec -= 1072915200; // convert to etsi-epoch
+        long long msg_timestamp_msec = msg_timestamp_sec * 1000 + msg_timestamp_nsec / 1000000;
+        // long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec;
+        object.timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec;
         // RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] %ld %ld %d", gdt_timestamp_, timestamp, object.timeOfMeasurement);
         if (object.timeOfMeasurement < -1500 || object.timeOfMeasurement > 1500) {
+          RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", object.timeOfMeasurement);
           continue;
         }
         objectsStack.push_back(object);
@@ -248,8 +255,7 @@ namespace v2x
     updating_objects_stack_ = false;
   }
 
-  void CpmApplication::send()
+  void CpmApplication::send() {
-  {
     sending_ = true;
     
     RCLCPP_INFO(node_->get_logger(), "[SEND] Sending CPM...");
@@ -377,5 +383,7 @@ namespace v2x
       // }
     sending_ = false;
     // RCLCPP_INFO(node->get_logger(), "Application::request END");
+    rclcpp::Time current_time = node_->now();
+    RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds());
   }
 }

src/v2x_app.cpp

@@ -40,6 +40,9 @@ namespace v2x
 
   void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
     // RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received");
+    if (!tf_received_) {
+      RCLCPP_WARN(node_->get_logger(), "[V2XApp::objectsCallback] tf not received yet");
+    }
     if (tf_received_ && cp_started_) {
       cp->updateObjectsStack(msg);
     }
@@ -62,7 +65,7 @@ namespace v2x
     // long long gdt_timestamp_msec_etsi_epoch = gdt_timestamp_msec - 1072915200000;
     // int gdt = gdt_timestamp_msec_etsi_epoch % 65536; // milliseconds
     int gdt = timestamp_msec % 65536;
-    RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec);
+    // RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec);
 
     double rot_x = msg->transforms[0].transform.rotation.x;
     double rot_y = msg->transforms[0].transform.rotation.y;

src/v2x_node.cpp

@@ -25,14 +25,13 @@ using namespace std::chrono;
 
 namespace v2x
 {
-  V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options), received_time(this->now()), cpm_received(false)
+  V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) {
-  {
     using std::placeholders::_1;
     subscription_ = this->create_subscription<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
 
     subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
 
-    publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/objects", rclcpp::QoS{10});
+    publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/v2x/cpm/objects", rclcpp::QoS{10});
 
     this->declare_parameter<std::string>("network_interface", "vmnet1");
 
@@ -40,13 +39,16 @@ namespace v2x
     boost::thread v2xApp(boost::bind(&V2XApp::start, app));
 
     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());
+
   }
 
   void V2XNode::publishObjects(std::vector<CpmApplication::Object> *objectsStack) {
     autoware_perception_msgs::msg::DynamicObjectArray output_dynamic_object_msg;
     std_msgs::msg::Header header;
     rclcpp::Time current_time = this->now();
-    received_time = current_time;
     output_dynamic_object_msg.header.frame_id = "map";
     output_dynamic_object_msg.header.stamp = current_time;
 
@@ -79,27 +81,25 @@ namespace v2x
 
       output_dynamic_object_msg.objects.push_back(object);
     }
-    cpm_received = true;
+
+    current_time = this->now();
+    RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds());
+
     publisher_->publish(output_dynamic_object_msg);
   }
 
-  void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg)
+  void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
-  {
     rclcpp::Time current_time = this->now();
-    // RCLCPP_INFO(get_logger(), "%ld", current_time.nanoseconds() - received_time.nanoseconds());
+    rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg.
-    if (cpm_received) {
+    RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size());
-      // if time difference is more than 10ms
+
-      if (current_time - received_time > rclcpp::Duration(std::chrono::nanoseconds(10000000))) {
+    // Measuring T_A1R1
-        app->objectsCallback(msg);
+    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());
-      cpm_received = false;
+    app->objectsCallback(msg);
-    } else {
-      app->objectsCallback(msg);
-    }
   }
 
-  void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg)
+  void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
-  {
     app->tfCallback(msg);
   }
 }