diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..008692a --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +db/*.db \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 5227c07..f80c7f4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,7 +45,7 @@ ament_auto_add_library(autoware_v2x SHARED src/security.cpp ) -target_link_libraries(autoware_v2x Vanetza::vanetza ${GeographicLib_LIBRARIES} Boost::thread) +target_link_libraries(autoware_v2x Vanetza::vanetza ${GeographicLib_LIBRARIES} Boost::thread sqlite3) rclcpp_components_register_node(autoware_v2x PLUGIN "v2x::V2XNode" diff --git a/db/.gitplaceholder b/db/.gitplaceholder new file mode 100644 index 0000000..e69de29 diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index ea3aab5..c345de7 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -7,6 +7,7 @@ #include #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" #include "autoware_v2x/positioning.hpp" +#include namespace v2x { @@ -28,6 +29,18 @@ namespace v2x void printObjectsList(int); void send(); + /** Creates the database schema. + * Creates tables for cpm_sent, cpm_received + */ + void createTables(); + + /** Inserts CPM into the database. + * Constructs and executes queries for inserting the specified CPM data. + * @param cpm The CPM to be inserted + * @param table_name The table to insert the CPM into (cpm_sent or cpm_received) + */ + void insertCpmToCpmTable(vanetza::asn1::Cpm, char*); + struct Object { std::string uuid; int objectID; // 0-255 for CPM diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index 6cf4cf9..2504f98 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -24,6 +24,8 @@ #include #include +#include + #define _USE_MATH_DEFINES #include @@ -49,6 +51,7 @@ namespace v2x { { RCLCPP_INFO(node_->get_logger(), "CpmApplication started. is_sender: %d", is_sender_); set_interval(milliseconds(100)); + createTables(); } void CpmApplication::set_interval(Clock::duration interval) { @@ -135,7 +138,7 @@ namespace v2x { if (poc != NULL) { for (int i = 0; i < poc->list.count; ++i) { - RCLCPP_INFO(node_->get_logger(), "[INDICATE] Object: #%d", poc->list.array[i]->objectID); + // RCLCPP_INFO(node_->get_logger(), "[INDICATE] Object: #%d", poc->list.array[i]->objectID); CpmApplication::Object object; double x1 = poc->list.array[i]->xDistance.value; @@ -166,6 +169,8 @@ namespace v2x { // RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC"); } + insertCpmToCpmTable(message, (char*) "cpm_received"); + if (reflect_packet_) { Application::DownPacketPtr packet{new DownPacket()}; std::unique_ptr payload{new geonet::DownPacket()}; @@ -422,7 +427,7 @@ namespace v2x { sending_ = true; - printObjectsList(cpm_num_); + // printObjectsList(cpm_num_); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM..."); @@ -443,7 +448,6 @@ namespace v2x { management.stationType = StationType_passengerCar; management.referencePosition.latitude = ego_.latitude * 1e7; management.referencePosition.longitude = ego_.longitude * 1e7; - cpm.cpmParameters.numberOfPerceivedObjects = objectsList.size(); StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer; sdc = vanetza::asn1::allocate(); @@ -459,6 +463,8 @@ namespace v2x { ovc.heading.headingValue = heading; ovc.heading.headingConfidence = 1; + int perceivedObjectsCount = 0; + if (objectsList.size() > 0) { PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; poc = vanetza::asn1::allocate(); @@ -468,6 +474,7 @@ namespace v2x { // RCLCPP_INFO(node_->get_logger(), "object.to_send: %d", object.to_send); if (object.to_send) { + PerceivedObject *pObj = vanetza::asn1::allocate(); // Update CPM-specific values for Object @@ -528,15 +535,23 @@ namespace v2x { object.to_send = false; // RCLCPP_INFO(node_->get_logger(), "Sending object: %s", object.uuid.c_str()); + + ++perceivedObjectsCount; + } else { // Object.to_send is set to False // RCLCPP_INFO(node_->get_logger(), "Object: %s not being sent.", object.uuid.c_str()); } } + + cpm.cpmParameters.numberOfPerceivedObjects =perceivedObjectsCount; + } else { cpm.cpmParameters.perceivedObjectContainer = NULL; // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Empty POC"); } + + insertCpmToCpmTable(message, (char*) "cpm_sent"); Application::DownPacketPtr packet{new DownPacket()}; std::unique_ptr payload{new geonet::DownPacket()}; @@ -561,4 +576,73 @@ namespace v2x { ++cpm_num_; } } + + void CpmApplication::createTables() { + sqlite3 *db = NULL; + char* err = NULL; + + int ret = sqlite3_open("./src/autoware_v2x/db/autoware_v2x.db", &db); + if (ret != SQLITE_OK) { + RCLCPP_INFO(node_->get_logger(), "DB File Open Error"); + return; + } + + 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); + if (ret != SQLITE_OK) { + RCLCPP_INFO(node_->get_logger(), "DB Execution Error"); + sqlite3_close(db); + sqlite3_free(err); + return; + } + + sql_command = (char*) "create table if not exists cpm_received(id INTEGER PRIMARY KEY, timestamp BIGINT, perceivedObjectCount INTEGER);"; + + ret = sqlite3_exec(db, sql_command, NULL, NULL, &err); + if (ret != SQLITE_OK) { + RCLCPP_INFO(node_->get_logger(), "DB Execution Error"); + sqlite3_close(db); + sqlite3_free(err); + return; + } + + sqlite3_close(db); + RCLCPP_INFO(node_->get_logger(), "CpmApplication::createTables Finished"); + } + + void CpmApplication::insertCpmToCpmTable(vanetza::asn1::Cpm cpm, char* table_name) { + sqlite3 *db = NULL; + char* err = NULL; + + int ret = sqlite3_open("./src/autoware_v2x/db/autoware_v2x.db", &db); + if (ret != SQLITE_OK) { + RCLCPP_INFO(node_->get_logger(), "DB File Open Error"); + return; + } + + int64_t timestamp = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + int perceivedObjectCount = 0; + if (cpm->cpm.cpmParameters.numberOfPerceivedObjects) { + perceivedObjectCount = cpm->cpm.cpmParameters.numberOfPerceivedObjects; + } + + 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); + if (ret != SQLITE_OK) { + RCLCPP_INFO(node_->get_logger(), "DB Execution Error"); + sqlite3_close(db); + sqlite3_free(err); + return; + } + + sqlite3_close(db); + // RCLCPP_INFO(node_->get_logger(), "CpmApplication::insertCpmToCpmTable Finished"); + } } \ No newline at end of file