chore(pcd): add processing debug ms

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>

perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp

@@ -28,6 +28,16 @@ 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();
+  }
+
   distance_threshold_ = static_cast<double>(declare_parameter("distance_threshold", 0.3));
 
   set_map_in_voxel_grid_ = false;
@@ -50,6 +60,12 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter(
     output = *input;
     return;
   }
+  // add processing time for debug
+  if (stop_watch_ptr_) {
+    const double processing_time_ms = stop_watch_ptr_->toc(true);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+  }
   pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input(new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
   pcl::fromROSMsg(*input, *pcl_input);

perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp

@@ -28,6 +28,15 @@ 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();
+  }
+
   distance_threshold_ = static_cast<double>(declare_parameter("distance_threshold", 0.3));
 
   set_map_in_voxel_grid_ = false;
@@ -50,6 +59,12 @@ void VoxelBasedCompareMapFilterComponent::filter(
     output = *input;
     return;
   }
+  // add processing time for debug
+  if (stop_watch_ptr_) {
+    const double processing_time_ms = stop_watch_ptr_->toc(true);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+  }
   pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input(new pcl::PointCloud<pcl::PointXYZ>);
   pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
   pcl::fromROSMsg(*input, *pcl_input);

perception/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);

perception/lidar_apollo_instance_segmentation/package.xml

@@ -20,6 +20,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>

perception/lidar_apollo_instance_segmentation/src/node.cpp

@@ -16,11 +16,22 @@
 
 #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();
+  }
   detector_ptr_ = std::make_shared<LidarApolloInstanceSegmentation>(this);
   debugger_ptr_ = std::make_shared<Debugger>(this);
   pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
@@ -34,6 +45,12 @@ LidarInstanceSegmentationNode::LidarInstanceSegmentationNode(
 void LidarInstanceSegmentationNode::pointCloudCallback(
   const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
 {
+  // add processing time for debug
+  if (stop_watch_ptr_) {
+    const double processing_time_ms = stop_watch_ptr_->toc(true);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+  }
   tier4_perception_msgs::msg::DetectedObjectsWithFeature output_msg;
   detector_ptr_->detectDynamicObjects(*msg, output_msg);
   dynamic_objects_pub_->publish(output_msg);

perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp

@@ -114,6 +114,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_;

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

@@ -176,6 +176,15 @@ 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();
+  }
+
   /* params */
   map_frame_ = declare_parameter("map_frame", "map");
   base_link_frame_ = declare_parameter("base_link_frame", "base_link");
@@ -215,6 +224,12 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
   if (!transformPointcloud(*input_pc, *tf2_, input_ogm->header.frame_id, ogm_frame_pc)) {
     return;
   }
+  // add processing time for debug
+  if (stop_watch_ptr_) {
+    const double processing_time_ms = stop_watch_ptr_->toc(true);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+  }
   // Occupancy grid map based filter
   PclPointCloud high_confidence_pc{};
   PclPointCloud low_confidence_pc{};

sensing/pointcloud_preprocessor/config/processing_time_ms.xml

@@ -0,0 +1,66 @@
+<?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>
+

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp

@@ -27,6 +27,10 @@
 #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 <string>
 
@@ -56,6 +60,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_};
 

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;
@@ -168,6 +172,10 @@ class Filter : public rclcpp::Node
   /** \brief Internal mutex. */
   boost::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.

sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp

@@ -60,6 +60,15 @@ 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();
+  }
+
   // set initial parameters
   {
     auto & p = param_;
@@ -91,6 +100,12 @@ void CropBoxFilterComponent::filter(
   PointCloud2 & output)
 {
   boost::mutex::scoped_lock lock(mutex_);
+  // add processing time for debug
+  if (stop_watch_ptr_) {
+    const double processing_time_ms = stop_watch_ptr_->toc(true);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+  }
 
   output.data.resize(input->data.size());
   Eigen::Vector3f pt(Eigen::Vector3f::Zero());

sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -24,6 +24,15 @@ namespace pointcloud_preprocessor
 DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOptions & options)
 : Node("distortion_corrector_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, "distortion_corrector");
+    stop_watch_ptr_->tic();
+  }
+
   // Parameter
   time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp");
 
@@ -69,6 +78,13 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms
     return;
   }
 
+  // add processing time for debug
+  if (stop_watch_ptr_) {
+    const double processing_time_ms = stop_watch_ptr_->toc(true);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+  }
+
   tf2::Transform tf2_base_link_to_sensor{};
   getTransform(points_msg->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor);
 

sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp

@@ -21,6 +21,15 @@ 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();
+  }
+
   // set initial parameters
   {
     distance_ratio_ = static_cast<double>(declare_parameter("distance_ratio", 1.03));
@@ -39,6 +48,12 @@ void RingOutlierFilterComponent::filter(
   PointCloud2 & output)
 {
   boost::mutex::scoped_lock lock(mutex_);
+  // add processing time for debug
+  if (stop_watch_ptr_) {
+    const double processing_time_ms = stop_watch_ptr_->toc(true);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+  }
   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 =