TiagoRG
/
AutowareV2X
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #21 from tlab-wide/feature/add_generation_time 

Change generationDeltaTime to generationTime
This commit is contained in:
Yu Asabe 2022-11-24 18:35:42 +09:00 committed by GitHub
parent 6789c2f74d ee54974808
commit ee5a1813ad
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 17 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/autoware_v2x/cpm_application.hpp
View File

@ -24,7 +24,7 @@ namespace v2x
void updateObjectsList(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr); void updateObjectsList(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr);
void updateMGRS(double *, double *); void updateMGRS(double *, double *);
void updateRP(double *, double *, double *); void updateRP(double *, double *, double *);
void updateGenerationDeltaTime(int *, long long *); void updateGenerationTime(int *, long *);
void updateHeading(double *); void updateHeading(double *);
void printObjectsList(int); void printObjectsList(int);
void send(); void send();
@ -92,8 +92,8 @@ namespace v2x
Ego_station ego_; Ego_station ego_;
int generationDeltaTime_; int generationTime_;
long long gdt_timestamp_; long gdt_timestamp_;
double objectConfidenceThreshold_; double objectConfidenceThreshold_;

19
src/cpm_application.cpp
View File

@ -36,7 +36,7 @@ namespace v2x {
node_(node), node_(node),
runtime_(rt), runtime_(rt),
ego_(), ego_(),
generationDeltaTime_(0), generationTime_(0),
updating_objects_list_(false), updating_objects_list_(false),
sending_(false), sending_(false),
is_sender_(is_sender), is_sender_(is_sender),
@ -101,10 +101,10 @@ namespace v2x {
// Calculate GDT and get GDT from CPM and calculate the "Age of CPM" // Calculate GDT and get GDT from CPM and calculate the "Age of CPM"
GenerationDeltaTime_t gdt_cpm = message->cpm.generationDeltaTime; TimestampIts_t gt_cpm = message->cpm.generationTime;
const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch()); // const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch());
uint16_t gdt = time_now.count(); // uint16_t gdt = time_now.count();
int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536; // 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_CPM: %ld", gdt_cpm);
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT: %u", gdt); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT: %u", gdt);
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_R1R2: %d", gdt_diff); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_R1R2: %d", gdt_diff);
@ -218,8 +218,8 @@ namespace v2x {
ego_.altitude = *altitude; ego_.altitude = *altitude;
} }
void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) { void CpmApplication::updateGenerationTime(int *gdt, long *gdt_timestamp) {
generationDeltaTime_ = *gdt; generationTime_ = *gdt;
gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
} }
@ -458,8 +458,9 @@ namespace v2x {
CollectivePerceptionMessage_t &cpm = message->cpm; CollectivePerceptionMessage_t &cpm = message->cpm;
// Set GenerationDeltaTime // Set GenerationTime
cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec; RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_);
asn_long2INTEGER(&cpm.generationTime, (long) gdt_timestamp_);
CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer; CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
management.stationType = StationType_passengerCar; management.stationType = StationType_passengerCar;

8
src/v2x_app.cpp
View File

@ -54,10 +54,10 @@ namespace v2x
double y = msg->transforms[0].transform.translation.y; double y = msg->transforms[0].transform.translation.y;
double z = msg->transforms[0].transform.translation.z; double z = msg->transforms[0].transform.translation.z;
long long timestamp_sec = msg->transforms[0].header.stamp.sec; // seconds long timestamp_sec = msg->transforms[0].header.stamp.sec; // seconds
long long timestamp_nsec = msg->transforms[0].header.stamp.nanosec; // nanoseconds long timestamp_nsec = msg->transforms[0].header.stamp.nanosec; // nanoseconds
timestamp_sec -= 1072915200; // convert to etsi-epoch 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; int gdt = timestamp_msec % 65536;
double rot_x = msg->transforms[0].transform.rotation.x; double rot_x = msg->transforms[0].transform.rotation.x;
@ -92,7 +92,7 @@ namespace v2x
cp->updateMGRS(&x, &y); cp->updateMGRS(&x, &y);
cp->updateRP(&lat, &lon, &z); cp->updateRP(&lat, &lon, &z);
cp->updateHeading(&yaw); cp->updateHeading(&yaw);
cp->updateGenerationDeltaTime(&gdt, &timestamp_msec); cp->updateGenerationTime(&gdt, &timestamp_msec);
} }
} }