From c3766f5f83b093c9391acb6b45e33f956933fbc9 Mon Sep 17 00:00:00 2001
From: Yu Asabe <yuasabe@g.ecc.u-tokyo.ac.jp>
Date: Tue, 23 Nov 2021 13:02:54 +0900
Subject: [PATCH] Add timeOfMeasurement

---
 include/autoware_v2x/cpm_application.hpp |  3 ++-
 src/cpm_application.cpp                  | 15 ++++++++++-----
 src/v2x_app.cpp                          |  9 ++++++---
 3 files changed, 18 insertions(+), 9 deletions(-)

diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp
index d8c5c3e..8879b3e 100644
--- a/include/autoware_v2x/cpm_application.hpp
+++ b/include/autoware_v2x/cpm_application.hpp
@@ -21,7 +21,7 @@ namespace v2x
         void updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr);
         void updateMGRS(double *, double *);
         void updateRP(double *, double *, double *);
-        void updateGenerationDeltaTime(int *);
+        void updateGenerationDeltaTime(int *, long long *);
         void updateHeading(double *);
         void send();
 
@@ -65,6 +65,7 @@ namespace v2x
         double ego_altitude_;
         double ego_heading_;
         int generationDeltaTime_;
+        long long gdt_timestamp_;
 
         bool updating_objects_stack_;
         bool sending_;
diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp
index 1c0cf89..0e52681 100644
--- a/src/cpm_application.cpp
+++ b/src/cpm_application.cpp
@@ -44,7 +44,7 @@ namespace v2x
     sending_(false)
   {
     RCLCPP_INFO(node_->get_logger(), "CpmApplication started...");
-    set_interval(milliseconds(100));
+    set_interval(milliseconds(1000));
   }
 
   void CpmApplication::set_interval(Clock::duration interval)
@@ -162,10 +162,10 @@ namespace v2x
     ego_altitude_ = *altitude;
   }
 
-  void CpmApplication::updateGenerationDeltaTime(int *gdt)
+  void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp)
   {
-    // RCLCPP_INFO(node_->get_logger(), "Update GDT");
     generationDeltaTime_ = *gdt;
+    gdt_timestamp_ = *gdt_timestamp;
   }
 
   void CpmApplication::updateHeading(double *yaw)
@@ -222,7 +222,12 @@ namespace v2x
           object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600
         }
         
-        object.timeOfMeasurement = 100;
+        long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec;
+        object.timeOfMeasurement = (gdt_timestamp_ - timestamp) / 1e6;
+        // RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] %ld %ld %d", gdt_timestamp_, timestamp, object.timeOfMeasurement);
+        if (object.timeOfMeasurement < -1500 || object.timeOfMeasurement > 1500) {
+          continue;
+        }
         objectsStack.push_back(object);
         ++i;
         // RCLCPP_INFO(node_->get_logger(), "Added to stack: %f %f %f", obj.shape.dimensions.x, obj.shape.dimensions.y, obj.shape.dimensions.z);
@@ -290,7 +295,7 @@ namespace v2x
     if (heading < 0) heading += 3600;
     ovc.heading.headingValue = heading;
     ovc.heading.headingConfidence = 1;
-    RCLCPP_INFO(node_->get_logger(), "[SEND] headingValue: %f %d", ego_heading_, ovc.heading.headingValue);
+    // RCLCPP_INFO(node_->get_logger(), "[SEND] headingValue: %f %d", ego_heading_, ovc.heading.headingValue);
 
     if (objectsStack.size() > 0) {
       PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp
index dae41bc..c6b18a5 100644
--- a/src/v2x_app.cpp
+++ b/src/v2x_app.cpp
@@ -53,8 +53,11 @@ namespace v2x
       double x = msg->transforms[0].transform.translation.x;
       double y = msg->transforms[0].transform.translation.y;
       double z = msg->transforms[0].transform.translation.z;
-      int timestamp = msg->transforms[0].header.stamp.sec;
-      int gdt = timestamp % 65536;
+      int gdt_timestamp_sec = msg->transforms[0].header.stamp.sec; // seconds
+      int gdt_timestamp_nsec = msg->transforms[0].header.stamp.nanosec; // nanoseconds
+      long long gdt_timestamp = gdt_timestamp_sec * 1e9 + gdt_timestamp_nsec; // nanoseconds
+      int gdt_timestamp_msec = gdt_timestamp / 1e6;
+      int gdt = gdt_timestamp_msec % 65536; // milliseconds
 
       double rot_x = msg->transforms[0].transform.rotation.x;
       double rot_y = msg->transforms[0].transform.rotation.y;
@@ -88,7 +91,7 @@ namespace v2x
         cp->updateMGRS(&x, &y);
         cp->updateRP(&lat, &lon, &z);
         cp->updateHeading(&yaw);
-        cp->updateGenerationDeltaTime(&gdt);
+        cp->updateGenerationDeltaTime(&gdt, &gdt_timestamp);
       }
       tf_interval_ = 0;
     }