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> <launch>
<arg name="link_layer" default="ethernet"/>
<arg name="network_interface" default="v2x_testing"/> <arg name="network_interface" default="v2x_testing"/>
<arg name="cube_ip" default="127.0.0.1"/>
<arg name="is_sender" default="true"/> <arg name="is_sender" default="true"/>
<arg name="security" default="none"/>
<node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen"> <node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
<param from="$(find-pkg-share autoware_v2x)/config/autoware_v2x.param.yaml"/> <param from="$(find-pkg-share autoware_v2x)/config/autoware_v2x.param.yaml"/>
</node> </node>

9
src/cam_application.cpp
View File

@ -11,6 +11,7 @@
#include <vanetza/btp/ports.hpp> #include <vanetza/btp/ports.hpp>
#include <vanetza/asn1/cam.hpp> #include <vanetza/asn1/cam.hpp>
#include <vanetza/asn1/packet_visitor.hpp> #include <vanetza/asn1/packet_visitor.hpp>
#include <vanetza/facilities/cam_functions.hpp>
#include <chrono> #include <chrono>
#include <functional> #include <functional>
#include <iostream> #include <iostream>
@ -95,10 +96,12 @@ namespace v2x
} }
asn1::Cam rec_cam = *rec_cam_ptr; 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()); 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; cam_ts_CAM_t ts_cam;
std::memset(&ts_cam, 0, sizeof(ts_cam));
cam_ts_ItsPduHeader_t &header = ts_cam.header; cam_ts_ItsPduHeader_t &header = ts_cam.header;
header.protocolVersion = rec_cam->header.protocolVersion; header.protocolVersion = rec_cam->header.protocolVersion;
@ -139,6 +142,7 @@ namespace v2x
bvc.yawRate.yawRateConfidence = rec_bvc.yawRate.yawRateConfidence; bvc.yawRate.yawRateConfidence = rec_bvc.yawRate.yawRateConfidence;
etsi_its_cam_ts_msgs::msg::CAM ros_msg; 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); etsi_its_cam_ts_conversion::toRos_CAM(ts_cam, ros_msg);
node_->publishReceivedCam(ros_msg); node_->publishReceivedCam(ros_msg);
@ -227,7 +231,6 @@ namespace v2x
// Convert Unix timestamp to ETSI epoch (2004-01-01 00:00:00) // Convert Unix timestamp to ETSI epoch (2004-01-01 00:00:00)
cam.generationDeltaTime = (now_ms.count() - 1072915200000) % 65536; 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; BasicContainer_t &basic_container = cam.camParameters.basicContainer;
basic_container.stationType = StationType_passengerCar; basic_container.stationType = StationType_passengerCar;
@ -345,7 +348,7 @@ namespace v2x
bvc.yawRate.yawRateConfidence = YawRateConfidence_unavailable; 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()}; std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
payload->layer(OsiLayer::Application) = std::move(message); payload->layer(OsiLayer::Application) = std::move(message);

32
src/cpm_application.cpp
View File

