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

19
src/cpm_application.cpp
View File

@ -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<milliseconds> (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<milliseconds> (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;

8
src/v2x_app.cpp
View File

@ -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, &timestamp_msec);
cp->updateGenerationTime(&gdt, &timestamp_msec);
}
}