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 updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr);
void updateMGRS(double *, double *); void updateMGRS(double *, double *);
void updateRP(double *, double *, double *); void updateRP(double *, double *, double *);
void updateGenerationDeltaTime(int *); void updateGenerationDeltaTime(int *, long long *);
void updateHeading(double *); void updateHeading(double *);
void send(); void send();
@ -65,6 +65,7 @@ namespace v2x
double ego_altitude_; double ego_altitude_;
double ego_heading_; double ego_heading_;
int generationDeltaTime_; int generationDeltaTime_;
long long gdt_timestamp_;
bool updating_objects_stack_; bool updating_objects_stack_;
bool sending_; bool sending_;

15
src/cpm_application.cpp
View File

@ -44,7 +44,7 @@ namespace v2x
sending_(false) sending_(false)
{ {
RCLCPP_INFO(node_->get_logger(), "CpmApplication started..."); RCLCPP_INFO(node_->get_logger(), "CpmApplication started...");
set_interval(milliseconds(100)); set_interval(milliseconds(1000));
} }
void CpmApplication::set_interval(Clock::duration interval) void CpmApplication::set_interval(Clock::duration interval)
@ -162,10 +162,10 @@ namespace v2x
ego_altitude_ = *altitude; 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; generationDeltaTime_ = *gdt;
gdt_timestamp_ = *gdt_timestamp;
} }
void CpmApplication::updateHeading(double *yaw) 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.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); objectsStack.push_back(object);
++i; ++i;
// RCLCPP_INFO(node_->get_logger(), "Added to stack: %f %f %f", obj.shape.dimensions.x, obj.shape.dimensions.y, obj.shape.dimensions.z); // 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; if (heading < 0) heading += 3600;
ovc.heading.headingValue = heading; ovc.heading.headingValue = heading;
ovc.heading.headingConfidence = 1; 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) { if (objectsStack.size() > 0) {
PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; 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 x = msg->transforms[0].transform.translation.x;
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;
int timestamp = msg->transforms[0].header.stamp.sec; int gdt_timestamp_sec = msg->transforms[0].header.stamp.sec; // seconds
int gdt = timestamp % 65536; 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_x = msg->transforms[0].transform.rotation.x;
double rot_y = msg->transforms[0].transform.rotation.y; double rot_y = msg->transforms[0].transform.rotation.y;
@ -88,7 +91,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); cp->updateGenerationDeltaTime(&gdt, &gdt_timestamp);
} }
tf_interval_ = 0; tf_interval_ = 0;
} }