fix(occupancy_grid_map_outlier_filter): add intensity field

[badai-nguyen]

Description

• To add intensity field for occupancy_grid_map_outlier_filter to solve the issue Add intensity for poincloud processing in perception and an intensity_based_unknown_validator #6785
• This PR will avoid fixed pointcloud type like pcl::PointXYZ or pcl::PointXYZI.
• Almost of all calculations are replaced and done directly in Pointcloud2 except kd_tree_->radiusSearch. So that the pointcloud type will be coppied through the node by reference point_step of input topic so that the pointcloud type which is processed and output depends and is the same as input pointcloud type.

Related links

• TIER IV INTERNAL LINK

• Releated PR: fix(ground_segmentation): add intensity field #6791

Tests performed

Confirmed by logging_simulator that the intensity is added into the current empty padding fiedl (offset 12)

• Testing on the sample rosbag shows the processing time isn't affected much, even seems get slightly faster.

Before	After

• Here is testing result when pcl::PointXYZ pointcloud type is input into the node. It shows that the node also work well with PointXYZ type input
  

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[yukkysaito]

Is it possible to support XYZ so that it works with XYZ and XYZI?
If it doesn't work without XYZI, it will be difficult to use it with other sensors.

[badai-nguyen]

@yukkysaito Sorry that line is just typo. I fixed here 0aed0ad

In summary, this PR will avoid fixed pointcloud type like pcl::PointXYZ or pcl::PointXYZI.
Almost of all calculations are replaced and done directly in Pointcloud2 except kd_tree_->radiusSearch. So that the pointcloud type will be coppied through the node by reference point_step of input topic. Then the pointcloud type which is processed and output depends and is the same as input pointcloud type.
Here is testing result of output when I input pcl::PointXYZ type into the node.
Left: input topic
Right output topic

[badai-nguyen]

@yukkysaito do you have other concerns related with this?

[YoshiRi]

Sorry for late review, I wrote 1 major comment and 1 minor comment.

[YoshiRi]

Though I know that normally height and point_step would not become 0, I prefer adding some error handling here.

  // you can write it with rclcpp error handling
  if (output.height == 0 || output.point_step == 0) {
    std::cerr << "Error: output.height or output.point_step cannot be zero." << std::endl;
    return;
  }

[YoshiRi]

LGTM

[badai-nguyen]

I aslo confirmed that the output of occupancy_grid_map_outlier_filter node are the same before and after change in case input PointXYZ topics at here