From 7d22c7d5ca3f5586a2afacc903fd8b2551b32bb9 Mon Sep 17 00:00:00 2001 From: Tiago Garcia Date: Mon, 28 Oct 2024 10:19:01 +0000 Subject: [PATCH] Add the option to publish self generated CAMs to ROS network Signed-off-by: Tiago Garcia --- config/autoware_v2x.param.yaml | 5 ++- include/autoware_v2x/cam_application.hpp | 21 ++++++---- src/cam_application.cpp | 51 +++++++++++++++++++++++- src/v2x_app.cpp | 4 +- src/v2x_node.cpp | 1 + 5 files changed, 70 insertions(+), 12 deletions(-) diff --git a/config/autoware_v2x.param.yaml b/config/autoware_v2x.param.yaml index 5aaec51..a111343 100644 --- a/config/autoware_v2x.param.yaml +++ b/config/autoware_v2x.param.yaml @@ -1,9 +1,10 @@ /**: 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 + publish_own_cams: false security: "none" certificate: "" certificate-key: "" diff --git a/include/autoware_v2x/cam_application.hpp b/include/autoware_v2x/cam_application.hpp index b1a77cf..9141e3e 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 { @@ -18,7 +23,7 @@ class V2XNode; class CamApplication : public Application { public: - CamApplication(V2XNode *node, vanetza::Runtime &, bool is_sender); + CamApplication(V2XNode *node, vanetza::Runtime &, bool is_sender, bool publish_own_cams); PortType port() override; void indicate(const DataIndication &, UpPacketPtr) override; void set_interval(vanetza::Clock::duration); @@ -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_; @@ -135,6 +141,7 @@ private: bool sending_; bool is_sender_; + bool publish_own_cams_; unsigned long stationId_; diff --git a/src/cam_application.cpp b/src/cam_application.cpp index 0c2650d..efb07ea 100644 --- a/src/cam_application.cpp +++ b/src/cam_application.cpp @@ -36,7 +36,7 @@ using namespace std::chrono; namespace v2x { - CamApplication::CamApplication(V2XNode * node, Runtime & rt, bool is_sender) + CamApplication::CamApplication(V2XNode * node, Runtime & rt, bool is_sender, bool publish_own_cams) : node_(node), runtime_(rt), cam_interval_(milliseconds(1000)), @@ -47,9 +47,10 @@ namespace v2x vehicleStatus_(), sending_(false), is_sender_(is_sender), + publish_own_cams_(publish_own_cams), use_dynamic_generation_rules_(true) { - RCLCPP_INFO(node_->get_logger(), "CamApplication started. is_sender: %d", is_sender_); + RCLCPP_INFO(node_->get_logger(), "CamApplication started. is_sender: %d, publish_own_cams: %d", is_sender_, publish_own_cams_); set_interval(cam_interval_); //createTables(); @@ -412,6 +413,52 @@ namespace v2x std::unique_ptr payload{new geonet::DownPacket()}; payload->layer(OsiLayer::Application) = std::move(message); + if (publish_own_cams_) { + cam_ts_CAM_t ts_cam; + std::memset(&ts_cam, 0, sizeof(ts_cam)); + + cam_ts_ItsPduHeader_t &ros_header = ts_cam.header; + ros_header.protocolVersion = header.protocolVersion; + ros_header.messageId = header.messageID; + ros_header.stationId = header.stationID; + + cam_ts_CamPayload_t &coopAwareness = ts_cam.cam; + coopAwareness.generationDeltaTime = cam.generationDeltaTime; + + 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; + + 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; + + 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); + } + Application::DataRequest request; request.its_aid = aid::CP; request.transport_type = geonet::TransportType::SHB; diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index f100a89..f423e19 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -189,9 +189,11 @@ namespace v2x context.set_link_layer(link_layer.get()); bool is_sender; + bool publish_own_cams; node_->get_parameter("is_sender", is_sender); + node_->get_parameter("publish_own_cams", publish_own_cams); cp = new CpmApplication(node_, trigger.runtime(), is_sender); - cam = new CamApplication(node_, trigger.runtime(), is_sender); + cam = new CamApplication(node_, trigger.runtime(), is_sender, publish_own_cams); context.enable(cp); context.enable(cam); diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index ee5c5ab..98af784 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -64,6 +64,7 @@ namespace v2x this->declare_parameter("network_interface", "v2x_testing"); this->declare_parameter("cube_ip", "127.0.0.1"); this->declare_parameter("is_sender", true); + this->declare_parameter("publish_own_cams", true); this->declare_parameter("security", "none"); // Launch V2XApp in a new thread