diff --git a/perception/autoware_image_projection_based_fusion/package.xml b/perception/autoware_image_projection_based_fusion/package.xml index 025ed5377fa72..fa6217f589627 100644 --- a/perception/autoware_image_projection_based_fusion/package.xml +++ b/perception/autoware_image_projection_based_fusion/package.xml @@ -17,6 +17,7 @@ autoware_cmake autoware_euclidean_cluster + autoware_internal_debug_msgs autoware_lidar_centerpoint autoware_object_recognition_utils autoware_perception_msgs diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index f7861d91b6712..53b18e20883bc 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -253,12 +254,12 @@ void FusionNode::exportProcess() std::chrono::nanoseconds( (this->get_clock()->now() - cached_det3d_msg_ptr_->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); processing_time_ms = 0; } @@ -353,9 +354,9 @@ void FusionNode::subCallback( // add timestamp interval for debug if (debug_internal_pub_) { double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; - debug_internal_pub_->publish( + debug_internal_pub_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - debug_internal_pub_->publish( + debug_internal_pub_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", timestamp_interval_ms - det2d.input_offset_ms); } @@ -418,9 +419,9 @@ void FusionNode::roiCallback( if (debug_internal_pub_) { double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; - debug_internal_pub_->publish( + debug_internal_pub_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - debug_internal_pub_->publish( + debug_internal_pub_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", timestamp_interval_ms - det2d.input_offset_ms); }