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; }