From 579f6fe16f1e6803bb174819502e0da30a85bf82 Mon Sep 17 00:00:00 2001 From: Tiago Garcia Date: Wed, 10 Jul 2024 17:37:24 +0100 Subject: [PATCH] Fix generationDeltaTime Signed-off-by: Tiago Garcia --- include/autoware_v2x/cam_application.hpp | 5 ++--- src/cam_application.cpp | 10 ++++----- src/cpm_application.cpp | 26 ++++++++++++------------ src/v2x_app.cpp | 2 +- 4 files changed, 20 insertions(+), 23 deletions(-) diff --git a/include/autoware_v2x/cam_application.hpp b/include/autoware_v2x/cam_application.hpp index bf9f39a..e606f43 100644 --- a/include/autoware_v2x/cam_application.hpp +++ b/include/autoware_v2x/cam_application.hpp @@ -23,7 +23,7 @@ public: void set_interval(vanetza::Clock::duration); void updateMGRS(double *, double *); void updateRP(double *, double *, double *); - void updateGenerationTime(int *, long *); + void updateGenerationDeltaTime(int *, long *); void updateHeading(double *); void setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &); void updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr); @@ -130,7 +130,7 @@ private: }; VehicleStatus vehicleStatus_; - int generationTime_; + int generationDeltaTime_; long gdt_timestamp_; double objectConfidenceThreshold_; @@ -140,7 +140,6 @@ private: bool sending_; bool is_sender_; bool reflect_packet_; - bool include_all_persons_and_animals_; int cam_num_; int received_cam_num_; diff --git a/src/cam_application.cpp b/src/cam_application.cpp index 4951956..7ba4e90 100644 --- a/src/cam_application.cpp +++ b/src/cam_application.cpp @@ -41,14 +41,13 @@ namespace v2x positionConfidenceEllipse_(), velocityReport_(), vehicleStatus_(), - generationTime_(0), + generationDeltaTime_(0), updating_velocity_report_(false), updating_vehicle_status_(false), sending_(false), is_sender_(is_sender), reflect_packet_(false), objectConfidenceThreshold_(0.0), - include_all_persons_and_animals_(false), cam_num_(0), received_cam_num_(0), use_dynamic_generation_rules_(false) @@ -96,8 +95,8 @@ namespace v2x positionConfidenceEllipse_.y.insert(*lon); } - void CamApplication::updateGenerationTime(int *gdt, long *gdt_timestamp) { - generationTime_ = *gdt; + void CamApplication::updateGenerationDeltaTime(int *gdt, long *gdt_timestamp) { + generationDeltaTime_ = *gdt; gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp } @@ -175,9 +174,8 @@ namespace v2x CoopAwareness_t &cam = message->cam; - // Set GenerationTime RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_); - cam.generationDeltaTime = gdt_timestamp_; + cam.generationDeltaTime = generationDeltaTime_; BasicContainer_t &basic_container = cam.camParameters.basicContainer; basic_container.stationType = StationType_passengerCar; diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index 478af3d..e426994 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -32,7 +32,7 @@ using namespace vanetza; using namespace std::chrono; namespace v2x { - CpmApplication::CpmApplication(V2XNode *node, Runtime &rt, bool is_sender) : + CpmApplication::CpmApplication(V2XNode *node, Runtime &rt, bool is_sender) : node_(node), runtime_(rt), ego_(), @@ -156,7 +156,7 @@ namespace v2x { x1 = x1 / 100.0; y1 = y1 / 100.0; object.position_x = x_mgrs + (cos(orientation) * x1 - sin(orientation) * y1); - object.position_y = y_mgrs + (sin(orientation) * x1 + cos(orientation) * y1); + object.position_y = y_mgrs + (sin(orientation) * x1 + cos(orientation) * y1); object.shape_x = poc->list.array[i]->planarObjectDimension2->value; object.shape_y = poc->list.array[i]->planarObjectDimension1->value; object.shape_z = poc->list.array[i]->verticalObjectDimension->value; @@ -236,7 +236,7 @@ namespace v2x { }); if (found_object == objectsList.end()) { - + } else { found_object->to_send = true; } @@ -317,7 +317,7 @@ namespace v2x { ++cpm_object_id_; } else { - + // Object was already in internal memory // Object belongs to class person or animal @@ -327,7 +327,7 @@ namespace v2x { found_object->to_send = true; found_object->to_send_trigger = 5; } - + // Object has not been included in a CPM in the past 500 ms. if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 500000) { // Include all objects of class person or animal in the current CPM @@ -349,7 +349,7 @@ namespace v2x { found_object->to_send = true; found_object->to_send_trigger = 1; } else { - + } // Absolute speed changed by more than 0.5 m/s @@ -359,7 +359,7 @@ namespace v2x { if (speed > 0.5) { found_object->to_send = true; found_object->to_send_trigger = 2; - } + } // Orientation of speed vector changed by more than 4 degrees double twist_angular_x_diff = (obj.kinematics.initial_twist_with_covariance.twist.angular.x - found_object->twist_angular_x) * 180 / M_PI; @@ -434,7 +434,7 @@ namespace v2x { } else { RCLCPP_INFO(node_->get_logger(), "[objectsList] %d,,,,", cpm_num); } - + // RCLCPP_INFO(node_->get_logger(), "------------------------"); } @@ -445,7 +445,7 @@ namespace v2x { sending_ = true; printObjectsList(cpm_num_); - + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM..."); vanetza::asn1::Cpm message; @@ -578,7 +578,7 @@ namespace v2x { RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM with %d objects", perceivedObjectsCount); insertCpmToCpmTable(message, (char*) "cpm_sent"); - + std::unique_ptr payload{new geonet::DownPacket()}; payload->layer(OsiLayer::Application) = std::move(message); @@ -618,7 +618,7 @@ namespace v2x { } char* sql_command; - + sql_command = (char*) "create table if not exists cpm_sent(id INTEGER PRIMARY KEY, timestamp BIGINT, perceivedObjectCount INTEGER);"; ret = sqlite3_exec(db, sql_command, NULL, NULL, &err); @@ -661,7 +661,7 @@ namespace v2x { } std::stringstream sql_command; - + sql_command << "insert into " << table_name << " (timestamp, perceivedObjectCount) values (" << timestamp << ", " << perceivedObjectCount << ");"; ret = sqlite3_exec(db, sql_command.str().c_str(), NULL, NULL, &err); @@ -677,4 +677,4 @@ namespace v2x { sqlite3_close(db); // RCLCPP_INFO(node_->get_logger(), "CpmApplication::insertCpmToCpmTable Finished"); } -} +} \ No newline at end of file diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 90cddf0..2b9fdac 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -128,7 +128,7 @@ namespace v2x cam->updateMGRS(&x, &y); cam->updateRP(&lat, &lon, &z); cam->updateHeading(&yaw); - cam->updateGenerationTime(&gdt, ×tamp_msec); + cam->updateGenerationDeltaTime(&gdt, ×tamp_msec); } }