From e9b85635a381fd80ae66f7d8a202a4427fbcc1f7 Mon Sep 17 00:00:00 2001 From: Tiago Garcia Date: Mon, 28 Oct 2024 10:19:01 +0000 Subject: [PATCH] [FORGE] test Signed-off-by: Tiago Garcia --- CMakeLists.txt | 1 + config/autoware_v2x.param.yaml | 4 +- include/autoware_v2x/cam_application.hpp | 18 +++-- src/cam_application.cpp | 86 +++++++++++++----------- 4 files changed, 63 insertions(+), 46 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3561185..b4c9a48 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(GeographicLib 1.37 REQUIRED) find_package(Boost COMPONENTS thread program_options REQUIRED) find_package(etsi_its_cam_ts_coding REQUIRED) find_package(etsi_its_cam_ts_conversion REQUIRED) +find_package(etsi_its_msgs_utils REQUIRED) ament_auto_find_build_dependencies() find_package(std_msgs REQUIRED) find_package(Protobuf REQUIRED) diff --git a/config/autoware_v2x.param.yaml b/config/autoware_v2x.param.yaml index 5aaec51..b21724c 100644 --- a/config/autoware_v2x.param.yaml +++ b/config/autoware_v2x.param.yaml @@ -1,8 +1,8 @@ /**: ros__parameters: - link_layer: "ethernet" + link_layer: "cube-evk" network_interface: "v2x_testing" - cube_ip: "127.0.0.1" + cube_ip: "192.168.94.84" is_sender: true security: "none" certificate: "" diff --git a/include/autoware_v2x/cam_application.hpp b/include/autoware_v2x/cam_application.hpp index b1a77cf..5d68538 100644 --- a/include/autoware_v2x/cam_application.hpp +++ b/include/autoware_v2x/cam_application.hpp @@ -2,15 +2,20 @@ #define CAM_APPLICATION_HPP_EUIC2VFR #include "autoware_v2x/application.hpp" +#include "autoware_v2x/positioning.hpp" #include "rclcpp/rclcpp.hpp" -#include -#include -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" + +#include + +#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp" #include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp" -#include "autoware_v2x/positioning.hpp" -#include +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" + +#include +#include + +#include namespace v2x { @@ -35,6 +40,7 @@ private: void calc_interval(); void schedule_timer(); void on_timer(vanetza::Clock::time_point); + static void build_etsi_its_cam_ts_from_vanetza(vanetza::asn1::Cam &, cam_ts_CAM_t &); V2XNode *node_; vanetza::Runtime &runtime_; diff --git a/src/cam_application.cpp b/src/cam_application.cpp index 0c2650d..a8f714b 100644 --- a/src/cam_application.cpp +++ b/src/cam_application.cpp @@ -23,7 +23,6 @@ #include #include -#include #include #include @@ -160,43 +159,7 @@ namespace v2x cam_ts_CAM_t ts_cam; std::memset(&ts_cam, 0, sizeof(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; + CamApplication::build_etsi_its_cam_ts_from_vanetza(rec_cam, ts_cam); etsi_its_cam_ts_msgs::msg::CAM ros_msg; std::memset(&ros_msg, 0, sizeof(ros_msg)); @@ -419,6 +382,14 @@ 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"); } @@ -432,4 +403,43 @@ 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; + } }