Merge pull request #7 from tlab-wide/feature/save-cpm-to-sqlite3

Feature/save cpm to sqlite3

.gitignore

@@ -0,0 +1 @@
+db/*.db

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"

include/autoware_v2x/cpm_application.hpp

@@ -7,6 +7,7 @@
 #include <boost/asio/steady_timer.hpp>
 #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_v2x/positioning.hpp"
+#include <vanetza/asn1/cpm.hpp>
 
 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

src/cpm_application.cpp

@@ -24,6 +24,8 @@
 #include <boost/units/cmath.hpp>
 #include <boost/units/systems/si/prefixes.hpp>
 
+#include <sqlite3.h>
+
 #define _USE_MATH_DEFINES
 #include <math.h>
 
@@ -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<geonet::DownPacket> 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<StationDataContainer_t>();
@@ -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<PerceivedObjectContainer_t>();
@@ -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<PerceivedObject>();
 
             // 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<geonet::DownPacket> 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::milliseconds>(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");
+  }
 }