perf(processing_time): add cyclic and processing time ms to pcd relat…

…ed package (tier4#946) * feat(pcd): add debug processing time ms Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * feat: update pcd package Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * feat: add processing time to behavior package Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * fix: pre comimt and conflict Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * Revert "feat: add processing time to behavior package" This reverts commit fe6a8be. * chore: add cyclic time to processing time Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
boyali · Oct 19, 2022 · dda0d3e · dda0d3e
1 parent 6ff1622
commit dda0d3e
Show file tree

Hide file tree

Showing 2 changed files with 40 additions and 1 deletion.
diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp
@@ -60,6 +60,16 @@ namespace pointcloud_preprocessor
 CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & options)
 : Filter("CropBoxFilter", options)
 {
+  // initialize debug tool
+  {
+    using tier4_autoware_utils::DebugPublisher;
+    using tier4_autoware_utils::StopWatch;
+    stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
+    debug_publisher_ = std::make_unique<DebugPublisher>(this, "crop_box_filter");
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
+
   // set initial parameters
   {
     auto & p = param_;
@@ -91,7 +101,7 @@ void CropBoxFilterComponent::filter(
   PointCloud2 & output)
 {
   std::scoped_lock lock(mutex_);
-
+  stop_watch_ptr_->toc("processing_time", true);
   output.data.resize(input->data.size());
   Eigen::Vector3f pt(Eigen::Vector3f::Zero());
   size_t j = 0;
@@ -132,6 +142,15 @@ void CropBoxFilterComponent::filter(
   output.row_step = static_cast<uint32_t>(output.data.size() / output.height);
 
   publishCropBoxPolygon();
+  // add processing time for debug
+  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);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/cyclic_time_ms", cyclic_time_ms);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+  }
 }
 
 void CropBoxFilterComponent::publishCropBoxPolygon()

diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp
@@ -21,6 +21,16 @@ namespace pointcloud_preprocessor
 RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options)
 : Filter("RingOutlierFilter", options)
 {
+  // initialize debug tool
+  {
+    using tier4_autoware_utils::DebugPublisher;
+    using tier4_autoware_utils::StopWatch;
+    stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
+    debug_publisher_ = std::make_unique<DebugPublisher>(this, "ring_outlier_filter");
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
+
   // set initial parameters
   {
     distance_ratio_ = static_cast<double>(declare_parameter("distance_ratio", 1.03));
@@ -39,6 +49,7 @@ void RingOutlierFilterComponent::filter(
   PointCloud2 & output)
 {
   std::scoped_lock lock(mutex_);
+  stop_watch_ptr_->toc("processing_time", true);
   std::unordered_map<uint16_t, std::vector<std::size_t>> input_ring_map;
   input_ring_map.reserve(128);
   sensor_msgs::msg::PointCloud2::SharedPtr input_ptr =
@@ -109,6 +120,15 @@ void RingOutlierFilterComponent::filter(
     }
     tmp_indices.clear();
   }
+  // add processing time for debug
+  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);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/cyclic_time_ms", cyclic_time_ms);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+  }
 }
 
 rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallback(