From 1bb8d51f702ec9a468037697646ba84f17b1f98e Mon Sep 17 00:00:00 2001 From: Tiago Garcia Date: Mon, 28 Oct 2024 11:20:08 +0000 Subject: [PATCH] [FORGE] fix Signed-off-by: Tiago Garcia --- src/cam_application.cpp | 138 ++++++++++++++++++++++++++-------------- 1 file changed, 90 insertions(+), 48 deletions(-) diff --git a/src/cam_application.cpp b/src/cam_application.cpp index a8f714b..3b39250 100644 --- a/src/cam_application.cpp +++ b/src/cam_application.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -159,7 +160,43 @@ namespace v2x cam_ts_CAM_t ts_cam; std::memset(&ts_cam, 0, sizeof(ts_cam)); - CamApplication::build_etsi_its_cam_ts_from_vanetza(rec_cam, ts_cam); + cam_ts_ItsPduHeader_t &header = ts_cam.header; + header.protocolVersion = rec_cam->header.protocolVersion; + header.messageId = rec_cam->header.messageID; + header.stationId = rec_cam->header.stationID; + + cam_ts_CamPayload_t &coopAwareness = ts_cam.cam; + coopAwareness.generationDeltaTime = rec_cam->cam.generationDeltaTime; + + cam_ts_BasicContainer_t &basic_container = coopAwareness.camParameters.basicContainer; + BasicContainer_t &rec_basic_container = rec_cam->cam.camParameters.basicContainer; + basic_container.stationType = rec_basic_container.stationType; + basic_container.referencePosition.latitude = rec_basic_container.referencePosition.latitude; + basic_container.referencePosition.longitude = rec_basic_container.referencePosition.longitude; + basic_container.referencePosition.altitude.altitudeValue = rec_basic_container.referencePosition.altitude.altitudeValue; + basic_container.referencePosition.altitude.altitudeConfidence = rec_basic_container.referencePosition.altitude.altitudeConfidence; + basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisLength = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence; + basic_container.referencePosition.positionConfidenceEllipse.semiMinorAxisLength = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence; + basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation; + + cam_ts_BasicVehicleContainerHighFrequency_t &bvc = coopAwareness.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; + coopAwareness.camParameters.highFrequencyContainer.present = cam_ts_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; + BasicVehicleContainerHighFrequency_t &rec_bvc = rec_cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; + bvc.heading.headingValue = rec_bvc.heading.headingValue; + bvc.heading.headingConfidence = rec_bvc.heading.headingConfidence; + bvc.speed.speedValue = rec_bvc.speed.speedValue; + bvc.speed.speedConfidence = rec_bvc.speed.speedConfidence; + bvc.driveDirection = rec_bvc.driveDirection; + bvc.vehicleLength.vehicleLengthValue = rec_bvc.vehicleLength.vehicleLengthValue; + bvc.vehicleLength.vehicleLengthConfidenceIndication = rec_bvc.vehicleLength.vehicleLengthConfidenceIndication; + bvc.vehicleWidth = rec_bvc.vehicleWidth; + bvc.longitudinalAcceleration.value = rec_bvc.longitudinalAcceleration.longitudinalAccelerationValue; + bvc.longitudinalAcceleration.confidence = rec_bvc.longitudinalAcceleration.longitudinalAccelerationConfidence; + bvc.curvature.curvatureValue = rec_bvc.curvature.curvatureValue; + bvc.curvature.curvatureConfidence = rec_bvc.curvature.curvatureConfidence; + bvc.curvatureCalculationMode = rec_bvc.curvatureCalculationMode; + bvc.yawRate.yawRateValue = rec_bvc.yawRate.yawRateValue; + bvc.yawRate.yawRateConfidence = rec_bvc.yawRate.yawRateConfidence; etsi_its_cam_ts_msgs::msg::CAM ros_msg; std::memset(&ros_msg, 0, sizeof(ros_msg)); @@ -375,6 +412,58 @@ namespace v2x std::unique_ptr payload{new geonet::DownPacket()}; payload->layer(OsiLayer::Application) = std::move(message); + RCLCPP_INFO(node_->get_logger(), "Trynna build ros_msg"); + cam_ts_CAM_t ts_cam; + std::memset(&ts_cam, 0, sizeof(ts_cam)); + + RCLCPP_INFO(node_->get_logger(), "Setting header"); + cam_ts_ItsPduHeader_t &ros_header = ts_cam.header; + ros_header.protocolVersion = header.protocolVersion; + ros_header.messageId = header.messageID; + ros_header.stationId = header.stationID; + + RCLCPP_INFO(node_->get_logger(), "Setting generationDeltaTime"); + cam_ts_CamPayload_t &coopAwareness = ts_cam.cam; + coopAwareness.generationDeltaTime = cam.generationDeltaTime; + + RCLCPP_INFO(node_->get_logger(), "Settings basic container"); + cam_ts_BasicContainer_t &ros_basic_container = coopAwareness.camParameters.basicContainer; + ros_basic_container.stationType = basic_container.stationType; + ros_basic_container.referencePosition.latitude = basic_container.referencePosition.latitude; + ros_basic_container.referencePosition.longitude = basic_container.referencePosition.longitude; + ros_basic_container.referencePosition.altitude.altitudeValue = basic_container.referencePosition.altitude.altitudeValue; + ros_basic_container.referencePosition.altitude.altitudeConfidence = basic_container.referencePosition.altitude.altitudeConfidence; + ros_basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisLength = basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence; + ros_basic_container.referencePosition.positionConfidenceEllipse.semiMinorAxisLength = basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence; + ros_basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation; + + RCLCPP_INFO(node_->get_logger(), "Setting bvc"); + cam_ts_BasicVehicleContainerHighFrequency_t &ros_bvc = coopAwareness.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; + coopAwareness.camParameters.highFrequencyContainer.present = cam_ts_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; + ros_bvc.heading.headingValue = bvc.heading.headingValue; + ros_bvc.heading.headingConfidence = bvc.heading.headingConfidence; + ros_bvc.speed.speedValue = bvc.speed.speedValue; + ros_bvc.speed.speedConfidence = bvc.speed.speedConfidence; + ros_bvc.driveDirection = bvc.driveDirection; + ros_bvc.vehicleLength.vehicleLengthValue = bvc.vehicleLength.vehicleLengthValue; + ros_bvc.vehicleLength.vehicleLengthConfidenceIndication = bvc.vehicleLength.vehicleLengthConfidenceIndication; + ros_bvc.vehicleWidth = bvc.vehicleWidth; + ros_bvc.longitudinalAcceleration.value = bvc.longitudinalAcceleration.longitudinalAccelerationValue; + ros_bvc.longitudinalAcceleration.confidence = bvc.longitudinalAcceleration.longitudinalAccelerationConfidence; + ros_bvc.curvature.curvatureValue = bvc.curvature.curvatureValue; + ros_bvc.curvature.curvatureConfidence = bvc.curvature.curvatureConfidence; + ros_bvc.curvatureCalculationMode = bvc.curvatureCalculationMode; + ros_bvc.yawRate.yawRateValue = bvc.yawRate.yawRateValue; + ros_bvc.yawRate.yawRateConfidence = bvc.yawRate.yawRateConfidence; + + RCLCPP_INFO(node_->get_logger(), "Converting to ROS"); + etsi_its_cam_ts_msgs::msg::CAM ros_msg; + std::memset(&ros_msg, 0, sizeof(ros_msg)); + etsi_its_cam_ts_conversion::toRos_CAM(ts_cam, ros_msg); + + RCLCPP_INFO(node_->get_logger(), "Publishing received CAM"); + node_->publishReceivedCam(ros_msg); + Application::DataRequest request; request.its_aid = aid::CP; request.transport_type = geonet::TransportType::SHB; @@ -382,14 +471,6 @@ namespace v2x Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); - cam_ts_CAM_t ts_cam; - std::memset(&ts_cam, 0, sizeof(ts_cam)); - CamApplication::build_etsi_its_cam_ts_from_vanetza(message, ts_cam); - etsi_its_cam_ts_msgs::msg::CAM ros_msg; - std::memset(&ros_msg, 0, sizeof(ros_msg)); - etsi_its_cam_ts_conversion::toRos_CAM(ts_cam, ros_msg); - node_->publishReceivedCam(ros_msg); - if (!confirm.accepted()) { throw std::runtime_error("[CamApplication::send] CAM application data request failed"); } @@ -403,43 +484,4 @@ namespace v2x ); } - void CamApplication::build_etsi_its_cam_ts_from_vanetza(vanetza::asn1::Cam &in, cam_ts_CAM_t &out) { - cam_ts_ItsPduHeader_t &header = out.header; - header.protocolVersion = in->header.protocolVersion; - header.messageId = in->header.messageID; - header.stationId = in->header.stationID; - - cam_ts_CamPayload_t &coopAwareness = out.cam; - coopAwareness.generationDeltaTime = in->cam.generationDeltaTime; - - cam_ts_BasicContainer_t &basic_container = coopAwareness.camParameters.basicContainer; - BasicContainer_t &rec_basic_container = in->cam.camParameters.basicContainer; - basic_container.stationType = rec_basic_container.stationType; - basic_container.referencePosition.latitude = rec_basic_container.referencePosition.latitude; - basic_container.referencePosition.longitude = rec_basic_container.referencePosition.longitude; - basic_container.referencePosition.altitude.altitudeValue = rec_basic_container.referencePosition.altitude.altitudeValue; - basic_container.referencePosition.altitude.altitudeConfidence = rec_basic_container.referencePosition.altitude.altitudeConfidence; - basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisLength = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence; - basic_container.referencePosition.positionConfidenceEllipse.semiMinorAxisLength = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence; - basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation; - - cam_ts_BasicVehicleContainerHighFrequency_t &bvc = coopAwareness.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; - coopAwareness.camParameters.highFrequencyContainer.present = cam_ts_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; - BasicVehicleContainerHighFrequency_t &rec_bvc = in->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; - bvc.heading.headingValue = rec_bvc.heading.headingValue; - bvc.heading.headingConfidence = rec_bvc.heading.headingConfidence; - bvc.speed.speedValue = rec_bvc.speed.speedValue; - bvc.speed.speedConfidence = rec_bvc.speed.speedConfidence; - bvc.driveDirection = rec_bvc.driveDirection; - bvc.vehicleLength.vehicleLengthValue = rec_bvc.vehicleLength.vehicleLengthValue; - bvc.vehicleLength.vehicleLengthConfidenceIndication = rec_bvc.vehicleLength.vehicleLengthConfidenceIndication; - bvc.vehicleWidth = rec_bvc.vehicleWidth; - bvc.longitudinalAcceleration.value = rec_bvc.longitudinalAcceleration.longitudinalAccelerationValue; - bvc.longitudinalAcceleration.confidence = rec_bvc.longitudinalAcceleration.longitudinalAccelerationConfidence; - bvc.curvature.curvatureValue = rec_bvc.curvature.curvatureValue; - bvc.curvature.curvatureConfidence = rec_bvc.curvature.curvatureConfidence; - bvc.curvatureCalculationMode = rec_bvc.curvatureCalculationMode; - bvc.yawRate.yawRateValue = rec_bvc.yawRate.yawRateValue; - bvc.yawRate.yawRateConfidence = rec_bvc.yawRate.yawRateConfidence; - } }