TiagoRG
/
AutowareV2X
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
AutowareV2X/src/cpm_application.cpp

645 lines
27 KiB
C++
Raw Blame History

#include "autoware_v2x/cpm_application.hpp"
#include "autoware_v2x/positioning.hpp"
#include "autoware_v2x/security.hpp"
#include "autoware_v2x/link_layer.hpp"
#include "autoware_v2x/v2x_node.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "rclcpp/rclcpp.hpp"
#include <vanetza/btp/ports.hpp>
#include <vanetza/asn1/cpm.hpp>
#include <vanetza/asn1/packet_visitor.hpp>
#include <vanetza/facilities/cpm_functions.hpp>
#include <chrono>
#include <functional>
#include <iostream>
#include <sstream>
#include <exception>
#include <GeographicLib/UTMUPS.hpp>
#include <GeographicLib/MGRS.hpp>
#include <string>
#include <boost/units/cmath.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <sqlite3.h>
#define _USE_MATH_DEFINES
#include <math.h>
using namespace vanetza;
using namespace vanetza::facilities;
using namespace std::chrono;
namespace v2x {
CpmApplication::CpmApplication(V2XNode *node, Runtime &rt, bool is_sender) :
node_(node),
runtime_(rt),
ego_(),
generationDeltaTime_(0),
updating_objects_list_(false),
sending_(false),
is_sender_(is_sender),
reflect_packet_(false),
objectConfidenceThreshold_(0.0),
include_all_persons_and_animals_(false),
cpm_num_(0),
received_cpm_num_(0),
cpm_object_id_(0)
{
RCLCPP_INFO(node_->get_logger(), "CpmApplication started. is_sender: %d", is_sender_);
set_interval(milliseconds(100));
createTables();
}
void CpmApplication::set_interval(Clock::duration interval) {
cpm_interval_ = interval;
runtime_.cancel(this);
schedule_timer();
}
void CpmApplication::schedule_timer() {
runtime_.schedule(cpm_interval_, std::bind(&CpmApplication::on_timer, this, std::placeholders::_1), this);
}
void CpmApplication::on_timer(Clock::time_point) {
schedule_timer();
send();
}
CpmApplication::PortType CpmApplication::port() {
return btp::ports::CPM;
}
std::string CpmApplication::uuidToHexString(const unique_identifier_msgs::msg::UUID &id) {
std::stringstream ss;
for (auto i = 0; i < 16; ++i) {
ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i];
}
return ss.str();
}
void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet) {
asn1::PacketVisitor<asn1::Cpm> visitor;
std::shared_ptr<const asn1::Cpm> cpm = boost::apply_visitor(visitor, *packet);
if (cpm) {
RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received CPM #%d", received_cpm_num_);
rclcpp::Time current_time = node_->now();
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds());
asn1::Cpm message = *cpm;
ItsPduHeader_t &header = message->header;
// 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());
uint16_t gdt = time_now.count();
int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536;
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT_CPM: %ld", gdt_cpm);
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT: %u", gdt);
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_R1R2: %d", gdt_diff);
CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer;
double lat = management.referencePosition.latitude / 1.0e7;
double lon = management.referencePosition.longitude / 1.0e7;
int zone;
int grid_num_x = 4;
int grid_num_y = 39;
int grid_size = 100000;
bool northp;
double x, y;
GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, x, y);
double x_mgrs = x - grid_num_x * grid_size;
double y_mgrs = y - grid_num_y * grid_size;
OriginatingVehicleContainer_t &ovc = message->cpm.cpmParameters.stationDataContainer->choice.originatingVehicleContainer;
// Calculate ego-vehicle orientation (radians) from heading (degree).
// orientation: True-East, counter-clockwise angle. (0.1 degree accuracy)
int heading = ovc.heading.headingValue;
double orientation = (90.0 - (double) heading / 10.0) * M_PI / 180.0;
if (orientation < 0.0) orientation += (2.0 * M_PI);
// double orientation = heading / 10.0;
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] heading: %d", heading);
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] orientation: %f", orientation);
// Get PerceivedObjects
receivedObjectsStack.clear();
PerceivedObjectContainer_t *&poc = message->cpm.cpmParameters.perceivedObjectContainer;
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);
CpmApplication::Object object;
double x1 = poc->list.array[i]->xDistance.value;
double y1 = poc->list.array[i]->yDistance.value;
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.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;
object.yawAngle = poc->list.array[i]->yawAngle->value;
double yaw_radian = (M_PI * object.yawAngle / 10.0) / 180.0;
tf2::Quaternion quat;
quat.setRPY(0, 0, yaw_radian);
object.orientation_x = quat.x();
object.orientation_y = quat.y();
object.orientation_z = quat.z();
object.orientation_w = quat.w();
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] object.quat: %f, %f, %f, %f", object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w);
receivedObjectsStack.push_back(object);
}
node_->publishObjects(&receivedObjectsStack);
} else {
// RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC");
}
if (reflect_packet_) {
Application::DownPacketPtr packet{new DownPacket()};
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
payload->layer(OsiLayer::Application) = std::move(message);
Application::DataRequest request;
request.its_aid = aid::CP;
request.transport_type = geonet::TransportType::SHB;
request.communication_profile = geonet::CommunicationProfile::ITS_G5;
Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
if (!confirm.accepted()) {
throw std::runtime_error("[CpmApplication::indicate] Packet reflection failed");
}
}
++received_cpm_num_;
} else {
RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received broken content");
}
}
void CpmApplication::updateMGRS(double *x, double *y) {
ego_.mgrs_x = *x;
ego_.mgrs_y = *y;
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateMGRS] ego-vehicle.position: %.10f, %.10f", ego_.mgrs_x, ego_.mgrs_y);
}
void CpmApplication::updateRP(double *lat, double *lon, double *altitude) {
ego_.latitude = *lat;
ego_.longitude = *lon;
ego_.altitude = *altitude;
}
void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) {
generationDeltaTime_ = *gdt;
gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
}
void CpmApplication::updateHeading(double *yaw) {
ego_.heading = *yaw;
}
void CpmApplication::setAllObjectsOfPersonsAnimalsToSend(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
if (msg->objects.size() > 0) {
for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) {
std::string object_uuid = uuidToHexString(obj.object_id);
auto found_object = std::find_if(objectsList.begin(), objectsList.end(), [&](auto const &e) {
return !strcmp(e.uuid.c_str(), object_uuid.c_str());
});
if (found_object == objectsList.end()) {
} else {
found_object->to_send = true;
}
}
}
}
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();
}
if (msg->objects.size() > 0) {
for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) {
// RCLCPP_INFO(node_->get_logger(), "%d", obj.classification.front().label);
double existence_probability = obj.existence_probability;
// RCLCPP_INFO(node_->get_logger(), "existence_probability: %f", existence_probability);
std::string object_uuid = uuidToHexString(obj.object_id);
// RCLCPP_INFO(node_->get_logger(), "received object_id: %s", object_uuid.c_str());
// RCLCPP_INFO(node_->get_logger(), "ObjectsList count: %d", objectsList.size());
if (existence_probability >= objectConfidenceThreshold_) {
// ObjectConfidence > ObjectConfidenceThreshold
// Object tracked in internal memory? (i.e. Is object included in ObjectsList?)
auto found_object = std::find_if(objectsList.begin(), objectsList.end(), [&](auto const &e) {
return !strcmp(e.uuid.c_str(), object_uuid.c_str());
});
if (found_object == objectsList.end()) {
// Object is new to internal memory
if (cpm_object_id_ > 255) {
cpm_object_id_ = 0;
}
// Add new object to ObjectsList
CpmApplication::Object object;
object.objectID = cpm_object_id_;
object.uuid = object_uuid;
object.timestamp_ros = msg->header.stamp;
object.position_x = obj.kinematics.initial_pose_with_covariance.pose.position.x;
object.position_y = obj.kinematics.initial_pose_with_covariance.pose.position.y;
object.position_z = obj.kinematics.initial_pose_with_covariance.pose.position.z;
object.orientation_x = obj.kinematics.initial_pose_with_covariance.pose.orientation.x;
object.orientation_y = obj.kinematics.initial_pose_with_covariance.pose.orientation.y;
object.orientation_z = obj.kinematics.initial_pose_with_covariance.pose.orientation.z;
object.orientation_w = obj.kinematics.initial_pose_with_covariance.pose.orientation.w;
object.shape_x = std::lround(obj.shape.dimensions.x * 10.0);
object.shape_y = std::lround(obj.shape.dimensions.y * 10.0);
object.shape_z = std::lround(obj.shape.dimensions.z * 10.0);
long long msg_timestamp_sec = msg->header.stamp.sec;
long long msg_timestamp_nsec = msg->header.stamp.nanosec;
msg_timestamp_sec -= 1072915200; // convert to etsi-epoch
long long msg_timestamp_msec = msg_timestamp_sec * 1000 + msg_timestamp_nsec / 1000000;
object.timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec;
if (object.timeOfMeasurement < -1500 || object.timeOfMeasurement > 1500) {
RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", object.timeOfMeasurement);
continue;
}
object.to_send = false;
object.to_send_trigger = 0;
object.timestamp = runtime_.now();
objectsList.push_back(object);
++cpm_object_id_;
} else {
// Object was already in internal memory
// Object belongs to class person or animal
if (obj.classification.front().label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN || obj.classification.front().label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) {
if (include_all_persons_and_animals_) {
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
include_all_persons_and_animals_ = true;
found_object->to_send = true;
found_object->to_send_trigger = 5;
setAllObjectsOfPersonsAnimalsToSend(msg);
RCLCPP_INFO(node_->get_logger(), "Include all objects of person/animal class");
}
} else {
// Object does not belong to class person or animal
// Euclidean absolute distance has changed by more than 4m
double dist = pow(obj.kinematics.initial_pose_with_covariance.pose.position.x - found_object->position_x, 2) + pow(obj.kinematics.initial_pose_with_covariance.pose.position.y - found_object->position_y, 2);
dist = sqrt(dist);
// RCLCPP_INFO(node_->get_logger(), "Distance changed: %f", dist);
if (dist > 4) {
found_object->to_send = true;
found_object->to_send_trigger = 1;
} else {
}
// Absolute speed changed by more than 0.5 m/s
double speed = pow(obj.kinematics.initial_twist_with_covariance.twist.linear.x - found_object->twist_linear_x, 2) + pow(obj.kinematics.initial_twist_with_covariance.twist.linear.x- found_object->twist_linear_y, 2);
speed = sqrt(speed);
// RCLCPP_INFO(node_->get_logger(), "Speed changed: %f", dist);
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;
double twist_angular_y_diff = (obj.kinematics.initial_twist_with_covariance.twist.angular.y - found_object->twist_angular_y) * 180 / M_PI;
// RCLCPP_INFO(node_->get_logger(), "Orientation speed vector changed x: %f", twist_angular_x_diff);
// RCLCPP_INFO(node_->get_logger(), "Orientation speed vector changed y: %f", twist_angular_y_diff);
if( twist_angular_x_diff > 4 || twist_angular_y_diff > 4 ) {
found_object->to_send = true;
found_object->to_send_trigger = 3;
}
// It has been more than 1 s since last transmission of this object
if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 1000000) {
found_object->to_send = true;
found_object->to_send_trigger = 4;
// RCLCPP_INFO(node_->get_logger(), "Been more than 1s: %ld", runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count());
}
}
// Update found_object
found_object->timestamp_ros = msg->header.stamp;
found_object->position_x = obj.kinematics.initial_pose_with_covariance.pose.position.x;
found_object->position_y = obj.kinematics.initial_pose_with_covariance.pose.position.y;
found_object->position_z = obj.kinematics.initial_pose_with_covariance.pose.position.z;
found_object->orientation_x = obj.kinematics.initial_pose_with_covariance.pose.orientation.x;
found_object->orientation_y = obj.kinematics.initial_pose_with_covariance.pose.orientation.y;
found_object->orientation_z = obj.kinematics.initial_pose_with_covariance.pose.orientation.z;
found_object->orientation_w = obj.kinematics.initial_pose_with_covariance.pose.orientation.w;
found_object->shape_x = std::lround(obj.shape.dimensions.x * 10.0);
found_object->shape_y = std::lround(obj.shape.dimensions.y * 10.0);
found_object->shape_z = std::lround(obj.shape.dimensions.z * 10.0);
long long msg_timestamp_sec = msg->header.stamp.sec;
long long msg_timestamp_nsec = msg->header.stamp.nanosec;
msg_timestamp_sec -= 1072915200; // convert to etsi-epoch
long long msg_timestamp_msec = msg_timestamp_sec * 1000 + msg_timestamp_nsec / 1000000;
found_object->timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec;
if (found_object->timeOfMeasurement < -1500 || found_object->timeOfMeasurement > 1500) {
RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", found_object->timeOfMeasurement);
continue;
}
found_object->timestamp = runtime_.now();
}
}
}
} else {
// No objects detected
}
// RCLCPP_INFO(node_->get_logger(), "ObjectsStack: %d objects", objectsStack.size());
rclcpp::Time current_time = node_->now();
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] [measure] T_objstack_updated %ld", current_time.nanoseconds());
updating_objects_list_ = false;
}
void CpmApplication::printObjectsList(int cpm_num) {
// RCLCPP_INFO(node_->get_logger(), "------------------------");
if (objectsList.size() > 0) {
for (auto& object : objectsList) {
RCLCPP_INFO(node_->get_logger(), "[objectsList] %d,%d,%s,%d,%d", cpm_num, object.objectID, object.uuid.c_str(), object.to_send, object.to_send_trigger);
}
} else {
RCLCPP_INFO(node_->get_logger(), "[objectsList] %d,,,,", cpm_num);
}
// RCLCPP_INFO(node_->get_logger(), "------------------------");
}
void CpmApplication::send() {
if (is_sender_) {
sending_ = true;
printObjectsList(cpm_num_);
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM...");
vanetza::asn1::Cpm message;
// ITS PDU Header
ItsPduHeader_t &header = message->header;
header.protocolVersion = 1;
header.messageID = 14;
header.stationID = 1;
CollectivePerceptionMessage_t &cpm = message->cpm;
// Set GenerationDeltaTime
cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec;
CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
management.stationType = StationType_passengerCar;
management.referencePosition.latitude = ego_.latitude * 1e7;
management.referencePosition.longitude = ego_.longitude * 1e7;
StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer;
sdc = vanetza::asn1::allocate<StationDataContainer_t>();
sdc->present = StationDataContainer_PR_originatingVehicleContainer;
OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer;
ovc.speed.speedValue = 0;
ovc.speed.speedConfidence = 1;
// Calculate headingValue of ego-vehicle.
// Convert ego-vehicle yaw to True-North clockwise angle (heading). 0.1 degree accuracy.
int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0);
if (heading < 0) heading += 3600;
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>();
for (auto& object : objectsList) {
// 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
object.xDistance = std::lround((
(object.position_x - ego_.mgrs_x) * cos(-ego_.heading) - (object.position_y - ego_.mgrs_y) * sin(-ego_.heading)
) * 100.0);
object.yDistance = std::lround((
(object.position_x - ego_.mgrs_x) * sin(-ego_.heading) + (object.position_y - ego_.mgrs_y) * cos(-ego_.heading)
) * 100.0);
if (object.xDistance < -132768 || object.xDistance > 132767) {
RCLCPP_WARN(node_->get_logger(), "xDistance out of bounds. objectID: #%d", object.objectID);
continue;
}
if (object.yDistance < -132768 || object.yDistance > 132767) {
RCLCPP_WARN(node_->get_logger(), "yDistance out of bounds. objectID: #%d", object.objectID);
continue;
}
// Calculate orientation of detected object
tf2::Quaternion quat(object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w);
tf2::Matrix3x3 matrix(quat);
double roll, pitch, yaw;
matrix.getRPY(roll, pitch, yaw);
if (yaw < 0) {
object.yawAngle = std::lround(((yaw + 2*M_PI) * 180.0 / M_PI) * 10.0); // 0 - 3600
} else {
object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600
}
object.xSpeed = 0;
object.ySpeed = 0;
pObj->objectID = object.objectID;
pObj->timeOfMeasurement = object.timeOfMeasurement;
pObj->xDistance.value = object.xDistance;
pObj->xDistance.confidence = 1;
pObj->yDistance.value = object.yDistance;
pObj->yDistance.confidence = 1;
pObj->xSpeed.value = object.xSpeed;
pObj->xSpeed.confidence = 1;
pObj->ySpeed.value = object.ySpeed;
pObj->ySpeed.confidence = 1;
pObj->planarObjectDimension1 = vanetza::asn1::allocate<ObjectDimension_t>();
pObj->planarObjectDimension2 = vanetza::asn1::allocate<ObjectDimension_t>();
pObj->verticalObjectDimension = vanetza::asn1::allocate<ObjectDimension_t>();
(*(pObj->planarObjectDimension1)).value = object.shape_y;
(*(pObj->planarObjectDimension1)).confidence = 1;
(*(pObj->planarObjectDimension2)).value = object.shape_x;
(*(pObj->planarObjectDimension2)).confidence = 1;
(*(pObj->verticalObjectDimension)).value = object.shape_z;
(*(pObj->verticalObjectDimension)).confidence = 1;
pObj->yawAngle = vanetza::asn1::allocate<CartesianAngle>();
(*(pObj->yawAngle)).value = object.yawAngle;
(*(pObj->yawAngle)).confidence = 1;
ASN_SEQUENCE_ADD(poc, pObj);
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()};
payload->layer(OsiLayer::Application) = std::move(message);
Application::DataRequest request;
request.its_aid = aid::CP;
request.transport_type = geonet::TransportType::SHB;
request.communication_profile = geonet::CommunicationProfile::ITS_G5;
Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
if (!confirm.accepted()) {
throw std::runtime_error("[CpmApplication::send] CPM application data request failed");
}
sending_ = false;
rclcpp::Time current_time = node_->now();
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds());
++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 = duration_cast<milliseconds>(runtime_.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");
}
}
Reference in New Issue View Git Blame Copy Permalink