feat(occupancy_grid_map_outlier_filter): add time_keeper

[a-maumau]

Description

This PR will add time keeper feature to occupancy_grid_map_outlier_filter.

Since it get launched from ground_segmentation.launch.py, the parameter is currently directly written in it.
Need to be merged with autowarefoundation/autoware_launch#1147 to launch in autoware.

---

Sample output from autoware_debug_tools (updated from original comment)

100.00% onOccupancyGridMapAndPointCloud2: total 3202.65 [ms], avg. 11.73 [ms], run count: 273
    ├── 1.29% initializerPointCloud2: total 41.38 [ms], avg. 0.02 [ms], run count: 2184
    ├── 2.44% splitPointCloudFrontBack: total 78.19 [ms], avg. 0.29 [ms], run count: 273
    ├── 0.01% finalizePointCloud2: total 0.16 [ms], avg. 0.00 [ms], run count: 546
    ├── 58.98% transformPointcloud: total 1888.93 [ms], avg. 3.46 [ms], run count: 546
    ├── 2.65% filterByOccupancyGridMap: total 84.95 [ms], avg. 0.31 [ms], run count: 273
    │   ├── 0.00% finalizePointCloud2: total 0.03 [ms], avg. 0.00 [ms], run count: 819
    │   └── 2.65% rest: 84.92 [ms]
    ├── 32.69% radius_search_2d_filter: total 1047.06 [ms], avg. 3.84 [ms], run count: 273
    ├── 1.31% point_concat: total 41.87 [ms], avg. 0.15 [ms], run count: 273
    │   ├── 1.26% concatPointCloud2: total 40.44 [ms], avg. 0.04 [ms], run count: 1092
    │   ├── 0.00% finalizePointCloud2: total 0.02 [ms], avg. 0.00 [ms], run count: 273
    │   └── 0.04% rest: 1.42 [ms]
    └── 0.63% rest: 20.12 [ms]

Related links

Parent Issue:

• Link

How was this PR tested?

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[technolojin]

100.00% onOccupancyGridMapAndPointCloud2: total 66.42 [ms], avg. 11.07 [ms], run count: 6
├── 2.71% splitPointCloudFrontBack: total 1.80 [ms], avg. 0.30 [ms], run count: 6
├── 2.35% filterByOccupancyGridMap: total 1.56 [ms], avg. 0.26 [ms], run count: 6
├── 2.10% concatPointCloud2: total 1.40 [ms], avg. 0.06 [ms], run count: 24
└── 92.83% rest: 61.66 [ms]

Most of the processing time is from the rest.
Can you add additional measurement to find which part is heavy?

For example

  if (radius_search_2d_filter_ptr_) {
    std::unique_ptr<ScopedTimeTrack> st_ptr;
    if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>("radius_search_2d_filter", *time_keeper_);

    auto pc_frame_pose_stamped = getPoseStamped(
      *tf2_, input_ogm->header.frame_id, input_pc->header.frame_id, input_ogm->header.stamp);
    radius_search_2d_filter_ptr_->filter(
      high_confidence_pc, low_confidence_pc, pc_frame_pose_stamped.pose, filtered_low_confidence_pc,
      outlier_pc);
  } else {
    std::unique_ptr<ScopedTimeTrack> st_ptr;
    if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>("low_confidence_pc", *time_keeper_);

    std::memcpy(&outlier_pc.data[0], &low_confidence_pc.data[0], low_confidence_pc.data.size());
    outlier_pc.data.resize(low_confidence_pc.data.size());
  }

[a-maumau]

I added more timekeepers to make the processing time clearer (with less rest in the visualization) in 444e349, as @technolojin suggested.

the output will be like:

# total time

