diff --git a/launch/v2x.launch.xml b/launch/v2x.launch.xml index d15d223..9e9fc01 100644 --- a/launch/v2x.launch.xml +++ b/launch/v2x.launch.xml @@ -1,6 +1,9 @@ + + + diff --git a/src/cam_application.cpp b/src/cam_application.cpp index b2ca927..875493e 100644 --- a/src/cam_application.cpp +++ b/src/cam_application.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -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::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 payload{new geonet::DownPacket()}; payload->layer(OsiLayer::Application) = std::move(message); diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index e426994..83970c7 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -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 (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 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"); } -} \ No newline at end of file +} diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index abb889c..ee5c5ab 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -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("/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("/v2x/cam_ts/received", rclcpp::QoS{10}); // Declare Parameters - this->declare_parameter("link_layer", "cube-evk"); + this->declare_parameter("link_layer", "ethernet"); this->declare_parameter("network_interface", "v2x_testing"); this->declare_parameter("cube_ip", "127.0.0.1"); this->declare_parameter("is_sender", true);