From 4214d00e524eb38d96f0ee34d39437a196449efc Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 26 Jan 2024 10:41:20 +0900 Subject: [PATCH 1/6] feat(perception): add pipeline_latency publishers Signed-off-by: kminoda --- .../detected_object_filter/object_lanelet_filter.hpp | 2 ++ .../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 | 6 ++++++ .../src/voxel_grid_based_euclidean_cluster_node.cpp | 6 ++++++ .../src/fusion_node.cpp | 7 +++++++ .../pointcloud_based_occupancy_grid_map_node.cpp | 7 +++++++ 8 files changed, 52 insertions(+) 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..ff8881ef94f80 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 @@ -48,6 +49,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..9ebb02a35fc74 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -75,10 +75,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..fb944af95d0aa 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 @@ -87,10 +87,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 6bced39bc61ef..d5cf84bb62973 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -175,11 +175,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/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..e466bb0cf1698 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", processing_time_ms); } } From a52ce29fe783f458fa9cb152fd5003647f1eb138 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 29 Jan 2024 10:48:10 +0900 Subject: [PATCH 2/6] fix bug Signed-off-by: kminoda --- .../pointcloud_based_occupancy_grid_map_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e466bb0cf1698..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 @@ -216,7 +216,7 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); debug_publisher_ptr_->publish( - "debug/pipeline_latency_ms", processing_time_ms); + "debug/pipeline_latency_ms", pipeline_latency_ms); } } From 92277267a4a0702ce881105fae35e40ebc50bbdb Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 29 Jan 2024 11:55:52 +0900 Subject: [PATCH 3/6] feat: add pipeline latency publisher in centerpoint Signed-off-by: kminoda --- perception/lidar_centerpoint/src/node.cpp | 7 +++++++ 1 file changed, 7 insertions(+) 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); } } From 4d1fc10ae3f542e6133dfdfc4a8bd070cc733025 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 29 Jan 2024 12:06:36 +0900 Subject: [PATCH 4/6] fix: fix bug in euclidean_cluster Signed-off-by: kminoda --- .../src/voxel_grid_based_euclidean_cluster_node.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) 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 fb944af95d0aa..7f12658dbe49e 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); From 0306187e9296afc4097acc0da0974b60110628fc Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 29 Jan 2024 13:14:08 +0900 Subject: [PATCH 5/6] fix(object_lanelet_filter): add include Signed-off-by: kminoda --- .../detected_object_filter/object_lanelet_filter.hpp | 1 + 1 file changed, 1 insertion(+) 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 ff8881ef94f80..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 @@ -28,6 +28,7 @@ #include #include +#include #include namespace object_lanelet_filter From be2e218a2dd51985eaecba0afccae3e589ac7400 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 29 Jan 2024 14:15:29 +0900 Subject: [PATCH 6/6] fix(euclidean_cluster): fix bug Signed-off-by: kminoda --- perception/euclidean_cluster/src/euclidean_cluster_node.cpp | 5 +---- .../src/voxel_grid_based_euclidean_cluster_node.cpp | 2 +- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index 9ebb02a35fc74..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); 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 7f12658dbe49e..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,7 +76,7 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( cluster_pub_->publish(output); // build debug msg - if (debug_pub_->get_subscription_count() <= 1) { + if (debug_pub_->get_subscription_count() >= 1) { sensor_msgs::msg::PointCloud2 debug; convertObjectMsg2SensorMsg(output, debug); debug_pub_->publish(debug);