diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index c9183d0..c11c5ed 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -24,7 +24,7 @@ namespace v2x void updateObjectsList(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr); void updateMGRS(double *, double *); void updateRP(double *, double *, double *); - void updateGenerationDeltaTime(int *, long long *); + void updateGenerationTime(int *, long *); void updateHeading(double *); void printObjectsList(int); void send(); @@ -92,8 +92,8 @@ namespace v2x Ego_station ego_; - int generationDeltaTime_; - long long gdt_timestamp_; + int generationTime_; + long gdt_timestamp_; double objectConfidenceThreshold_; diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index 0a89efe..53568a5 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -36,7 +36,7 @@ namespace v2x { node_(node), runtime_(rt), ego_(), - generationDeltaTime_(0), + generationTime_(0), updating_objects_list_(false), sending_(false), is_sender_(is_sender), @@ -101,10 +101,10 @@ namespace v2x { // 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()); - uint16_t gdt = time_now.count(); - int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536; + TimestampIts_t gt_cpm = message->cpm.generationTime; + // const auto time_now = duration_cast (runtime_.now().time_since_epoch()); + // uint16_t gdt = time_now.count(); + // int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536; // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT_CPM: %ld", gdt_cpm); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT: %u", gdt); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_R1R2: %d", gdt_diff); @@ -218,8 +218,8 @@ namespace v2x { ego_.altitude = *altitude; } - void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) { - generationDeltaTime_ = *gdt; + void CpmApplication::updateGenerationTime(int *gdt, long *gdt_timestamp) { + generationTime_ = *gdt; gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp } @@ -458,8 +458,9 @@ namespace v2x { CollectivePerceptionMessage_t &cpm = message->cpm; - // Set GenerationDeltaTime - cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec; + // Set GenerationTime + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_); + asn_long2INTEGER(&cpm.generationTime, (long) gdt_timestamp_); CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer; management.stationType = StationType_passengerCar; diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 0eaf010..c687bc7 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -54,10 +54,10 @@ namespace v2x double y = msg->transforms[0].transform.translation.y; double z = msg->transforms[0].transform.translation.z; - long long timestamp_sec = msg->transforms[0].header.stamp.sec; // seconds - long long timestamp_nsec = msg->transforms[0].header.stamp.nanosec; // nanoseconds + long timestamp_sec = msg->transforms[0].header.stamp.sec; // seconds + long timestamp_nsec = msg->transforms[0].header.stamp.nanosec; // nanoseconds timestamp_sec -= 1072915200; // convert to etsi-epoch - long long timestamp_msec = timestamp_sec * 1000 + timestamp_nsec / 1000000; + long timestamp_msec = timestamp_sec * 1000 + timestamp_nsec / 1000000; int gdt = timestamp_msec % 65536; double rot_x = msg->transforms[0].transform.rotation.x; @@ -92,7 +92,7 @@ namespace v2x cp->updateMGRS(&x, &y); cp->updateRP(&lat, &lon, &z); cp->updateHeading(&yaw); - cp->updateGenerationDeltaTime(&gdt, ×tamp_msec); + cp->updateGenerationTime(&gdt, ×tamp_msec); } }