From 1360510b2aa1bfebe02a641dd3684b2ad2fd27d0 Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Thu, 11 Nov 2021 05:30:21 +0900 Subject: [PATCH] Debug receiver, integration --- include/autoware_v2x/v2x_app.hpp | 1 + src/cpm_application.cpp | 12 ++++-- src/v2x_app.cpp | 73 +++++++++++++++++--------------- 3 files changed, 49 insertions(+), 37 deletions(-) diff --git a/include/autoware_v2x/v2x_app.hpp b/include/autoware_v2x/v2x_app.hpp index 26e80b8..7526af4 100644 --- a/include/autoware_v2x/v2x_app.hpp +++ b/include/autoware_v2x/v2x_app.hpp @@ -34,6 +34,7 @@ namespace v2x friend class Application; V2XNode* node_; bool tf_received_; + int tf_interval_; bool cp_started_; }; } diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index a639a87..0daf0c1 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -41,7 +41,7 @@ namespace v2x sending_(false) { RCLCPP_INFO(node_->get_logger(), "CpmApplication started..."); - set_interval(milliseconds(1000)); + set_interval(milliseconds(100)); } void CpmApplication::set_interval(Clock::duration interval) @@ -108,8 +108,12 @@ namespace v2x CpmApplication::Object object; double x1 = poc->list.array[i]->xDistance.value; double y1 = poc->list.array[i]->yDistance.value; - object.position_x = x_mgrs - (cos(orientation) * x1 - sin(orientation) * y1); - object.position_y = y_mgrs - (sin(orientation) * x1 + cos(orientation) * y1); + RCLCPP_INFO(node_->get_logger(), "cpm object: xDistance: %f, yDistance: %f", x1, y1); + 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); + RCLCPP_INFO(node_->get_logger(), "cpm object: %f, %f, %f, %f", x1, y1, object.position_x, object.position_y); receivedObjectsStack.push_back(object); } node_->publishObjects(&receivedObjectsStack); @@ -253,6 +257,8 @@ namespace v2x for (CpmApplication::Object object : objectsStack) { + if (object.xDistance > 10000) continue; + if (object.yDistance > 10000) continue; PerceivedObject *pObj = vanetza::asn1::allocate(); pObj->objectID = object.objectID; pObj->timeOfMeasurement = object.timeOfMeasurement; diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 15b25fd..4d290ce 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -33,6 +33,7 @@ namespace v2x V2XApp::V2XApp(V2XNode *node) : node_(node), tf_received_(false), + tf_interval_(0), cp_started_(false) { } @@ -45,49 +46,53 @@ namespace v2x } void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { - // RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received"); - tf_received_ = true; + if (tf_interval_ == 4) { + RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received"); + tf_received_ = true; - 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; + 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; - double rot_x = msg->transforms[0].transform.rotation.x; - double rot_y = msg->transforms[0].transform.rotation.y; - double rot_z = msg->transforms[0].transform.rotation.z; - double rot_w = msg->transforms[0].transform.rotation.w; + double rot_x = msg->transforms[0].transform.rotation.x; + double rot_y = msg->transforms[0].transform.rotation.y; + double rot_z = msg->transforms[0].transform.rotation.z; + double rot_w = msg->transforms[0].transform.rotation.w; - // Convert the quarternion to euler (yaw, pitch, roll) - tf2::Quaternion quat(rot_x, rot_y, rot_z, rot_w); - tf2::Matrix3x3 matrix(quat); - double roll, pitch, yaw; - matrix.getRPY(roll, pitch, yaw); + // Convert the quarternion to euler (yaw, pitch, roll) + tf2::Quaternion quat(rot_x, rot_y, rot_z, rot_w); + tf2::Matrix3x3 matrix(quat); + double roll, pitch, yaw; + matrix.getRPY(roll, pitch, yaw); - char mgrs[20]; - int zone, prec; - bool northp; - double x_mgrs, y_mgrs; - double lat, lon; - sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y); - // RCLCPP_INFO(node_->get_logger(), "MGRS: %s", mgrs); + char mgrs[20]; + int zone, prec; + bool northp; + double x_mgrs, y_mgrs; + double lat, lon; + sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y); + // RCLCPP_INFO(node_->get_logger(), "MGRS: %s", mgrs); - GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); - GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon); + GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); + GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon); - // RCLCPP_INFO(node_->get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon); - // RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f, %f, %f, %f", rot_x, rot_y, rot_z, rot_w); - // RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f %f %f", roll, pitch, yaw); - // RCLCPP_INFO(node_->get_logger(), "Timestamp: %d, GDT: %d", timestamp, gdt); + // RCLCPP_INFO(node_->get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon); + // RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f, %f, %f, %f", rot_x, rot_y, rot_z, rot_w); + RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f %f %f", roll, pitch, yaw); + // RCLCPP_INFO(node_->get_logger(), "Timestamp: %d, GDT: %d", timestamp, gdt); - if (cp && cp_started_) { - cp->updateMGRS(&x, &y); - cp->updateRP(&lat, &lon, &z); - cp->updateHeading(&yaw); - cp->updateGenerationDeltaTime(&gdt); + if (cp && cp_started_) { + cp->updateMGRS(&x, &y); + cp->updateRP(&lat, &lon, &z); + cp->updateHeading(&yaw); + cp->updateGenerationDeltaTime(&gdt); + } + tf_interval_ = 0; } + tf_interval_ += 1; } void V2XApp::start() {