From 34e33165481af72d975a21ac52ae9ff798b1acb4 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 9 Feb 2024 16:48:59 +0900 Subject: [PATCH] feat(perception): add pipeline_latency_ms publisher (#6181) Signed-off-by: kminoda --- .../detected_object_filter/object_lanelet_filter.hpp | 3 +++ .../obstacle_pointcloud_based_validator.hpp | 2 ++ .../src/object_lanelet_filter.cpp | 12 ++++++++++++ .../src/obstacle_pointcloud_based_validator.cpp | 10 ++++++++++ .../euclidean_cluster/src/euclidean_cluster_node.cpp | 11 +++++++---- .../src/voxel_grid_based_euclidean_cluster_node.cpp | 11 +++++++---- .../src/fusion_node.cpp | 7 +++++++ perception/lidar_centerpoint/src/node.cpp | 7 +++++++ .../pointcloud_based_occupancy_grid_map_node.cpp | 7 +++++++ 9 files changed, 62 insertions(+), 8 deletions(-) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index 4331b5c19d3f1..e3c3263466033 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -27,6 +28,7 @@ #include #include +#include #include namespace object_lanelet_filter @@ -48,6 +50,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr object_pub_; rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr object_sub_; + std::unique_ptr debug_publisher_{nullptr}; lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::ConstLanelets road_lanelets_; diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 2d70247445043..eb6da6bc45b24 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -20,6 +20,7 @@ #include "detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp" #include +#include #include #include @@ -133,6 +134,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node rclcpp::Publisher::SharedPtr objects_pub_; message_filters::Subscriber objects_sub_; message_filters::Subscriber obstacle_pointcloud_sub_; + std::unique_ptr debug_publisher_{nullptr}; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index cd16d414425cb..74000a91e60fc 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -52,6 +52,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod "input/object", rclcpp::QoS{1}, std::bind(&ObjectLaneletFilterNode::objectCallback, this, _1)); object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); + + debug_publisher_ = + std::make_unique(this, "object_lanelet_filter"); } void ObjectLaneletFilterNode::mapCallback( @@ -116,6 +119,15 @@ void ObjectLaneletFilterNode::objectCallback( ++index; } object_pub_->publish(output_object_msg); + + // Publish debug info + const double pipeline_latency = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - output_object_msg.header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency); } geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint( diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index d903a9ca3bb41..f3f56652792e9 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -304,6 +304,8 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); + debug_publisher_ = std::make_unique( + this, "obstacle_pointcloud_based_validator"); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); @@ -350,6 +352,14 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header); debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header); } + + // Publish processing time info + const double pipeline_latency = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency); } } // namespace obstacle_pointcloud_based_validator diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index a890c19e25d06..746ef1bafb583 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -64,10 +64,7 @@ void EuclideanClusterNode::onPointCloud( cluster_pub_->publish(output); // build debug msg - if (debug_pub_->get_subscription_count() < 1) { - return; - } - { + if (debug_pub_->get_subscription_count() >= 1) { sensor_msgs::msg::PointCloud2 debug; convertObjectMsg2SensorMsg(output, debug); debug_pub_->publish(debug); @@ -75,10 +72,16 @@ void EuclideanClusterNode::onPointCloud( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) + .count(); debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index a28791e5d4057..f58d9ac6dcc48 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -76,10 +76,7 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( cluster_pub_->publish(output); // build debug msg - if (debug_pub_->get_subscription_count() < 1) { - return; - } - { + if (debug_pub_->get_subscription_count() >= 1) { sensor_msgs::msg::PointCloud2 debug; convertObjectMsg2SensorMsg(output, debug); debug_pub_->publish(debug); @@ -87,10 +84,16 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( if (debug_publisher_) { const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) + .count(); debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index f06ff96abe257..76dc50980a926 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -173,11 +173,18 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - cached_msg_.second->header.stamp).nanoseconds())) + .count(); debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); processing_time_ms = 0; } } diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 39683755bcaf0..d370a60a26aa5 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -169,10 +169,17 @@ void LidarCenterPointNode::pointCloudCallback( if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - output_msg.header.stamp).nanoseconds())) + .count(); debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); + debug_publisher_ptr_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 82da92da7f146..63b6715948f93 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -206,10 +206,17 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - input_raw_msg->header.stamp).nanoseconds())) + .count(); debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); + debug_publisher_ptr_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } }