Fix memory issues when creating receiving ts_cam struct

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

launch/v2x.launch.xml

@@ -1,6 +1,9 @@
 <launch>
+  <arg name="link_layer" default="ethernet"/>
   <arg name="network_interface" default="v2x_testing"/>
+  <arg name="cube_ip" default="127.0.0.1"/>
   <arg name="is_sender" default="true"/>
+  <arg name="security" default="none"/>
   <node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
     <param from="$(find-pkg-share autoware_v2x)/config/autoware_v2x.param.yaml"/>
   </node>

src/cam_application.cpp

@@ -11,6 +11,7 @@
 #include <vanetza/btp/ports.hpp>
 #include <vanetza/asn1/cam.hpp>
 #include <vanetza/asn1/packet_visitor.hpp>
+#include <vanetza/facilities/cam_functions.hpp>
 #include <chrono>
 #include <functional>
 #include <iostream>
@@ -95,10 +96,12 @@ namespace v2x
     }
 
     asn1::Cam rec_cam = *rec_cam_ptr;
-    RCLCPP_INFO(node_->get_logger(), "[CamApplication::indicate] Received CAM from station with ID #%ld", rec_cam->header.stationID);
     std::chrono::milliseconds now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
+    RCLCPP_INFO(node_->get_logger(), "[CamApplication::indicate] Received CAM from station with ID #%ld at %ld epoch time", rec_cam->header.stationID, now_ms.count());
+    vanetza::facilities::print_indented(std::cout, rec_cam, "  ", 0);
 
     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;
@@ -139,6 +142,7 @@ namespace v2x
     bvc.yawRate.yawRateConfidence = rec_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);
@@ -227,7 +231,6 @@ namespace v2x
 
     // Convert Unix timestamp to ETSI epoch (2004-01-01 00:00:00)
     cam.generationDeltaTime = (now_ms.count() - 1072915200000) % 65536;
-    RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM with generationDeltaTime %ld", cam.generationDeltaTime);
 
     BasicContainer_t &basic_container = cam.camParameters.basicContainer;
     basic_container.stationType = StationType_passengerCar;
@@ -345,7 +348,7 @@ namespace v2x
     bvc.yawRate.yawRateConfidence = YawRateConfidence_unavailable;
     // ------------------------------
 
-    RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM from station with ID %ld", stationId_);
+    RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM from station with ID %ld with generationDeltaTime %ld", stationId_, cam.generationDeltaTime);
     std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
     payload->layer(OsiLayer::Application) = std::move(message);
 

src/cpm_application.cpp

@@ -101,7 +101,8 @@ namespace v2x {
 
 
       // Calculate GDT and get GDT from CPM and calculate the "Age of CPM"
-      TimestampIts_t gt_cpm = message->cpm.generationTime;
+      TimestampIts_t gt_cpm;
+      asn_long2INTEGER(&gt_cpm, (long) message->cpm.generationDeltaTime);
       // const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch());
       // uint16_t gdt = time_now.count();
       // int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536;
@@ -460,7 +461,8 @@ namespace v2x {
 
       // Set GenerationTime
       RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_);
-      asn_long2INTEGER(&cpm.generationTime, (long) gdt_timestamp_);
+      //asn_long2INTEGER(&cpm.generationTime, (long) gdt_timestamp_);
+      cpm.generationDeltaTime = gdt_timestamp_;
 
       CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
       management.stationType = StationType_passengerCar;

src/v2x_node.cpp

@@ -36,7 +36,7 @@ namespace v2x
   V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) {
     using std::placeholders::_1;
 
-    int timeout = 60;
+    int timeout = 10;
     get_vehicle_dimensions_ = this->create_client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>("/api/vehicle/dimensions");
     if (get_vehicle_dimensions_->wait_for_service(std::chrono::seconds(timeout))) {
       RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] Service /api/vehicle/dimensions is now available.");
@@ -60,7 +60,7 @@ namespace v2x
     cam_rec_pub_ = create_publisher<etsi_its_cam_ts_msgs::msg::CAM>("/v2x/cam_ts/received", rclcpp::QoS{10});
 
     // Declare Parameters
-    this->declare_parameter<std::string>("link_layer", "cube-evk");
+    this->declare_parameter<std::string>("link_layer", "ethernet");
     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);