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

latency logging, etc.

This commit is contained in:
Yu Asabe 2022-07-21 09:13:25 +09:00
parent 4a5baf0db1
commit 7796833021
3 changed files with 90 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
src/cpm_application.cpp
View File

@ -47,7 +47,8 @@ namespace v2x {
include_all_persons_and_animals_(false),
cpm_num_(0),
received_cpm_num_(0),
cpm_object_id_(0)
cpm_object_id_(0),
use_dynamic_generation_rules_(true)
{
RCLCPP_INFO(node_->get_logger(), "CpmApplication started. is_sender: %d", is_sender_);
set_interval(milliseconds(100));
@ -95,6 +96,12 @@ namespace v2x {
asn1::Cpm message = *cpm;
ItsPduHeader_t &header = message->header;
std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> (
std::chrono::system_clock::now().time_since_epoch()
);
node_->latency_log_file << "T_received," << header.stationID << "," << ms.count() << std::endl;
// Calculate GDT and get GDT from CPM and calculate the "Age of CPM"
GenerationDeltaTime_t gdt_cpm = message->cpm.generationDeltaTime;
const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch());
@ -169,7 +176,7 @@ namespace v2x {
receivedObjectsStack.push_back(object);
}
node_->publishObjects(&receivedObjectsStack);
node_->publishObjects(&receivedObjectsStack, header.stationID);
} else {
// RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC");
}
@ -243,12 +250,13 @@ namespace v2x {
void CpmApplication::updateObjectsList(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
updating_objects_list_ = true;
// if (sending_) {
// RCLCPP_INFO(node_->get_logger(), "updateObjectsStack Skipped...");
// return;
// } else {
// // objectsList.clear();
// }
// Flag all objects as NOT_SEND
if (objectsList.size() > 0) {
for (auto& object : objectsList) {
object.to_send = false;
object.to_send_trigger = -1;
}
}
if (msg->objects.size() > 0) {
for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) {
@ -303,7 +311,7 @@ namespace v2x {
continue;
}
object.to_send = false;
object.to_send = true;
object.to_send_trigger = 0;
object.timestamp = runtime_.now();
@ -400,6 +408,12 @@ namespace v2x {
found_object->timestamp = runtime_.now();
// if use_dymanic_generation_rules_ == false, then always include object in CPM
if (!use_dynamic_generation_rules_) {
found_object->to_send = true;
found_object->to_send_trigger = 0;
}
}
}
}
@ -432,7 +446,7 @@ namespace v2x {
sending_ = true;
// printObjectsList(cpm_num_);
printObjectsList(cpm_num_);
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM...");
@ -442,7 +456,7 @@ namespace v2x {
ItsPduHeader_t &header = message->header;
header.protocolVersion = 1;
header.messageID = 14;
header.stationID = 1;
header.stationID = cpm_num_;
CollectivePerceptionMessage_t &cpm = message->cpm;
@ -539,7 +553,8 @@ namespace v2x {
ASN_SEQUENCE_ADD(poc, pObj);
object.to_send = false;
// object.to_send = false;
// object.to_send_trigger = -1;
// RCLCPP_INFO(node_->get_logger(), "Sending object: %s", object.uuid.c_str());
++perceivedObjectsCount;
@ -550,18 +565,21 @@ namespace v2x {
}
}
cpm.cpmParameters.numberOfPerceivedObjects =perceivedObjectsCount;
cpm.cpmParameters.numberOfPerceivedObjects = perceivedObjectsCount;
if (perceivedObjectsCount == 0) {
cpm.cpmParameters.perceivedObjectContainer = NULL;
}
} else {
cpm.cpmParameters.perceivedObjectContainer = NULL;
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Empty POC");
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Empty POC");
}
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM with %ld objects", objectsList.size());
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM with %d objects", perceivedObjectsCount);
insertCpmToCpmTable(message, (char*) "cpm_sent");
Application::DownPacketPtr packet{new DownPacket()};
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
payload->layer(OsiLayer::Application) = std::move(message);
@ -578,9 +596,14 @@ namespace v2x {
}
sending_ = false;
rclcpp::Time current_time = node_->now();
// rclcpp::Time current_time = node_->now();
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds());
std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> (
std::chrono::system_clock::now().time_since_epoch()
);
node_->latency_log_file << "T_depart," << cpm_num_ << "," << ms.count() << std::endl;
++cpm_num_;
}
}
@ -601,7 +624,7 @@ namespace v2x {
ret = sqlite3_exec(db, sql_command, NULL, NULL, &err);
if (ret != SQLITE_OK) {
RCLCPP_INFO(node_->get_logger(), "DB Execution Error");
RCLCPP_INFO(node_->get_logger(), "DB Execution Error (create table cpm_sent)");
sqlite3_close(db);
sqlite3_free(err);
return;
@ -611,7 +634,7 @@ namespace v2x {
ret = sqlite3_exec(db, sql_command, NULL, NULL, &err);
if (ret != SQLITE_OK) {
RCLCPP_INFO(node_->get_logger(), "DB Execution Error");
RCLCPP_INFO(node_->get_logger(), "DB Execution Error (create table cpm_received)");
sqlite3_close(db);
sqlite3_free(err);
return;
@ -644,7 +667,9 @@ namespace v2x {
ret = sqlite3_exec(db, sql_command.str().c_str(), NULL, NULL, &err);
if (ret != SQLITE_OK) {
RCLCPP_INFO(node_->get_logger(), "DB Execution Error");
RCLCPP_INFO(node_->get_logger(), "DB Execution Error (insertCpmToCpmTable)");
RCLCPP_INFO(node_->get_logger(), sql_command.str().c_str());
RCLCPP_INFO(node_->get_logger(), err);
sqlite3_close(db);
sqlite3_free(err);
return;

5
src/raw_socket_link.cpp
View File

@ -2,6 +2,7 @@
#include <iostream>
#include <vanetza/access/data_request.hpp>
#include <vanetza/net/ethernet_header.hpp>
#include <chrono>
using namespace vanetza;
@ -33,7 +34,9 @@ std::size_t RawSocketLink::transmit(std::unique_ptr<ChunkPacket> packet)
return socket_.send(const_buffers);
}
void RawSocketLink::indicate(IndicationCallback callback) { callback_ = callback; }
void RawSocketLink::indicate(IndicationCallback callback) {
callback_ = callback;
}
void RawSocketLink::do_receive()
{

48
src/v2x_node.cpp
View File

@ -18,6 +18,8 @@
#include <boost/date_time/posix_time/posix_time.hpp>
#include "tf2/LinearMath/Quaternion.h"
#include <chrono>
#include <iostream>
namespace gn = vanetza::geonet;
@ -44,8 +46,32 @@ namespace v2x
RCLCPP_INFO(get_logger(), "V2X Node Launched");
rclcpp::Time current_time = this->now();
RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] [measure] T_R1 %ld", current_time.nanoseconds());
//rclcpp::Time current_time = this->now();
//RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] [measure] T_R1 %ld", current_time.nanoseconds());
time_t t = time(nullptr);
const tm* lt = localtime(&t);
std::stringstream s;
s<<"20";
s<<lt->tm_year-100; //100を引くことで20xxのxxの部分になる
s<<"-";
s<<lt->tm_mon+1; //月を0からカウントしているため
s<<"-";
s<<lt->tm_mday; //そのまま
s<<"_";
s<<lt->tm_hour;
s<<":";
s<<lt->tm_min;
s<<":";
s<<lt->tm_sec;
std::string timestamp = s.str();
char cur_dir[1024];
getcwd(cur_dir, 1024);
std::string latency_log_filename = std::string(cur_dir) + "/latency_logs/latency_log_file_" + timestamp + ".csv";
latency_log_file.open(latency_log_filename, std::ios::out);
// latency_log_file << "test hello" << std::endl;
}
@ -91,7 +117,7 @@ namespace v2x
}
void V2XNode::publishObjects(std::vector<CpmApplication::Object> *objectsStack) {
void V2XNode::publishObjects(std::vector<CpmApplication::Object> *objectsStack, int cpm_num) {
autoware_auto_perception_msgs::msg::PredictedObjects output_dynamic_object_msg;
std_msgs::msg::Header header;
rclcpp::Time current_time = this->now();
@ -128,20 +154,28 @@ namespace v2x
output_dynamic_object_msg.objects.push_back(object);
}
current_time = this->now();
// RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds());
std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> (
std::chrono::system_clock::now().time_since_epoch()
);
latency_log_file << "T_publish," << cpm_num << "," << ms.count() << std::endl;
publisher_->publish(output_dynamic_object_msg);
}
void V2XNode::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
rclcpp::Time current_time = this->now();
// rclcpp::Time current_time = this->now();
rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg.
// RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size());
// Measuring T_A1R1
// RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj %ld", msg_time.nanoseconds());
// RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj_receive %ld", current_time.nanoseconds());
// RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_rosmsg %ld", current_time.nanoseconds());
std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> (
std::chrono::system_clock::now().time_since_epoch()
);
latency_log_file << "T_rosmsg,," << ms.count() << std::endl;
app->objectsCallback(msg);
}