Add the option to publish self generated CAMs to ROS network

Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>

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: ""

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 <boost/asio/io_service.hpp>
-#include <boost/asio/steady_timer.hpp>
-#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
+
+#include <vanetza/asn1/cam.hpp>
+
+#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 <vanetza/asn1/cam.hpp>
+#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
+
+#include <boost/asio/io_service.hpp>
+#include <boost/asio/steady_timer.hpp>
+
+#include <etsi_its_cam_ts_coding/cam_ts_CAM.h>
 
 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_;
 

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<geonet::DownPacket> 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;

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);

src/v2x_node.cpp

@@ -64,6 +64,7 @@ namespace v2x
     this->declare_parameter<std::string>("network_interface", "v2x_testing");
     this->declare_parameter<std::string>("cube_ip", "127.0.0.1");
     this->declare_parameter<bool>("is_sender", true);
+    this->declare_parameter<bool>("publish_own_cams", true);
     this->declare_parameter<std::string>("security", "none");
 
     // Launch V2XApp in a new thread