diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index d9dbd92..a94b004 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -70,12 +70,15 @@ namespace v2x return btp::ports::CPM; } - void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet) - { + void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet) { asn1::PacketVisitor visitor; std::shared_ptr cpm = boost::apply_visitor(visitor, *packet); if (cpm) { RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received decodable CPM content"); + + rclcpp::Time current_time = node_->now(); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r2 %ld", current_time.nanoseconds()); + asn1::Cpm message = *cpm; ItsPduHeader_t &header = message->header; @@ -172,8 +175,7 @@ namespace v2x void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) { generationDeltaTime_ = *gdt; - gdt_timestamp_ = *gdt_timestamp; - // RCLCPP_INFO(node_->get_logger(), "[updateGDT] %d %lu", generationDeltaTime_, gdt_timestamp_); + gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp } void CpmApplication::updateHeading(double *yaw) @@ -230,10 +232,15 @@ namespace v2x object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600 } - long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec; - object.timeOfMeasurement = (gdt_timestamp_ - timestamp) / 1e6; + 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; + // long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec; + object.timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec; // RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] %ld %ld %d", gdt_timestamp_, timestamp, object.timeOfMeasurement); if (object.timeOfMeasurement < -1500 || object.timeOfMeasurement > 1500) { + RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", object.timeOfMeasurement); continue; } objectsStack.push_back(object); @@ -248,8 +255,7 @@ namespace v2x updating_objects_stack_ = false; } - void CpmApplication::send() - { + void CpmApplication::send() { sending_ = true; RCLCPP_INFO(node_->get_logger(), "[SEND] Sending CPM..."); @@ -377,5 +383,7 @@ namespace v2x // } sending_ = false; // RCLCPP_INFO(node->get_logger(), "Application::request END"); + rclcpp::Time current_time = node_->now(); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds()); } } \ No newline at end of file diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 8491160..911b014 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -40,6 +40,9 @@ namespace v2x void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { // RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received"); + if (!tf_received_) { + RCLCPP_WARN(node_->get_logger(), "[V2XApp::objectsCallback] tf not received yet"); + } if (tf_received_ && cp_started_) { cp->updateObjectsStack(msg); } @@ -62,7 +65,7 @@ namespace v2x // long long gdt_timestamp_msec_etsi_epoch = gdt_timestamp_msec - 1072915200000; // int gdt = gdt_timestamp_msec_etsi_epoch % 65536; // milliseconds int gdt = timestamp_msec % 65536; - RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec); + // RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec); double rot_x = msg->transforms[0].transform.rotation.x; double rot_y = msg->transforms[0].transform.rotation.y; diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index e44aed2..31dbcd9 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -25,14 +25,13 @@ using namespace std::chrono; namespace v2x { - V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options), received_time(this->now()), cpm_received(false) - { + V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) { using std::placeholders::_1; subscription_ = this->create_subscription("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); subscription_pos_ = this->create_subscription("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); - publisher_ = create_publisher("/perception/object_recognition/objects", rclcpp::QoS{10}); + publisher_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); this->declare_parameter("network_interface", "vmnet1"); @@ -40,13 +39,16 @@ namespace v2x boost::thread v2xApp(boost::bind(&V2XApp::start, app)); 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()); + } void V2XNode::publishObjects(std::vector *objectsStack) { autoware_perception_msgs::msg::DynamicObjectArray output_dynamic_object_msg; std_msgs::msg::Header header; rclcpp::Time current_time = this->now(); - received_time = current_time; output_dynamic_object_msg.header.frame_id = "map"; output_dynamic_object_msg.header.stamp = current_time; @@ -79,27 +81,25 @@ namespace v2x output_dynamic_object_msg.objects.push_back(object); } - cpm_received = true; + + current_time = this->now(); + RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds()); + publisher_->publish(output_dynamic_object_msg); } - void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) - { + void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { rclcpp::Time current_time = this->now(); - // RCLCPP_INFO(get_logger(), "%ld", current_time.nanoseconds() - received_time.nanoseconds()); - if (cpm_received) { - // if time difference is more than 10ms - if (current_time - received_time > rclcpp::Duration(std::chrono::nanoseconds(10000000))) { - app->objectsCallback(msg); - } - cpm_received = false; - } else { - app->objectsCallback(msg); - } + 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()); + app->objectsCallback(msg); } - void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) - { + void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { app->tfCallback(msg); } }