Fix memory issues when creating receiving ts_cam struct
Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>
This commit is contained in:
parent
43a96d84de
commit
b249bc0ed2
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ using namespace vanetza;
|
|||
using namespace std::chrono;
|
||||
|
||||
namespace v2x {
|
||||
CpmApplication::CpmApplication(V2XNode *node, Runtime &rt, bool is_sender) :
|
||||
CpmApplication::CpmApplication(V2XNode *node, Runtime &rt, bool is_sender) :
|
||||
node_(node),
|
||||
runtime_(rt),
|
||||
ego_(),
|
||||
|
|
@ -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(>_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;
|
||||
|
|
@ -156,7 +157,7 @@ namespace v2x {
|
|||
x1 = x1 / 100.0;
|
||||
y1 = y1 / 100.0;
|
||||
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_y = poc->list.array[i]->planarObjectDimension1->value;
|
||||
object.shape_z = poc->list.array[i]->verticalObjectDimension->value;
|
||||
|
|
@ -236,7 +237,7 @@ namespace v2x {
|
|||
});
|
||||
|
||||
if (found_object == objectsList.end()) {
|
||||
|
||||
|
||||
} else {
|
||||
found_object->to_send = true;
|
||||
}
|
||||
|
|
@ -317,7 +318,7 @@ namespace v2x {
|
|||
++cpm_object_id_;
|
||||
|
||||
} else {
|
||||
|
||||
|
||||
// Object was already in internal memory
|
||||
|
||||
// Object belongs to class person or animal
|
||||
|
|
@ -327,7 +328,7 @@ namespace v2x {
|
|||
found_object->to_send = true;
|
||||
found_object->to_send_trigger = 5;
|
||||
}
|
||||
|
||||
|
||||
// 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) {
|
||||
// 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_trigger = 1;
|
||||
} else {
|
||||
|
||||
|
||||
}
|
||||
|
||||
// Absolute speed changed by more than 0.5 m/s
|
||||
|
|
@ -359,7 +360,7 @@ namespace v2x {
|
|||
if (speed > 0.5) {
|
||||
found_object->to_send = true;
|
||||
found_object->to_send_trigger = 2;
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
|
@ -434,7 +435,7 @@ namespace v2x {
|
|||
} else {
|
||||
RCLCPP_INFO(node_->get_logger(), "[objectsList] %d,,,,", cpm_num);
|
||||
}
|
||||
|
||||
|
||||
// RCLCPP_INFO(node_->get_logger(), "------------------------");
|
||||
}
|
||||
|
||||
|
|
@ -445,7 +446,7 @@ namespace v2x {
|
|||
sending_ = true;
|
||||
|
||||
printObjectsList(cpm_num_);
|
||||
|
||||
|
||||
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM...");
|
||||
|
||||
vanetza::asn1::Cpm message;
|
||||
|
|
@ -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;
|
||||
|
|
@ -578,7 +580,7 @@ namespace v2x {
|
|||
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM with %d objects", perceivedObjectsCount);
|
||||
|
||||
insertCpmToCpmTable(message, (char*) "cpm_sent");
|
||||
|
||||
|
||||
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
|
||||
|
||||
payload->layer(OsiLayer::Application) = std::move(message);
|
||||
|
|
@ -618,7 +620,7 @@ namespace v2x {
|
|||
}
|
||||
|
||||
char* sql_command;
|
||||
|
||||
|
||||
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);
|
||||
|
|
@ -661,7 +663,7 @@ namespace v2x {
|
|||
}
|
||||
|
||||
std::stringstream sql_command;
|
||||
|
||||
|
||||
sql_command << "insert into " << table_name << " (timestamp, perceivedObjectCount) values (" << timestamp << ", " << perceivedObjectCount << ");";
|
||||
|
||||
ret = sqlite3_exec(db, sql_command.str().c_str(), NULL, NULL, &err);
|
||||
|
|
@ -677,4 +679,4 @@ namespace v2x {
|
|||
sqlite3_close(db);
|
||||
// RCLCPP_INFO(node_->get_logger(), "CpmApplication::insertCpmToCpmTable Finished");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue