TiagoRG
/
AutowareV2X
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix memory issues when creating receiving ts_cam struct 

Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>
This commit is contained in:
Tiago Garcia 2024-09-11 12:25:32 +01:00
parent 43a96d84de
commit b249bc0ed2
No known key found for this signature in database
GPG Key ID: A3B1096AD19E1688
4 changed files with 28 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
launch/v2x.launch.xml
View File

@ -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>

9
src/cam_application.cpp
View File

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

6
src/cpm_application.cpp
View File

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

4
src/v2x_node.cpp
View File

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