From 13fde9edd99570bf6ae3fee0b566199441d1dde7 Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Wed, 17 Nov 2021 17:45:25 +0900 Subject: [PATCH] fix sender bugs --- include/autoware_v2x/cpm_application.hpp | 4 +- src/cpm_application.cpp | 191 ++++++++++++----------- 2 files changed, 102 insertions(+), 93 deletions(-) diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index 11f3c15..d8c5c3e 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -41,8 +41,8 @@ namespace v2x int shape_z; int xDistance; int yDistance; - double xSpeed; - double ySpeed; + int xSpeed; + int ySpeed; int yawAngle; vanetza::PositionFix position; int timeOfMeasurement; diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index 6c7bc81..de955ed 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -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) @@ -171,20 +171,16 @@ namespace v2x void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { - // RCLCPP_INFO(node_->get_logger(), "Update ObjectsStack"); updating_objects_stack_ = true; - if (sending_) - { + if (sending_) { RCLCPP_INFO(node_->get_logger(), "updateObjectsStack Skipped..."); return; + } else { + objectsStack.clear(); } - if (msg->objects.size() > 0) - { - objectsStack.clear(); - - // Create CpmApplication::Object per msg->object and add it to objectsStack + if (msg->objects.size() > 0) { int i = 0; for (auto obj : msg->objects) { @@ -198,94 +194,98 @@ namespace v2x object.orientation_y = obj.state.pose_covariance.pose.orientation.y; object.orientation_z = obj.state.pose_covariance.pose.orientation.z; object.orientation_w = obj.state.pose_covariance.pose.orientation.w; - object.shape_x = (int) obj.shape.dimensions.x * 10; - object.shape_y = (int) obj.shape.dimensions.y * 10; - object.shape_z = (int) obj.shape.dimensions.z * 10; - object.xDistance = (int)((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100; - object.yDistance = (int)((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100; - // RCLCPP_INFO(node_->get_logger(), "xDistance: %d, yDistance: %d", object.xDistance, object.yDistance); + 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.xSpeed = 0; + object.ySpeed = 0; tf2::Quaternion quat(object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w); tf2::Matrix3x3 matrix(quat); double roll, pitch, yaw; matrix.getRPY(roll, pitch, yaw); - // RCLCPP_INFO(node_->get_logger(), "yaw: %f", yaw); - object.yawAngle = (int) (yaw * 180 / M_PI) * 10 ; // 0 - 3600 - + if (yaw < 0) { + object.yawAngle = std::lround(((yaw + 2*M_PI) * 180.0 / M_PI) * 10.0); // 0 - 3600 + } else { + object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600 + } + object.timeOfMeasurement = 100; 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); } - RCLCPP_INFO(node_->get_logger(), "%d objects added to objectsStack", objectsStack.size()); } + RCLCPP_INFO(node_->get_logger(), "ObjectsStack: %d objects", objectsStack.size()); updating_objects_stack_ = false; } void CpmApplication::send() { sending_ = true; - if (!updating_objects_stack_ && objectsStack.size() > 0) - { - RCLCPP_INFO(node_->get_logger(), "Sending CPM..."); + + RCLCPP_INFO(node_->get_logger(), "Sending CPM..."); - // Send all objects in 1 CPM message - vanetza::asn1::Cpm message; + vanetza::asn1::Cpm message; - // ITS PDU Header - ItsPduHeader_t &header = message->header; - header.protocolVersion = 1; - header.messageID = 14; - header.stationID = 1; + // ITS PDU Header + ItsPduHeader_t &header = message->header; + header.protocolVersion = 1; + header.messageID = 14; + header.stationID = 1; - // const auto time_now = duration_cast(system_clock::now().time_since_epoch()); - // uint16_t gen_delta_time = time_now.count(); + // const auto time_now = duration_cast(system_clock::now().time_since_epoch()); + // uint16_t gen_delta_time = time_now.count(); - CollectivePerceptionMessage_t &cpm = message->cpm; - cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec; + CollectivePerceptionMessage_t &cpm = message->cpm; + cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec; - // auto position = positioning->position_specify(pos_lat, pos_lon); + // auto position = positioning->position_specify(pos_lat, pos_lon); - CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer; - management.stationType = StationType_passengerCar; - // management.referencePosition.latitude = pos_lat; - // management.referencePosition.longitude = pos_lon; - // management.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 1.0; - // management.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 1.0; - PositionFix fix; - // fix.timestamp = time_now; - fix.latitude = ego_lat_ * units::degree; - fix.longitude = ego_lon_ * units::degree; - // fix.altitude = ego_altitude_; - fix.confidence.semi_major = 1.0 * units::si::meter; - fix.confidence.semi_minor = fix.confidence.semi_major; - copy(fix, management.referencePosition); + CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer; + management.stationType = StationType_passengerCar; + // management.referencePosition.latitude = pos_lat; + // management.referencePosition.longitude = pos_lon; + // management.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 1.0; + // management.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 1.0; + PositionFix fix; + // fix.timestamp = time_now; + fix.latitude = ego_lat_ * units::degree; + fix.longitude = ego_lon_ * units::degree; + // fix.altitude = ego_altitude_; + fix.confidence.semi_major = 1.0 * units::si::meter; + fix.confidence.semi_minor = fix.confidence.semi_major; + copy(fix, management.referencePosition); - // cpm.cpmParameters.stationDataContainer = NULL; - // cpm.cpmParameters.perceivedObjectContainer = NULL; - cpm.cpmParameters.numberOfPerceivedObjects = objectsStack.size(); + // cpm.cpmParameters.stationDataContainer = NULL; + // cpm.cpmParameters.perceivedObjectContainer = NULL; + cpm.cpmParameters.numberOfPerceivedObjects = objectsStack.size(); - StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer; - sdc = vanetza::asn1::allocate(); - // RCLCPP_INFO(node->get_logger(), "Allocated sdc"); - sdc->present = StationDataContainer_PR_originatingVehicleContainer; - OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer; - ovc.speed.speedValue = 0; - ovc.speed.speedConfidence = 1; - // ovc.heading.headingValue = (int) (1.5708 - ego_heading_) * M_PI / 180; - // RCLCPP_INFO(node_->get_logger(), "headingValue..."); - ovc.heading.headingValue = (int)std::fmod((1.5708 - ego_heading_) * 180 / M_PI, 360.0) * 10; - ovc.heading.headingConfidence = 1; + StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer; + sdc = vanetza::asn1::allocate(); + // RCLCPP_INFO(node->get_logger(), "Allocated sdc"); + sdc->present = StationDataContainer_PR_originatingVehicleContainer; + OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer; + ovc.speed.speedValue = 0; + ovc.speed.speedConfidence = 1; + // ovc.heading.headingValue = (int) (1.5708 - ego_heading_) * M_PI / 180; + // RCLCPP_INFO(node_->get_logger(), "headingValue..."); + ovc.heading.headingValue = (int)std::fmod((1.5708 - ego_heading_) * 180 / M_PI, 360.0) * 10; + ovc.heading.headingConfidence = 1; - // RCLCPP_INFO(node_->get_logger(), "PerceviedObjectContainer..."); + if (objectsStack.size() > 0) { PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; poc = vanetza::asn1::allocate(); - for (CpmApplication::Object object : objectsStack) - { - if (object.xDistance > 10000) continue; - if (object.yDistance > 10000) continue; + for (CpmApplication::Object object : objectsStack) { + // if (object.xDistance > 10000) continue; + // if (object.yDistance > 10000) continue; - // RCLCPP_INFO(node_->get_logger(), "Adding Object..."); PerceivedObject *pObj = vanetza::asn1::allocate(); pObj->objectID = object.objectID; pObj->timeOfMeasurement = object.timeOfMeasurement; @@ -302,8 +302,6 @@ namespace v2x pObj->planarObjectDimension2 = vanetza::asn1::allocate(); pObj->verticalObjectDimension = vanetza::asn1::allocate(); - RCLCPP_INFO(node_->get_logger(), "%d %d %d", object.shape_y, object.shape_x, object.shape_z); - (*(pObj->planarObjectDimension1)).value = object.shape_y; (*(pObj->planarObjectDimension1)).confidence = 1; (*(pObj->planarObjectDimension2)).value = object.shape_x; @@ -315,30 +313,41 @@ namespace v2x (*(pObj->yawAngle)).value = object.yawAngle; (*(pObj->yawAngle)).confidence = 1; + RCLCPP_INFO(node_->get_logger(), "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); - // RCLCPP_INFO(node_->get_logger(), "ADD: %d, %d, %d, %d, %d, %d", pObj->objectID, pObj->timeOfMeasurement, pObj->xDistance.value, pObj->yDistance.value, pObj->xSpeed.value, pObj->ySpeed.value); - } - Application::DownPacketPtr packet{new DownPacket()}; - std::unique_ptr payload{new geonet::DownPacket()}; - // std::shared_ptr message_p = std::make_shared(message); - // std::unique_ptr buffer { new convertible::byte_buffer_impl(&message)}; - - payload->layer(OsiLayer::Application) = std::move(message); - - Application::DataRequest request; - request.its_aid = aid::CP; - request.transport_type = geonet::TransportType::SHB; - request.communication_profile = geonet::CommunicationProfile::ITS_G5; - - try { - Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); - if (!confirm.accepted()) { - throw std::runtime_error("CPM application data request failed"); - } - } catch (...) { - RCLCPP_INFO(node_->get_logger(), "Request Failed"); } + } else { + cpm.cpmParameters.perceivedObjectContainer = NULL; + RCLCPP_INFO(node_->get_logger(), "Empty POC"); } + + Application::DownPacketPtr packet{new DownPacket()}; + std::unique_ptr payload{new geonet::DownPacket()}; + // std::shared_ptr message_p = std::make_shared(message); + // std::unique_ptr buffer { new convertible::byte_buffer_impl(&message)}; + + payload->layer(OsiLayer::Application) = std::move(message); + + Application::DataRequest request; + request.its_aid = aid::CP; + request.transport_type = geonet::TransportType::SHB; + request.communication_profile = geonet::CommunicationProfile::ITS_G5; + + Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); + + if (!confirm.accepted()) { + throw std::runtime_error("CPM application data request failed"); + } + + // try { + // Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); + // if (!confirm.accepted()) { + // throw std::runtime_error("CPM application data request failed"); + // } + // } catch (...) { + // RCLCPP_INFO(node_->get_logger(), "Request Failed"); + // } sending_ = false; // RCLCPP_INFO(node->get_logger(), "Application::request END"); }