From eb6a9b20406c0cb8876f8d1ff0297fe86d32586e Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Wed, 16 Feb 2022 16:53:45 +0900 Subject: [PATCH] fixing positioning --- include/autoware_v2x/cpm_application.hpp | 17 +++++++---- launch/v2x.launch.xml | 2 +- src/cpm_application.cpp | 38 ++++++++++-------------- src/v2x_app.cpp | 13 +------- 4 files changed, 29 insertions(+), 41 deletions(-) diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index 236576d..98fadb8 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -58,12 +58,17 @@ namespace v2x vanetza::Runtime &runtime_; vanetza::Clock::duration cpm_interval_; - double ego_x_; - double ego_y_; - double ego_lat_; - double ego_lon_; - double ego_altitude_; - double ego_heading_; + struct Ego_station { + double mgrs_x; + double mgrs_y; + double latitude; + double longitude; + double altitude; + double heading; + }; + + Ego_station ego_; + int generationDeltaTime_; long long gdt_timestamp_; diff --git a/launch/v2x.launch.xml b/launch/v2x.launch.xml index 44032d2..636e94d 100644 --- a/launch/v2x.launch.xml +++ b/launch/v2x.launch.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index c020da0..bc2b084 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -28,17 +28,11 @@ using namespace vanetza; using namespace vanetza::facilities; using namespace std::chrono; -namespace v2x -{ +namespace v2x { CpmApplication::CpmApplication(V2XNode *node, Runtime &rt) : node_(node), runtime_(rt), - ego_x_(0), - ego_y_(0), - ego_lat_(0), - ego_lon_(0), - ego_altitude_(0), - ego_heading_(0), + ego_(), generationDeltaTime_(0), updating_objects_stack_(false), sending_(false), @@ -181,14 +175,14 @@ namespace v2x } void CpmApplication::updateMGRS(double *x, double *y) { - ego_x_ = *x; - ego_y_ = *y; + ego_.mgrs_x = *x; + ego_.mgrs_y = *y; } void CpmApplication::updateRP(double *lat, double *lon, double *altitude) { - ego_lat_ = *lat; - ego_lon_ = *lon; - ego_altitude_ = *altitude; + ego_.latitude = *lat; + ego_.longitude = *lon; + ego_.altitude = *altitude; } void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) { @@ -197,7 +191,7 @@ namespace v2x } void CpmApplication::updateHeading(double *yaw) { - ego_heading_ = *yaw; + ego_.heading = *yaw; } void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { @@ -226,8 +220,10 @@ namespace v2x object.shape_x = std::lround(obj.shape.dimensions.x * 10.0); object.shape_y = std::lround(obj.shape.dimensions.y * 10.0); object.shape_z = std::lround(obj.shape.dimensions.z * 10.0); - object.xDistance = std::lround(((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100.0); - object.yDistance = std::lround(((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100.0); + // object.xDistance = std::lround(((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100.0); + // object.yDistance = std::lround(((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100.0); + object.xDistance = std::lround(((object.position_x - ego_.mgrs_x) * cos(-ego_.heading) - (object.position_y - ego_.mgrs_y) * sin(-ego_.heading)) * 100.0); + object.yDistance = std::lround(((object.position_x - ego_.mgrs_x) * sin(-ego_.heading) + (object.position_y - ego_.mgrs_y) * cos(-ego_.heading)) * 100.0); if (object.xDistance < -132768 || object.xDistance > 132767) { continue; } @@ -260,9 +256,7 @@ namespace v2x } 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); - // RCLCPP_INFO(node_->get_logger(), "Added to stack: %f %f %f", obj.shape.dimensions.x * 10.0, obj.shape.dimensions.y * 10.0, obj.shape.dimensions.z * 10.0); - // RCLCPP_INFO(node_->get_logger(), "Added to stack: %d %d %d", std::lround(obj.shape.dimensions.x * 10.0), std::lround(obj.shape.dimensions.y * 10.0), std::lround(obj.shape.dimensions.z * 10.0)); + RCLCPP_INFO(node_->get_logger(), "Added to stack: #%d (%d, %d) (%d, %d) (%d, %d, %d) (%f: %d)", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_x, object.shape_y, object.shape_z, yaw, object.yawAngle); } } @@ -295,8 +289,8 @@ namespace v2x CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer; management.stationType = StationType_passengerCar; PositionFix fix; - fix.latitude = ego_lat_ * units::degree; - fix.longitude = ego_lon_ * units::degree; + fix.latitude = ego_.latitude * units::degree; + fix.longitude = ego_.longitude * units::degree; fix.confidence.semi_major = 1.0 * units::si::meter; fix.confidence.semi_minor = fix.confidence.semi_major; copy(fix, management.referencePosition); @@ -308,7 +302,7 @@ namespace v2x OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer; ovc.speed.speedValue = 0; ovc.speed.speedConfidence = 1; - int heading = std::lround(((-ego_heading_ * 180 / M_PI) + 90.0) * 10); + int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0); if (heading < 0) heading += 3600; ovc.heading.headingValue = heading; ovc.heading.headingConfidence = 1; diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 81533e2..3ed234f 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -49,7 +49,7 @@ namespace v2x } void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { - // RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received"); + tf_received_ = true; double x = msg->transforms[0].transform.translation.x; @@ -60,10 +60,6 @@ namespace v2x long 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 long gdt_timestamp = gdt_timestamp_sec * 1e9 + gdt_timestamp_nsec; // nanoseconds - // long long gdt_timestamp_msec = gdt_timestamp / 1000000; - // long long gdt_timestamp_msec_etsi_epoch = gdt_timestamp_msec - 1072915200000; - // int gdt = gdt_timestamp_msec_etsi_epoch % 65536; // milliseconds int gdt = timestamp_msec % 65536; // RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec); @@ -78,23 +74,16 @@ namespace v2x 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); 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); - if (cp && cp_started_) { cp->updateMGRS(&x, &y); cp->updateRP(&lat, &lon, &z);