diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index 35b2aad794e49..0957749e16c83 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -20,9 +20,11 @@ We trained the models using . ### Output -| Name | Type | Description | -| ------------------ | ----------------------------------------------------- | ---------------- | -| `~/output/objects` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | +| Name | Type | Description | +| -------------------------- | ----------------------------------------------------- | -------------------- | +| `~/output/objects` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | +| `debug/cyclic_time_ms` | `tier4_debug_msgs::msg::Float64Stamped` | cyclic time (msg) | +| `debug/processing_time_ms` | `tier4_debug_msgs::msg::Float64Stamped` | processing time (ms) | ## Parameters diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index 096a173421edc..66ffdfd400cfd 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #include #include @@ -51,6 +53,11 @@ class LidarCenterPointNode : public rclcpp::Node bool has_twist_{false}; std::unique_ptr detector_ptr_{nullptr}; + + // debugger + std::unique_ptr> stop_watch_ptr_{ + nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 2f39251547566..c733171d78c99 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -89,6 +89,16 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti std::bind(&LidarCenterPointNode::pointCloudCallback, this, std::placeholders::_1)); objects_pub_ = this->create_publisher( "~/output/objects", rclcpp::QoS{1}); + + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ptr_ = std::make_unique(this, "lidar_centerpoint"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } } void LidarCenterPointNode::pointCloudCallback( @@ -100,6 +110,10 @@ void LidarCenterPointNode::pointCloudCallback( return; } + if (stop_watch_ptr_) { + stop_watch_ptr_->toc("processing_time", true); + } + std::vector det_boxes3d; bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d); if (!is_success) { @@ -117,6 +131,16 @@ void LidarCenterPointNode::pointCloudCallback( if (objects_sub_count > 0) { objects_pub_->publish(output_msg); } + + // add processing time for debug + 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); + debug_publisher_ptr_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_ptr_->publish( + "debug/processing_time_ms", processing_time_ms); + } } } // namespace centerpoint