100.00% onOccupancyGridMapAndPointCloud2: total 3202.65 [ms], avg. 11.73 [ms], run count: 273
    ├── 1.29% initializerPointCloud2: total 41.38 [ms], avg. 0.02 [ms], run count: 2184
    ├── 2.44% splitPointCloudFrontBack: total 78.19 [ms], avg. 0.29 [ms], run count: 273
    ├── 0.01% finalizePointCloud2: total 0.16 [ms], avg. 0.00 [ms], run count: 546
    ├── 58.98% transformPointcloud: total 1888.93 [ms], avg. 3.46 [ms], run count: 546
    ├── 2.65% filterByOccupancyGridMap: total 84.95 [ms], avg. 0.31 [ms], run count: 273
    │   ├── 0.00% finalizePointCloud2: total 0.03 [ms], avg. 0.00 [ms], run count: 819
    │   └── 2.65% rest: 84.92 [ms]
    ├── 32.69% radius_search_2d_filter: total 1047.06 [ms], avg. 3.84 [ms], run count: 273
    ├── 1.31% point_concat: total 41.87 [ms], avg. 0.15 [ms], run count: 273
    │   ├── 1.26% concatPointCloud2: total 40.44 [ms], avg. 0.04 [ms], run count: 1092
    │   ├── 0.00% finalizePointCloud2: total 0.02 [ms], avg. 0.00 [ms], run count: 273
    │   └── 0.04% rest: 1.42 [ms]
    └── 0.63% rest: 20.12 [ms]

The time consuming transformPointcloud spends the time mainly on pcl_ros::transformPointCloud.

(single call)

tf2.lookupTransform         :   8 microseconds
transformToEigen            :   0 microseconds
pcl_ros::transformPointCloud: 325 microseconds

total duration              : 342 microseconds

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 36 lines in your changes missing coverage. Please review.

Project coverage is 24.12%. Comparing base (0a364d2) to head (46b6078).
Report is 81 commits behind head on main.

Files with missing lines	Patch %	Lines
...ter/src/occupancy_grid_map_outlier_filter_node.cpp	0.00%	36 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #8597      +/-   ##
==========================================
- Coverage   24.12%   24.12%   -0.01%     
==========================================
  Files        1400     1400              
  Lines      102260   102300      +40     
  Branches    38752    38767      +15     
==========================================
  Hits        24675    24675              
- Misses      75053    75093      +40     
  Partials     2532     2532              

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	24.13% <ø> (+<0.01%)	⬆️	Carriedforward from 69ff4f3

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[a-maumau]

I found some error setting the scopes, and updated the code.

The main spent time on the process is publishing the point clouds.

100.00% onOccupancyGridMapAndPointCloud2: total 1120.78 [ms], avg. 10.10 [ms], run count: 111
    ├── 1.14% initializerPointCloud2: total 12.75 [ms], avg. 0.02 [ms], run count: 777
    ├── 1.87% splitPointCloudFrontBack: total 20.94 [ms], avg. 0.19 [ms], run count: 111
    ├── 0.01% finalizePointCloud2: total 0.06 [ms], avg. 0.00 [ms], run count: 333
    ├── 7.15% transformPointcloud: total 80.11 [ms], avg. 0.36 [ms], run count: 222
    ├── 2.36% filterByOccupancyGridMap: total 26.40 [ms], avg. 0.24 [ms], run count: 111
    │   ├── 0.00% finalizePointCloud2: total 0.00 [ms], avg. 0.00 [ms], run count: 333
    │   └── 2.36% rest: 26.40 [ms]
    ├── 26.96% radius_search_2d_filter: total 302.17 [ms], avg. 2.72 [ms], run count: 111
    ├── 1.29% concatPointCloud2: total 14.43 [ms], avg. 0.03 [ms], run count: 444
    ├── 58.54% publish_pointcloud: total 656.10 [ms], avg. 5.91 [ms], run count: 111
    └── 0.70% rest: 7.82 [ms]

[technolojin]

#6745 has been merged.
Can you implement the additional parameter?

[a-maumau]

@technolojin I added the parameter in
69ff4f3, and added for autoware_launch too: autowarefoundation/autoware_launch#1147

[technolojin]

LGTM (with minor comment)

[YoshiRi]

LGTM