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

Add timeOfMeasurement

This commit is contained in:
Yu Asabe 2021-11-23 13:02:54 +09:00
parent 5e06ce690b
commit c3766f5f83
3 changed files with 18 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/autoware_v2x/cpm_application.hpp
View File

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

15
src/cpm_application.cpp
View File

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

9
src/v2x_app.cpp
View File

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