@ -32,7 +32,7 @@ using namespace vanetza;
using namespace std::chrono; using namespace std::chrono;
namespace v2x { namespace v2x {
CpmApplication::CpmApplication(V2XNode *node, Runtime &rt, bool is_sender) : CpmApplication::CpmApplication(V2XNode *node, Runtime &rt, bool is_sender) :
node_(node), node_(node),
runtime_(rt), runtime_(rt),
ego_(), ego_(),
@ -101,7 +101,8 @@ namespace v2x {
// Calculate GDT and get GDT from CPM and calculate the "Age of CPM" // 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()); // const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch());
// uint16_t gdt = time_now.count(); // uint16_t gdt = time_now.count();
// int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536; // int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536;
@ -156,7 +157,7 @@ namespace v2x {
x1 = x1 / 100.0; x1 = x1 / 100.0;
y1 = y1 / 100.0; y1 = y1 / 100.0;
object.position_x = x_mgrs + (cos(orientation) * x1 - sin(orientation) * y1); object.position_x = x_mgrs + (cos(orientation) * x1 - sin(orientation) * y1);
object.position_y = y_mgrs + (sin(orientation) * x1 + cos(orientation) * y1); object.position_y = y_mgrs + (sin(orientation) * x1 + cos(orientation) * y1);
object.shape_x = poc->list.array[i]->planarObjectDimension2->value; object.shape_x = poc->list.array[i]->planarObjectDimension2->value;
object.shape_y = poc->list.array[i]->planarObjectDimension1->value; object.shape_y = poc->list.array[i]->planarObjectDimension1->value;
object.shape_z = poc->list.array[i]->verticalObjectDimension->value; object.shape_z = poc->list.array[i]->verticalObjectDimension->value;
@ -236,7 +237,7 @@ namespace v2x {
}); });
if (found_object == objectsList.end()) { if (found_object == objectsList.end()) {
} else { } else {
found_object->to_send = true; found_object->to_send = true;
} }
@ -317,7 +318,7 @@ namespace v2x {
++cpm_object_id_; ++cpm_object_id_;
} else { } else {
// Object was already in internal memory // Object was already in internal memory
// Object belongs to class person or animal // Object belongs to class person or animal
@ -327,7 +328,7 @@ namespace v2x {
found_object->to_send = true; found_object->to_send = true;
found_object->to_send_trigger = 5; found_object->to_send_trigger = 5;
} }
// Object has not been included in a CPM in the past 500 ms. // Object has not been included in a CPM in the past 500 ms.
if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 500000) { if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 500000) {
// Include all objects of class person or animal in the current CPM // Include all objects of class person or animal in the current CPM
@ -349,7 +350,7 @@ namespace v2x {
found_object->to_send = true; found_object->to_send = true;
found_object->to_send_trigger = 1; found_object->to_send_trigger = 1;
} else { } else {
} }
// Absolute speed changed by more than 0.5 m/s // Absolute speed changed by more than 0.5 m/s
@ -359,7 +360,7 @@ namespace v2x {
if (speed > 0.5) { if (speed > 0.5) {
found_object->to_send = true; found_object->to_send = true;
found_object->to_send_trigger = 2; found_object->to_send_trigger = 2;
} }
// Orientation of speed vector changed by more than 4 degrees // Orientation of speed vector changed by more than 4 degrees
double twist_angular_x_diff = (obj.kinematics.initial_twist_with_covariance.twist.angular.x - found_object->twist_angular_x) * 180 / M_PI; double twist_angular_x_diff = (obj.kinematics.initial_twist_with_covariance.twist.angular.x - found_object->twist_angular_x) * 180 / M_PI;
@ -434,7 +435,7 @@ namespace v2x {
} else { } else {
RCLCPP_INFO(node_->get_logger(), "[objectsList] %d,,,,", cpm_num); RCLCPP_INFO(node_->get_logger(), "[objectsList] %d,,,,", cpm_num);
} }
// RCLCPP_INFO(node_->get_logger(), "------------------------"); // RCLCPP_INFO(node_->get_logger(), "------------------------");
} }
@ -445,7 +446,7 @@ namespace v2x {
sending_ = true; sending_ = true;
printObjectsList(cpm_num_); printObjectsList(cpm_num_);
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM..."); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM...");
vanetza::asn1::Cpm message; vanetza::asn1::Cpm message;
@ -460,7 +461,8 @@ namespace v2x {
// Set GenerationTime // Set GenerationTime
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_); 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; CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
management.stationType = StationType_passengerCar; management.stationType = StationType_passengerCar;
@ -578,7 +580,7 @@ namespace v2x {
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM with %d objects", perceivedObjectsCount); RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM with %d objects", perceivedObjectsCount);
insertCpmToCpmTable(message, (char*) "cpm_sent"); insertCpmToCpmTable(message, (char*) "cpm_sent");
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()}; std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
payload->layer(OsiLayer::Application) = std::move(message); payload->layer(OsiLayer::Application) = std::move(message);
@ -618,7 +620,7 @@ namespace v2x {
} }
char* sql_command; char* sql_command;
sql_command = (char*) "create table if not exists cpm_sent(id INTEGER PRIMARY KEY, timestamp BIGINT, perceivedObjectCount INTEGER);"; sql_command = (char*) "create table if not exists cpm_sent(id INTEGER PRIMARY KEY, timestamp BIGINT, perceivedObjectCount INTEGER);";
ret = sqlite3_exec(db, sql_command, NULL, NULL, &err); ret = sqlite3_exec(db, sql_command, NULL, NULL, &err);
@ -661,7 +663,7 @@ namespace v2x {
} }
std::stringstream sql_command; std::stringstream sql_command;
sql_command << "insert into " << table_name << " (timestamp, perceivedObjectCount) values (" << timestamp << ", " << perceivedObjectCount << ");"; sql_command << "insert into " << table_name << " (timestamp, perceivedObjectCount) values (" << timestamp << ", " << perceivedObjectCount << ");";
ret = sqlite3_exec(db, sql_command.str().c_str(), NULL, NULL, &err); ret = sqlite3_exec(db, sql_command.str().c_str(), NULL, NULL, &err);
@ -677,4 +679,4 @@ namespace v2x {
sqlite3_close(db); sqlite3_close(db);
// RCLCPP_INFO(node_->get_logger(), "CpmApplication::insertCpmToCpmTable Finished"); // RCLCPP_INFO(node_->get_logger(), "CpmApplication::insertCpmToCpmTable Finished");
} }
} }

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) { V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) {
using std::placeholders::_1; 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"); 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))) { if (get_vehicle_dimensions_->wait_for_service(std::chrono::seconds(timeout))) {
RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] Service /api/vehicle/dimensions is now available."); 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}); cam_rec_pub_ = create_publisher<etsi_its_cam_ts_msgs::msg::CAM>("/v2x/cam_ts/received", rclcpp::QoS{10});
// Declare Parameters // 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>("network_interface", "v2x_testing");
this->declare_parameter<std::string>("cube_ip", "127.0.0.1"); this->declare_parameter<std::string>("cube_ip", "127.0.0.1");
this->declare_parameter<bool>("is_sender", true); this->declare_parameter<bool>("is_sender", true);