chore(pcd): add processing debug ms

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
tier4 · May 12, 2022 · 1f1ec79 · 1f1ec79
1 parent 700a826
commit 1f1ec79
Show file tree

Hide file tree

Showing 6 changed files with 108 additions and 0 deletions.
diff --git a/sensing/pointcloud_preprocessor/config/processing_time_ms.xml b/sensing/pointcloud_preprocessor/config/processing_time_ms.xml
@@ -0,0 +1,68 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab tab_name="processing_time_ms" containers="1">
+   <Container>
+    <DockSplitter sizes="1" count="1" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" mode="TimeSeries" flip_y="false" style="Lines">
+       <range right="1652341869.555872" top="50.000000" bottom="0.000000" left="1652341767.841453"/>
+       <limitY max="50" min="0"/>
+       <curve name="/sensing/lidar/right/ring_outlier_filter/debug/processing_time_ms/data" color="#1f77b4"/>
+       <curve name="/sensing/lidar/top/ring_outlier_filter/debug/processing_time_ms/data" color="#d62728"/>
+       <curve name="/sensing/lidar/rear/ring_outlier_filter/debug/processing_time_ms/data" color="#1ac938"/>
+       <curve name="/sensing/lidar/left/ring_outlier_filter/debug/processing_time_ms/data" color="#ff7f0e"/>
+       <curve name="/perception/obstacle_segmentation/crop_box_filter/debug/processing_time_ms/data" color="#f14cc1"/>
+       <curve name="/sensing/lidar/left/crop_box_filter/debug/processing_time_ms/data" color="#9467bd"/>
+       <curve name="/sensing/lidar/rear/crop_box_filter/debug/processing_time_ms/data" color="#17becf"/>
+       <curve name="/sensing/lidar/top/crop_box_filter/debug/processing_time_ms/data" color="#bcbd22"/>
+       <curve name="/sensing/lidar/right/crop_box_filter/debug/processing_time_ms/data" color="#1f77b4"/>
+      </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/..._preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp b/..._preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp
@@ -56,6 +56,8 @@
 #include "pointcloud_preprocessor/filter.hpp"
 
 #include <geometry_msgs/msg/polygon_stamped.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
 
 #include <pcl/filters/crop_box.h>
 
@@ -88,6 +90,9 @@ class CropBoxFilterComponent : public pointcloud_preprocessor::Filter
   /** \brief Parameter service callback result : needed to be hold */
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
 
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_;
+
   /** \brief Parameter service callback */
   rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
 

diff --git a/...eprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/...eprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
@@ -17,6 +17,8 @@
 
 #include "autoware_point_types/types.hpp"
 #include "pointcloud_preprocessor/filter.hpp"
+#include <tier4_autoware_utils/system/stop_watch.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
 
 #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
 
@@ -45,6 +47,9 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
   /** \brief Parameter service callback */
   rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
 
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_;
+
   bool isCluster(
     sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr,
     const std::vector<std::size_t> & tmp_indices)

diff --git a/...ocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/...ocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp
@@ -22,6 +22,8 @@
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tier4_autoware_utils/ros/processing_time_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <pcl/filters/voxel_grid.h>
 #include <pcl_conversions/pcl_conversions.h>
@@ -62,6 +64,10 @@ class Lanelet2MapFilterComponent : public rclcpp::Node
   float voxel_size_x_;
   float voxel_size_y_;
 
+  std::unique_ptr<tier4_autoware_utils::ProcessingTimePublisher> processing_time_publisher_ptr_;
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+
+
   void pointcloudCallback(const PointCloud2ConstPtr msg);
 
   void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);

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,12 @@ namespace pointcloud_preprocessor
 CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & options)
 : Filter("CropBoxFilter", options)
 {
+  // initialize debug tool
+  {
+    stop_watch_ptr_ = std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
+    debug_publisher_ = std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "crop_box_filter");
+  }
+
   // set initial parameters
   {
     auto & p = param_;
@@ -91,6 +97,7 @@ void CropBoxFilterComponent::filter(
   PointCloud2 & output)
 {
   boost::mutex::scoped_lock lock(mutex_);
+  stop_watch_ptr_->tic();
 
   output.data.resize(input->data.size());
   Eigen::Vector3f pt(Eigen::Vector3f::Zero());
@@ -132,6 +139,11 @@ void CropBoxFilterComponent::filter(
   output.row_step = static_cast<uint32_t>(output.data.size() / output.height);
 
   publishCropBoxPolygon();
+  // add processing time for debug 
+  {
+    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);
+  }
 }
 
 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,12 @@ namespace pointcloud_preprocessor
 RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options)
 : Filter("RingOutlierFilter", options)
 {
+  // initialize debug tool
+  {
+    stop_watch_ptr_ = std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
+    debug_publisher_ = std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "ring_outlier_filter");
+  }
+
   // set initial parameters
   {
     distance_ratio_ = static_cast<double>(declare_parameter("distance_ratio", 1.03));
@@ -39,6 +45,7 @@ void RingOutlierFilterComponent::filter(
   PointCloud2 & output)
 {
   boost::mutex::scoped_lock lock(mutex_);
+  stop_watch_ptr_->tic();
   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 =
@@ -49,7 +56,12 @@ void RingOutlierFilterComponent::filter(
   for (std::size_t idx = 0U; idx < input_ptr->data.size(); idx += input_ptr->point_step) {
     input_ring_map[*reinterpret_cast<uint16_t *>(&input_ptr->data[idx + ring_offset])].push_back(
       idx);
+  // add processing time for debug 
+  {
+    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);
   }
+}
 
   PointCloud2Modifier<PointXYZI> output_modifier{output, input->header.frame_id};
   output_modifier.reserve(input->width);