From 91962c72863cc8e939d36f1ebc73c2a5316b32ec Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Fri, 25 Feb 2022 18:38:23 +0900 Subject: [PATCH 1/3] Fix coordinate transformation of tf --- src/cpm_application.cpp | 9 ++++++--- src/v2x_app.cpp | 32 +++++++++++++++++++++++++------- 2 files changed, 31 insertions(+), 10 deletions(-) diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index bc2b084..2fc381e 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -93,13 +93,16 @@ namespace v2x { std::string mgrs; int zone; + int grid_num_x = 4; + int grid_num_y = 39; + int grid_size = 100000; bool northp; double x, y; GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, x, y); - GeographicLib::MGRS::Forward(zone, northp, x, y, lat, 5, mgrs); + // GeographicLib::MGRS::Forward(zone, northp, x, y, lat, 5, mgrs); - int x_mgrs = std::stoi(mgrs.substr(5, 5)); - int y_mgrs = std::stoi(mgrs.substr(10, 5)); + double x_mgrs = x - grid_num_x * grid_size; + double y_mgrs = y - grid_num_y * grid_size; // RCLCPP_INFO(node_->get_logger(), "cpm.(RP).mgrs = %s, %d, %d", mgrs.c_str(), x_mgrs, y_mgrs); // Calculate orientation from Heading diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 3ed234f..e8a25c1 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -74,15 +74,33 @@ namespace v2x double roll, pitch, yaw; matrix.getRPY(roll, pitch, yaw); - char mgrs[20]; - int zone, prec; - bool northp; + char mgrs[30]; + int zone = 54; + int grid_num_x = 4; + int grid_num_y = 39; + int grid_size = 100000; + int prec; + bool northp = true; double x_mgrs, y_mgrs; - double lat, lon; - sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y); - GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); - GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon); + double lat, lon; + // sprintf(mgrs, "54SVE%.3f%.3f", x, y); + // RCLCPP_INFO(node_->get_logger(), "%s", mgrs); + + // Convert MGRS coordinate to UTM coordinate. + // GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); + + // Reverse projection from UTM to geographic. + GeographicLib::UTMUPS::Reverse( + zone, + northp, + grid_num_x * grid_size + x, + grid_num_y * grid_size + y, + lat, + lon + ); + + RCLCPP_INFO(node_->get_logger(), "%f, %f", lat, lon); if (cp && cp_started_) { cp->updateMGRS(&x, &y); From 60eea65de3414f996a4e7043b4be58424d49166c Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Sun, 27 Feb 2022 12:26:11 +0900 Subject: [PATCH 2/3] Add is_sender --- launch/v2x.launch.xml | 2 ++ src/v2x_app.cpp | 18 ++++-------------- src/v2x_node.cpp | 9 +++++---- 3 files changed, 11 insertions(+), 18 deletions(-) diff --git a/launch/v2x.launch.xml b/launch/v2x.launch.xml index 636e94d..30124ae 100644 --- a/launch/v2x.launch.xml +++ b/launch/v2x.launch.xml @@ -1,6 +1,8 @@ + + \ No newline at end of file diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index e8a25c1..a56db42 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -61,7 +61,6 @@ namespace v2x timestamp_sec -= 1072915200; // convert to etsi-epoch long long timestamp_msec = timestamp_sec * 1000 + timestamp_nsec / 1000000; int gdt = timestamp_msec % 65536; - // RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec); double rot_x = msg->transforms[0].transform.rotation.x; double rot_y = msg->transforms[0].transform.rotation.y; @@ -74,21 +73,12 @@ namespace v2x double roll, pitch, yaw; matrix.getRPY(roll, pitch, yaw); - char mgrs[30]; int zone = 54; int grid_num_x = 4; int grid_num_y = 39; int grid_size = 100000; - int prec; bool northp = true; - double x_mgrs, y_mgrs; - double lat, lon; - // sprintf(mgrs, "54SVE%.3f%.3f", x, y); - // RCLCPP_INFO(node_->get_logger(), "%s", mgrs); - - // Convert MGRS coordinate to UTM coordinate. - // GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); // Reverse projection from UTM to geographic. GeographicLib::UTMUPS::Reverse( @@ -99,9 +89,7 @@ namespace v2x lat, lon ); - - RCLCPP_INFO(node_->get_logger(), "%f, %f", lat, lon); - + if (cp && cp_started_) { cp->updateMGRS(&x, &y); cp->updateRP(&lat, &lon, &z); @@ -141,7 +129,9 @@ namespace v2x context.set_link_layer(link_layer.get()); - cp = new CpmApplication(node_, trigger.runtime()); + bool is_sender; + node_->get_parameter("is_sender", is_sender); + cp = new CpmApplication(node_, trigger.runtime(), is_sender); context.enable(cp); diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index 31dbcd9..7e4dd0e 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -34,6 +34,7 @@ namespace v2x publisher_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); this->declare_parameter("network_interface", "vmnet1"); + this->declare_parameter("is_sender", true); app = new V2XApp(this); boost::thread v2xApp(boost::bind(&V2XApp::start, app)); @@ -83,7 +84,7 @@ namespace v2x } current_time = this->now(); - RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds()); + // RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds()); publisher_->publish(output_dynamic_object_msg); } @@ -91,11 +92,11 @@ namespace v2x void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { rclcpp::Time current_time = this->now(); rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg. - RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size()); + // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size()); // Measuring T_A1R1 - RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj %ld", msg_time.nanoseconds()); - RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj_receive %ld", current_time.nanoseconds()); + // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj %ld", msg_time.nanoseconds()); + // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj_receive %ld", current_time.nanoseconds()); app->objectsCallback(msg); } From 1238dea54d44fcd4625927859d5685d9002278aa Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Sun, 27 Feb 2022 12:26:28 +0900 Subject: [PATCH 3/3] Positional fixes, refactoring, is_sender --- include/autoware_v2x/cpm_application.hpp | 2 +- src/cpm_application.cpp | 87 +++++++++++++----------- 2 files changed, 48 insertions(+), 41 deletions(-) diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index 98fadb8..4f64a76 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -14,7 +14,7 @@ namespace v2x class CpmApplication : public Application { public: - CpmApplication(V2XNode *node, vanetza::Runtime &); + CpmApplication(V2XNode *node, vanetza::Runtime &, bool is_sender); PortType port() override; void indicate(const DataIndication &, UpPacketPtr) override; void set_interval(vanetza::Clock::duration); diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index 2fc381e..4d1cd7c 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -21,6 +21,9 @@ #include #include +#include +#include + #define _USE_MATH_DEFINES #include @@ -29,17 +32,17 @@ using namespace vanetza::facilities; using namespace std::chrono; namespace v2x { - CpmApplication::CpmApplication(V2XNode *node, Runtime &rt) : + CpmApplication::CpmApplication(V2XNode *node, Runtime &rt, bool is_sender) : node_(node), runtime_(rt), ego_(), generationDeltaTime_(0), updating_objects_stack_(false), sending_(false), - is_sender_(true), + is_sender_(is_sender), reflect_packet_(false) { - RCLCPP_INFO(node_->get_logger(), "CpmApplication started..."); + RCLCPP_INFO(node_->get_logger(), "CpmApplication started. is_sender: %d", is_sender_); set_interval(milliseconds(100)); } @@ -68,10 +71,10 @@ namespace v2x { std::shared_ptr cpm = boost::apply_visitor(visitor, *packet); if (cpm) { - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] Received decodable CPM content"); + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] Received decodable CPM content"); rclcpp::Time current_time = node_->now(); - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds()); + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds()); asn1::Cpm message = *cpm; ItsPduHeader_t &header = message->header; @@ -81,37 +84,36 @@ namespace v2x { const auto time_now = duration_cast (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); + // 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); CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer; double lat = management.referencePosition.latitude / 1.0e7; double lon = management.referencePosition.longitude / 1.0e7; - // RCLCPP_INFO(node_->get_logger(), "cpm.(reference position) = %f, %f", lat, lon); - std::string mgrs; int zone; int grid_num_x = 4; int grid_num_y = 39; int grid_size = 100000; bool northp; double x, y; - GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, x, y); - // GeographicLib::MGRS::Forward(zone, northp, x, y, lat, 5, mgrs); + GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, x, y); double x_mgrs = x - grid_num_x * grid_size; double y_mgrs = y - grid_num_y * grid_size; - // RCLCPP_INFO(node_->get_logger(), "cpm.(RP).mgrs = %s, %d, %d", mgrs.c_str(), x_mgrs, y_mgrs); - // Calculate orientation from Heading OriginatingVehicleContainer_t &ovc = message->cpm.cpmParameters.stationDataContainer->choice.originatingVehicleContainer; + + // Calculate ego-vehicle orientation (radians) from heading (degree). + // orientation: True-East, counter-clockwise angle. (0.1 degree accuracy) int heading = ovc.heading.headingValue; - // double orientation = 1.5708 - (M_PI * heading / 10) / 180; - double orientation = (90.0 - heading / 10.0) * M_PI / 180; - if (orientation < 0.0) orientation += (2 * M_PI); - // RCLCPP_INFO(node_->get_logger(), "cpm: heading = %d, orientation = %f", heading, orientation); + double orientation = (90.0 - (double) heading / 10.0) * M_PI / 180.0; + if (orientation < 0.0) orientation += (2.0 * M_PI); + // double orientation = heading / 10.0; + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] heading: %d", heading); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] orientation: %f", orientation); // Get PerceivedObjects receivedObjectsStack.clear(); @@ -120,24 +122,21 @@ namespace v2x { if (poc != NULL) { for (int i = 0; i < poc->list.count; ++i) { - RCLCPP_INFO(node_->get_logger(), "[INDICATE] Object: #%d", poc->list.array[i]->objectID); + // RCLCPP_INFO(node_->get_logger(), "[INDICATE] Object: #%d", poc->list.array[i]->objectID); CpmApplication::Object object; double x1 = poc->list.array[i]->xDistance.value; double y1 = poc->list.array[i]->yDistance.value; - // 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); - + object.position_y = y_mgrs + (sin(orientation) * x1 + cos(orientation) * y1); object.shape_x = poc->list.array[i]->planarObjectDimension2->value; object.shape_y = poc->list.array[i]->planarObjectDimension1->value; object.shape_z = poc->list.array[i]->verticalObjectDimension->value; object.yawAngle = poc->list.array[i]->yawAngle->value; - double yaw_radian = (M_PI * object.yawAngle / 10) / 180; + double yaw_radian = (M_PI * object.yawAngle / 10.0) / 180.0; tf2::Quaternion quat; quat.setRPY(0, 0, yaw_radian); @@ -145,6 +144,7 @@ namespace v2x { object.orientation_y = quat.y(); object.orientation_z = quat.z(); object.orientation_w = quat.w(); + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] object.quat: %f, %f, %f, %f", object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w); receivedObjectsStack.push_back(object); } @@ -180,6 +180,7 @@ namespace v2x { void CpmApplication::updateMGRS(double *x, double *y) { ego_.mgrs_x = *x; ego_.mgrs_y = *y; + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateMGRS] ego-vehicle.position: %.10f, %.10f", ego_.mgrs_x, ego_.mgrs_y); } void CpmApplication::updateRP(double *lat, double *lon, double *altitude) { @@ -223,10 +224,14 @@ 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_.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); + + // xDistance, yDistance: Int (-132768..132767), 0.01 meter accuracy + 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; } @@ -236,6 +241,7 @@ namespace v2x { object.xSpeed = 0; object.ySpeed = 0; + // Calculate orientation of detected object tf2::Quaternion quat(object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w); tf2::Matrix3x3 matrix(quat); double roll, pitch, yaw; @@ -245,6 +251,8 @@ namespace v2x { } else { object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600 } + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] object.yawAngle: %d", object.yawAngle); + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] object.quat: %f, %f, %f, %f", object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w); long long msg_timestamp_sec = msg->header.stamp.sec; long long msg_timestamp_nsec = msg->header.stamp.nanosec; @@ -260,12 +268,12 @@ namespace v2x { objectsStack.push_back(object); ++i; - 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); + // 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); } } - RCLCPP_INFO(node_->get_logger(), "ObjectsStack: %d objects", objectsStack.size()); + // RCLCPP_INFO(node_->get_logger(), "ObjectsStack: %d objects", objectsStack.size()); rclcpp::Time current_time = node_->now(); - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] [measure] T_objstack_updated %ld", current_time.nanoseconds()); + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] [measure] T_objstack_updated %ld", current_time.nanoseconds()); updating_objects_stack_ = false; } @@ -274,7 +282,7 @@ namespace v2x { if (is_sender_) { sending_ = true; - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM..."); + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM..."); vanetza::asn1::Cpm message; @@ -291,12 +299,8 @@ namespace v2x { CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer; management.stationType = StationType_passengerCar; - PositionFix fix; - 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); + management.referencePosition.latitude = ego_.latitude * 1e7; + management.referencePosition.longitude = ego_.longitude * 1e7; cpm.cpmParameters.numberOfPerceivedObjects = objectsStack.size(); StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer; @@ -305,6 +309,9 @@ namespace v2x { OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer; ovc.speed.speedValue = 0; ovc.speed.speedConfidence = 1; + + // Calculate headingValue of ego-vehicle. + // Convert ego-vehicle yaw to True-North clockwise angle (heading). 0.1 degree accuracy. int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0); if (heading < 0) heading += 3600; ovc.heading.headingValue = heading; @@ -342,7 +349,7 @@ namespace v2x { (*(pObj->yawAngle)).value = object.yawAngle; (*(pObj->yawAngle)).confidence = 1; - RCLCPP_INFO(node_->get_logger(), "[SEND] Added: #%d (%d, %d) (%d, %d) (%d, %d, %d) %d", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_y, object.shape_x, object.shape_z, object.yawAngle); + // RCLCPP_INFO(node_->get_logger(), "[SEND] Added: #%d (%d, %d) (%d, %d) (%d, %d, %d) %d", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_y, object.shape_x, object.shape_z, object.yawAngle); ASN_SEQUENCE_ADD(poc, pObj); } @@ -369,7 +376,7 @@ namespace v2x { sending_ = false; rclcpp::Time current_time = node_->now(); - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds()); + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds()); } }