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 3, 2022 · a5e7a18 · a5e7a18
1 parent 1f9d9fe
commit a5e7a18
Show file tree

Hide file tree

Showing 16 changed files with 261 additions and 1 deletion.
diff --git a/perception/compare_map_segmentation/package.xml b/perception/compare_map_segmentation/package.xml
@@ -24,6 +24,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
+  <depend>tier4_autoware_utils</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

diff --git a/...ption/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/...ption/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp
@@ -28,6 +28,17 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF
   const rclcpp::NodeOptions & options)
 : Filter("VoxelBasedApproximateCompareMapFilter", 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, "voxel_based_approximate_compare_map_filter");
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
+
   distance_threshold_ = static_cast<double>(declare_parameter("distance_threshold", 0.3));
 
   set_map_in_voxel_grid_ = false;
@@ -70,6 +81,7 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter(
 void VoxelBasedApproximateCompareMapFilterComponent::input_target_callback(
   const PointCloud2ConstPtr map)
 {
+  stop_watch_ptr_->toc("processing_time", true);
   pcl::PointCloud<pcl::PointXYZ> map_pcl;
   pcl::fromROSMsg<pcl::PointXYZ>(*map, map_pcl);
   const auto map_pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);
@@ -82,6 +94,15 @@ void VoxelBasedApproximateCompareMapFilterComponent::input_target_callback(
   voxel_grid_.setInputCloud(map_pcl_ptr);
   voxel_grid_.setSaveLeafLayout(true);
   voxel_grid_.filter(*voxel_map_ptr_);
+  // 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

diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp
@@ -28,6 +28,16 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent(
   const rclcpp::NodeOptions & options)
 : Filter("VoxelBasedCompareMapFilter", 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, "voxel_based_compare_map_filter");
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
+
   distance_threshold_ = static_cast<double>(declare_parameter("distance_threshold", 0.3));
 
   set_map_in_voxel_grid_ = false;
@@ -237,6 +247,7 @@ bool VoxelBasedCompareMapFilterComponent::is_in_voxel(
 
 void VoxelBasedCompareMapFilterComponent::input_target_callback(const PointCloud2ConstPtr map)
 {
+  stop_watch_ptr_->toc("processing_time", true);
   pcl::PointCloud<pcl::PointXYZ> map_pcl;
   pcl::fromROSMsg<pcl::PointXYZ>(*map, map_pcl);
   const auto map_pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);
@@ -249,6 +260,16 @@ void VoxelBasedCompareMapFilterComponent::input_target_callback(const PointCloud
   voxel_grid_.setInputCloud(map_pcl_ptr);
   voxel_grid_.setSaveLeafLayout(true);
   voxel_grid_.filter(*voxel_map_ptr_);
+
+  // 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 VoxelBasedCompareMapFilterComponent::paramCallback(

diff --git a/...on/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp b/...on/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp
@@ -15,6 +15,8 @@
 #include "lidar_apollo_instance_segmentation/debugger.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
@@ -40,6 +42,8 @@ class LidarInstanceSegmentationNode : public rclcpp::Node
   std::shared_ptr<LidarInstanceSegmentationInterface> detector_ptr_;
   std::shared_ptr<Debugger> debugger_ptr_;
   void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_;
 
 public:
   explicit LidarInstanceSegmentationNode(const rclcpp::NodeOptions & node_options);

diff --git a/perception/lidar_apollo_instance_segmentation/package.xml b/perception/lidar_apollo_instance_segmentation/package.xml
@@ -23,6 +23,7 @@
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
   <depend>tier4_perception_msgs</depend>
 
   <test_depend>ament_cmake_cppcheck</test_depend>

diff --git a/perception/lidar_apollo_instance_segmentation/src/node.cpp b/perception/lidar_apollo_instance_segmentation/src/node.cpp
@@ -16,11 +16,23 @@
 
 #include "lidar_apollo_instance_segmentation/detector.hpp"
 
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
+
 LidarInstanceSegmentationNode::LidarInstanceSegmentationNode(
   const rclcpp::NodeOptions & node_options)
 : Node("lidar_apollo_instance_segmentation_node", node_options)
 {
   using std::placeholders::_1;
+  // 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, "lidar_apollo_instance_segmentation");
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
   detector_ptr_ = std::make_shared<LidarApolloInstanceSegmentation>(this);
   debugger_ptr_ = std::make_shared<Debugger>(this);
   pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
@@ -34,10 +46,21 @@ LidarInstanceSegmentationNode::LidarInstanceSegmentationNode(
 void LidarInstanceSegmentationNode::pointCloudCallback(
   const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
 {
+  stop_watch_ptr_->toc("processing_time", true);
   tier4_perception_msgs::msg::DetectedObjectsWithFeature output_msg;
   detector_ptr_->detectDynamicObjects(*msg, output_msg);
   dynamic_objects_pub_->publish(output_msg);
   debugger_ptr_->publishColoredPointCloud(output_msg);
+
+  // 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);
+  }
 }
 
 #include <rclcpp_components/register_node_macro.hpp>

diff --git a/...r/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/...r/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp
@@ -115,6 +115,8 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node
 
   // Debugger
   std::shared_ptr<Debugger> debugger_ptr_;
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_;
 
   // ROS Parameters
   std::string map_frame_;

diff --git a/...ption/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/...ption/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp
@@ -181,6 +181,16 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(
   const rclcpp::NodeOptions & options)
 : Node("OccupancyGridMapOutlierFilter", 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, "occupancy_grid_map_outlier_filter");
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
+
   /* params */
   map_frame_ = declare_parameter("map_frame", "map");
   base_link_frame_ = declare_parameter("base_link_frame", "base_link");
@@ -215,6 +225,7 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(
 void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
   const OccupancyGrid::ConstSharedPtr & input_ogm, const PointCloud2::ConstSharedPtr & input_pc)
 {
+  stop_watch_ptr_->toc("processing_time", true);
   // Transform to occupancy grid map frame
   PointCloud2 ogm_frame_pc{};
   if (!transformPointcloud(*input_pc, *tf2_, input_ogm->header.frame_id, ogm_frame_pc)) {
@@ -256,6 +267,16 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
     debugger_ptr_->publishLowConfidence(filtered_low_confidence_pc, ogm_frame_pc.header);
     debugger_ptr_->publishOutlier(outlier_pc, ogm_frame_pc.header);
   }
+
+  // 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 OccupancyGridMapOutlierFilterComponent::filterByOccupancyGridMap(

diff --git a/sensing/pointcloud_preprocessor/config/processing_time_ms.xml b/sensing/pointcloud_preprocessor/config/processing_time_ms.xml
@@ -0,0 +1,65 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab containers="1" tab_name="tab1">
+   <Container>
+    <DockSplitter sizes="1" orientation="-" count="1">
+     <DockArea name="...">
+      <plot flip_y="false" flip_x="false" mode="TimeSeries" style="Lines">
+       <range top="250.000000" bottom="0.000000" left="1652436705.743731" right="1652436764.817605"/>
+       <limitY max="250" min="0"/>
+       <curve color="#1f77b4" name="/perception/object_recognition/detection/voxel_based_compare_map_filter/debug/processing_time_ms/data"/>
+       <curve color="#d62728" name="/perception/object_recognition/detection/apollo/lidar_apollo_instance_segmentation/debug/processing_time_ms/data"/>
+       <curve color="#1ac938" name="/sensing/lidar/left/crop_box_filter/debug/processing_time_ms/data"/>
+       <curve color="#ff7f0e" name="/sensing/lidar/top/crop_box_filter/debug/processing_time_ms/data"/>
+       <curve color="#f14cc1" name="/sensing/lidar/top/ring_outlier_filter/debug/processing_time_ms/data"/>
+       <curve color="#9467bd" name="/sensing/lidar/left/ring_outlier_filter/debug/processing_time_ms/data"/>
+       <curve color="#17becf" name="/perception/object_recognition/detection/voxel_based_approximate_compare_map_filter/debug/processing_time_ms/data"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="0"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <default time_axis="" delimiter="0"/>
+  </plugin>
+  <plugin ID="DataLoad ROS2 bags">
+   <use_header_stamp value="false"/>
+   <discard_large_arrays value="true"/>
+   <max_array_size value="100"/>
+   <boolean_strings_to_number value="true"/>
+   <remove_suffix_from_strings value="true"/>
+  </plugin>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="ROS2 Topic Subscriber">
+   <use_header_stamp value="false"/>
+   <discard_large_arrays value="false"/>
+   <max_array_size value="10"/>
+   <boolean_strings_to_number value="false"/>
+   <remove_suffix_from_strings value="false"/>
+  </plugin>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="Fast Fourier Transform"/>
+  <plugin ID="Quaternion to RPY"/>
+  <plugin ID="Reactive Script Editor">
+   <library code="--[[ Helper function to create a ScatterXY series from arrays.&#xa;&#xa; series_name: name of the created ScatterXY series&#xa; prefix:      prefix of the timeseries, before the index of the array&#xa; suffix_X:   suffix to complete the name of the series containing the X value.&#xa;             if [nil], the index of the array will be used.&#xa; suffix_Y:   suffix to complete the name of the series containing the Y value&#xa; timestamp:   usually the tracker_time variable&#xa;              &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa;   /trajectory/node.{N}/position/x&#xa;   /trajectory/node.{N}/position/y&#xa;   &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa;   CreateSeriesFromArray( &quot;my_trajectory&quot;, &quot;/trajectory/node&quot;,  &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;]]--&#xa;&#xa;function CreateSeriesFromArray( series_name, prefix, suffix_X, suffix_Y, timestamp )&#xa;  --- create a new series or overwite the previous one&#xa;  new_series = MutableScatterXY.new(series_name)&#xa;  &#xa;  --- Append points to new_series&#xa;  index = 0&#xa;  while(true) do&#xa;&#xa;    x = index;&#xa;    -- if not nil, get the X coordinate from a series&#xa;    if suffix_X ~= nil then &#xa;      series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa;      if series_x == nil then break end&#xa;      x = series_x:atTime(timestamp)&#x9; &#xa;    end&#xa;    &#xa;    series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa;    if series_x == nil then break end &#xa;    y = series_y:atTime(timestamp)&#xa;    &#xa;    new_series:push_back(x,y)&#xa;    index = index+1&#xa;  end&#xa;end&#xa;"/>
+   <scripts/>
+  </plugin>
+  <plugin ID="CSV Exporter"/>
+  <plugin ID="ROS2 Topic Re-Publisher"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <previouslyLoaded_Datafiles/>
+ <previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations/>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
diff --git a/...reprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp b/...reprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp
@@ -65,6 +65,8 @@
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
@@ -165,6 +167,10 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   void timer_callback();
 
   void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+  /** \brief processing time publisher. **/
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_;
 };
 
 }  // namespace pointcloud_preprocessor

diff --git a/...reprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/...reprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -33,7 +33,12 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+// Include tier4 autoware utils
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
+
 #include <deque>
+#include <memory>
 #include <string>
 
 namespace pointcloud_preprocessor
@@ -62,6 +67,9 @@ class DistortionCorrectorComponent : public rclcpp::Node
   rclcpp::Subscription<VelocityReport>::SharedPtr velocity_report_sub_;
   rclcpp::Publisher<PointCloud2>::SharedPtr undistorted_points_pub_;
 
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_;
+
   tf2_ros::Buffer tf2_buffer_{get_clock()};
   tf2_ros::TransformListener tf2_listener_{tf2_buffer_};
 

diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp
@@ -77,6 +77,10 @@
 #include <tf2_ros/create_timer_ros.h>
 #include <tf2_ros/transform_listener.h>
 
+// Include tier4 autoware utils
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
+
 namespace pointcloud_preprocessor
 {
 namespace sync_policies = message_filters::sync_policies;
@@ -169,6 +173,10 @@ class Filter : public rclcpp::Node
   /** \brief Internal mutex. */
   std::mutex mutex_;
 
+  /** \brief processing time publisher. **/
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_;
+
   /** \brief Virtual abstract filter method. To be implemented by every child.
    * \param input the input point cloud dataset.
    * \param indices a pointer to the vector of point indices to use.

diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp
@@ -69,6 +69,16 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
   const rclcpp::NodeOptions & node_options)
 : Node("point_cloud_concatenator_component", node_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, "concatenate_data_synchronizer");
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
+
   // Set parameters
   {
     output_frame_ = static_cast<std::string>(declare_parameter("output_frame", ""));
@@ -260,6 +270,7 @@ void PointCloudConcatenateDataSynchronizerComponent::combineClouds(
 
 void PointCloudConcatenateDataSynchronizerComponent::publish()
 {
+  stop_watch_ptr_->toc("processing_time", true);
   sensor_msgs::msg::PointCloud2::SharedPtr concat_cloud_ptr_ = nullptr;
   not_subscribed_topic_names_.clear();
 
@@ -293,6 +304,15 @@ void PointCloudConcatenateDataSynchronizerComponent::publish()
   std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) {
     e.second = nullptr;
   });
+  // 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);
+  }
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////