fix: using empty padding field

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

Author: badai-nguyen

perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp

@@ -56,8 +56,8 @@ class RadiusSearch2dFilter
     const PclPointCloud & input, const Pose & pose, PclPointCloud & output,
     PclPointCloud & outlier);
   void filter(
-    const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose,
-    PclPointCloud & output, PclPointCloud & outlier);
+    const PointCloud2 & high_conf_input, const PointCloud2 & low_conf_input, const Pose & pose,
+    PointCloud2 & output, PointCloud2 & outlier);
 
 private:
   float search_radius_;
@@ -79,22 +79,25 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node
     const PointCloud2::ConstSharedPtr & input_pointcloud);
   void filterByOccupancyGridMap(
     const OccupancyGrid & occupancy_grid_map, const PointCloud2 & pointcloud,
-    PclPointCloud & high_confidence, PclPointCloud & low_confidence, PclPointCloud & out_ogm);
+    PointCloud2 & high_confidence, PointCloud2 & low_confidence, PointCloud2 & out_ogm);
   void splitPointCloudFrontBack(
     const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc);
+  void initializerPointCloud2(const PointCloud2 & input, PointCloud2 & output);
+  void finalizePointCloud2(const PointCloud2 & input, PointCloud2 & output);
+  void concatPointCloud2(PointCloud2 & output, const PointCloud2 & input);
 
 private:
   class Debugger
   {
   public:
     explicit Debugger(OccupancyGridMapOutlierFilterComponent & node);
-    void publishOutlier(const PclPointCloud & input, const Header & header);
-    void publishHighConfidence(const PclPointCloud & input, const Header & header);
-    void publishLowConfidence(const PclPointCloud & input, const Header & header);
+    void publishOutlier(const PointCloud2 & input, const Header & header);
+    void publishHighConfidence(const PointCloud2 & input, const Header & header);
+    void publishLowConfidence(const PointCloud2 & input, const Header & header);
 
   private:
     void transformToBaseLink(
-      const PclPointCloud & input, const Header & header, PointCloud2 & output);
+      const PointCloud2 & input, const Header & header, PointCloud2 & output);
     rclcpp::Publisher<PointCloud2>::SharedPtr outlier_pointcloud_pub_;
     rclcpp::Publisher<PointCloud2>::SharedPtr low_confidence_pointcloud_pub_;
     rclcpp::Publisher<PointCloud2>::SharedPtr high_confidence_pointcloud_pub_;

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

@@ -147,34 +147,45 @@ void RadiusSearch2dFilter::filter(
 }
 
 void RadiusSearch2dFilter::filter(
-  const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose,
-  PclPointCloud & output, PclPointCloud & outlier)
+  const PointCloud2 & high_conf_xyz_cloud, const PointCloud2 & low_conf_xyz_cloud,
+  const Pose & pose, PointCloud2 & output, PointCloud2 & outlier)
 {
-  const auto & high_conf_xyz_cloud = high_conf_input;
-  const auto & low_conf_xyz_cloud = low_conf_input;
   // check the limit points number
-  if (low_conf_xyz_cloud.points.size() > max_filter_points_nb_) {
+  if (low_conf_xyz_cloud.width > max_filter_points_nb_) {
     RCLCPP_WARN(
       rclcpp::get_logger("OccupancyGridMapOutlierFilterComponent"),
       "Skip outlier filter since too much low_confidence pointcloud!");
     return;
   }
-
+  int x_offset = low_conf_xyz_cloud.fields[pcl::getFieldIndex(low_conf_xyz_cloud, "x")].offset;
+  int y_offset = low_conf_xyz_cloud.fields[pcl::getFieldIndex(low_conf_xyz_cloud, "y")].offset;
+  int point_step = low_conf_xyz_cloud.point_step;
   pcl::PointCloud<pcl::PointXY>::Ptr xy_cloud(new pcl::PointCloud<pcl::PointXY>);
-  xy_cloud->points.resize(low_conf_xyz_cloud.points.size() + high_conf_xyz_cloud.points.size());
-  for (size_t i = 0; i < low_conf_xyz_cloud.points.size(); ++i) {
-    xy_cloud->points[i].x = low_conf_xyz_cloud.points[i].x;
-    xy_cloud->points[i].y = low_conf_xyz_cloud.points[i].y;
+  xy_cloud->points.resize(low_conf_xyz_cloud.width + high_conf_xyz_cloud.width);
+  for (size_t i = 0; i < low_conf_xyz_cloud.width; ++i) {
+    std::memcpy(
+      &xy_cloud->points[i].x, &low_conf_xyz_cloud.data[i * point_step + x_offset], sizeof(float));
+    std::memcpy(
+      &xy_cloud->points[i].y, &low_conf_xyz_cloud.data[i * point_step + y_offset], sizeof(float));
   }
-  for (size_t i = low_conf_xyz_cloud.points.size(); i < xy_cloud->points.size(); ++i) {
-    xy_cloud->points[i].x = high_conf_xyz_cloud.points[i - low_conf_xyz_cloud.points.size()].x;
-    xy_cloud->points[i].y = high_conf_xyz_cloud.points[i - low_conf_xyz_cloud.points.size()].y;
+
+  for (size_t i = low_conf_xyz_cloud.width; i < xy_cloud->points.size(); ++i) {
+    size_t high_conf_xyz_cloud_index = i - low_conf_xyz_cloud.width;
+    std::memcpy(
+      &xy_cloud->points[i].x,
+      &high_conf_xyz_cloud.data[high_conf_xyz_cloud_index * point_step + x_offset], sizeof(float));
+    std::memcpy(
+      &xy_cloud->points[i].y,
+      &high_conf_xyz_cloud.data[high_conf_xyz_cloud_index * point_step + y_offset], sizeof(float));
   }
 
   std::vector<int> k_indices(xy_cloud->points.size());
   std::vector<float> k_distances(xy_cloud->points.size());
   kd_tree_->setInputCloud(xy_cloud);
-  for (size_t i = 0; i < low_conf_xyz_cloud.points.size(); ++i) {
+
+  size_t output_size = 0;
+  size_t outlier_size = 0;
+  for (size_t i = 0; i < low_conf_xyz_cloud.data.size() / low_conf_xyz_cloud.point_step; ++i) {
     const float distance =
       std::hypot(xy_cloud->points[i].x - pose.position.x, xy_cloud->points[i].y - pose.position.y);
     const int min_points_threshold = std::min(
@@ -184,11 +195,20 @@ void RadiusSearch2dFilter::filter(
       kd_tree_->radiusSearch(i, search_radius_, k_indices, k_distances, min_points_threshold);
 
     if (min_points_threshold <= points_num) {
-      output.points.push_back(low_conf_xyz_cloud.points.at(i));
+      std::memcpy(
+        &output.data[output_size], &low_conf_xyz_cloud.data[i * low_conf_xyz_cloud.point_step],
+        low_conf_xyz_cloud.point_step);
+      output_size += low_conf_xyz_cloud.point_step;
     } else {
-      outlier.points.push_back(low_conf_xyz_cloud.points.at(i));
+      std::memcpy(
+        &outlier.data[outlier_size], &low_conf_xyz_cloud.data[i * low_conf_xyz_cloud.point_step],
+        low_conf_xyz_cloud.point_step);
+      outlier_size += low_conf_xyz_cloud.point_step;
     }
   }
+
+  output.data.resize(output_size);
+  outlier.data.resize(outlier_size);
 }
 
 OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(
@@ -239,21 +259,50 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(
 void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack(
   const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc)
 {
-  PclPointCloud tmp_behind_pc;
-  PclPointCloud tmp_front_pc;
-  for (sensor_msgs::PointCloud2ConstIterator<float> x(*input_pc, "x"), y(*input_pc, "y"),
-       z(*input_pc, "z"), intensity(*input_pc, "intensity");
-       x != x.end(); ++x, ++y, ++z, ++intensity) {
-    if (*x < 0.0) {
-      tmp_behind_pc.push_back(pcl::PointXYZI(*x, *y, *z, *intensity));
+  int x_offset = input_pc->fields[pcl::getFieldIndex(*input_pc, "x")].offset;
+  int point_step = input_pc->point_step;
+
+  front_pc.data.resize(input_pc->data.size());
+  behind_pc.data.resize(input_pc->data.size());
+  front_pc.point_step = input_pc->point_step;
+  behind_pc.point_step = input_pc->point_step;
+  front_pc.fields = input_pc->fields;
+  behind_pc.fields = input_pc->fields;
+  front_pc.height = input_pc->height;
+  behind_pc.height = input_pc->height;
+
+  size_t front_count = 0;
+  size_t behind_count = 0;
+
+  for (size_t global_offset = 0; global_offset < input_pc->data.size();
+       global_offset += point_step) {
+    float x;
+    std::memcpy(&x, &input_pc->data[global_offset + x_offset], sizeof(float));
+    if (x < 0.0) {
+      std::memcpy(
+        &behind_pc.data[behind_count * point_step], &input_pc->data[global_offset],
+        input_pc->point_step);
+      behind_count++;
     } else {
-      tmp_front_pc.push_back(pcl::PointXYZI(*x, *y, *z, *intensity));
+      std::memcpy(
+        &front_pc.data[front_count * point_step], &input_pc->data[global_offset],
+        input_pc->point_step);
+      front_count++;
     }
   }
-  pcl::toROSMsg(tmp_front_pc, front_pc);
-  pcl::toROSMsg(tmp_behind_pc, behind_pc);
+  front_pc.data.resize(front_count * point_step);
+  front_pc.width = front_count;
+  front_pc.row_step = front_count * front_pc.point_step;
   front_pc.header = input_pc->header;
+  front_pc.is_bigendian = input_pc->is_bigendian;
+  front_pc.is_dense = input_pc->is_dense;
+
+  behind_pc.data.resize(behind_count * point_step);
+  behind_pc.width = behind_count;
+  behind_pc.row_step = behind_count * behind_pc.point_step;
   behind_pc.header = input_pc->header;
+  behind_pc.is_bigendian = input_pc->is_bigendian;
+  behind_pc.is_dense = input_pc->is_dense;
 }
 void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
   const OccupancyGrid::ConstSharedPtr & input_ogm, const PointCloud2::ConstSharedPtr & input_pc)
@@ -264,6 +313,10 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
   PointCloud2 input_front_pc{};
   PointCloud2 input_behind_pc{};
   PointCloud2 ogm_frame_input_behind_pc{};
+  PointCloud2 ogm_frame_pc;
+  PointCloud2 input_front_pc;
+  PointCloud2 input_behind_pc;
+  PointCloud2 ogm_frame_input_behind_pc;
   splitPointCloudFrontBack(input_pc, input_front_pc, input_behind_pc);
   if (
     !transformPointcloud(input_front_pc, *tf2_, input_ogm->header.frame_id, ogm_frame_pc) ||
@@ -272,33 +325,50 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
     return;
   }
   // Occupancy grid map based filter
-  PclPointCloud high_confidence_pc{};
-  PclPointCloud low_confidence_pc{};
-  PclPointCloud out_ogm_pc{};
-  PclPointCloud ogm_frame_behind_pc;
-  pcl::fromROSMsg(ogm_frame_input_behind_pc, ogm_frame_behind_pc);
+  PointCloud2 high_confidence_pc{};
+  PointCloud2 low_confidence_pc{};
+  PointCloud2 out_ogm_pc{};
+  initializerPointCloud2(ogm_frame_pc, high_confidence_pc);
+  initializerPointCloud2(ogm_frame_pc, low_confidence_pc);
+  initializerPointCloud2(ogm_frame_pc, out_ogm_pc);
+  // PclPointCloud ogm_frame_behind_pc;
+  // pcl::fromROSMsg(ogm_frame_input_behind_pc, ogm_frame_behind_pc);
   filterByOccupancyGridMap(
     *input_ogm, ogm_frame_pc, high_confidence_pc, low_confidence_pc, out_ogm_pc);
   // Apply Radius search 2d filter for low confidence pointcloud
-  PclPointCloud filtered_low_confidence_pc{};
-  PclPointCloud outlier_pc{};
+  PointCloud2 filtered_low_confidence_pc{};
+  PointCloud2 outlier_pc{};
+  initializerPointCloud2(low_confidence_pc, outlier_pc);
+  initializerPointCloud2(low_confidence_pc, filtered_low_confidence_pc);
+  PointCloud2 outlier_pc;
+  outlier_pc.data.resize(low_confidence_pc.data.size());
+  outlier_pc.point_step = low_confidence_pc.point_step;
+  outlier_pc.fields = low_confidence_pc.fields;
+  outlier_pc.height = 1;
   if (radius_search_2d_filter_ptr_) {
     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 {
-    outlier_pc = low_confidence_pc;
+    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());
   }
   // Concatenate high confidence pointcloud from occupancy grid map and non-outlier pointcloud
-  PclPointCloud concat_pc =
-    high_confidence_pc + filtered_low_confidence_pc + out_ogm_pc + ogm_frame_behind_pc;
+  PointCloud2 ogm_frame_filtered_pc{};
+  concatPointCloud2(ogm_frame_filtered_pc, high_confidence_pc);
+  concatPointCloud2(ogm_frame_filtered_pc, filtered_low_confidence_pc);
+  concatPointCloud2(ogm_frame_filtered_pc, out_ogm_pc);
+  concatPointCloud2(ogm_frame_filtered_pc, ogm_frame_input_behind_pc);
+
+  finalizePointCloud2(ogm_frame_pc, ogm_frame_filtered_pc);
+  finalizePointCloud2(ogm_frame_pc, filtered_low_confidence_pc);
+  finalizePointCloud2(ogm_frame_pc, outlier_pc);
+  finalizePointCloud2(ogm_frame_pc, high_confidence_pc);
   // Convert to ros msg
   {
-    PointCloud2 ogm_frame_filtered_pc{};
     auto base_link_frame_filtered_pc_ptr = std::make_unique<PointCloud2>();
-    pcl::toROSMsg(concat_pc, ogm_frame_filtered_pc);
     ogm_frame_filtered_pc.header = ogm_frame_pc.header;
     if (!transformPointcloud(
           ogm_frame_filtered_pc, *tf2_, base_link_frame_, *base_link_frame_filtered_pc_ptr)) {
@@ -323,24 +393,82 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
   }
 }
 
+void OccupancyGridMapOutlierFilterComponent::initializerPointCloud2(
+  const PointCloud2 & input, PointCloud2 & output)
+{
+  output.header = input.header;
+  output.point_step = input.point_step;
+  output.fields = input.fields;
+  output.height = input.height;
+  output.is_bigendian = input.is_bigendian;
+  output.is_dense = input.is_dense;
+  output.data.resize(input.data.size());
+}
+
+void OccupancyGridMapOutlierFilterComponent::finalizePointCloud2(
+  const PointCloud2 & input, PointCloud2 & output)
+{
+  output.header = input.header;
+  output.point_step = input.point_step;
+  output.fields = input.fields;
+  output.height = input.height;
+  output.is_bigendian = input.is_bigendian;
+  output.is_dense = input.is_dense;
+  output.width = output.data.size() / output.point_step / output.height;
+  output.row_step = output.data.size() / output.height;
+}
+
+void OccupancyGridMapOutlierFilterComponent::concatPointCloud2(
+  PointCloud2 & output, const PointCloud2 & input)
+{
+  size_t output_size = output.data.size();
+  output.data.resize(output.data.size() + input.data.size());
+  std::memcpy(&output.data[output_size], &input.data[0], input.data.size());
+}
 void OccupancyGridMapOutlierFilterComponent::filterByOccupancyGridMap(
   const OccupancyGrid & occupancy_grid_map, const PointCloud2 & pointcloud,
-  PclPointCloud & high_confidence, PclPointCloud & low_confidence, PclPointCloud & out_ogm)
+  PointCloud2 & high_confidence, PointCloud2 & low_confidence, PointCloud2 & out_ogm)
 {
-  for (sensor_msgs::PointCloud2ConstIterator<float> x(pointcloud, "x"), y(pointcloud, "y"),
-       z(pointcloud, "z"), intensity(pointcloud, "intensity");
-       x != x.end(); ++x, ++y, ++z, ++intensity) {
-    const auto cost = getCost(occupancy_grid_map, *x, *y);
+  int x_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "x")].offset;
+  int y_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "y")].offset;
+  // int z_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "z")].offset;
+  // int intensity_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "intensity")].offset;
+  size_t high_confidence_size = 0;
+  size_t low_confidence_size = 0;
+  size_t out_ogm_size = 0;
+
+  for (size_t global_offset = 0; global_offset < pointcloud.data.size();
+       global_offset += pointcloud.point_step) {
+    float x;
+    float y;
+    std::memcpy(&x, &pointcloud.data[global_offset + x_offset], sizeof(float));
+    std::memcpy(&y, &pointcloud.data[global_offset + y_offset], sizeof(float));
+
+    const auto cost = getCost(occupancy_grid_map, x, y);
     if (cost) {
       if (cost_threshold_ < *cost) {
-        high_confidence.push_back(pcl::PointXYZI(*x, *y, *z, *intensity));
+        std::memcpy(
+          &high_confidence.data[high_confidence_size], &pointcloud.data[global_offset],
+          pointcloud.point_step);
+        high_confidence_size += pointcloud.point_step;
       } else {
-        low_confidence.push_back(pcl::PointXYZI(*x, *y, *z, *intensity));
+        std::memcpy(
+          &low_confidence.data[low_confidence_size], &pointcloud.data[global_offset],
+          pointcloud.point_step);
+        low_confidence_size += pointcloud.point_step;
       }
     } else {
-      out_ogm.push_back(pcl::PointXYZI(*x, *y, *z, *intensity));
+      std::memcpy(
+        &out_ogm.data[out_ogm_size], &pointcloud.data[global_offset], pointcloud.point_step);
+      out_ogm_size += pointcloud.point_step;
     }
   }
+  high_confidence.data.resize(high_confidence_size);
+  low_confidence.data.resize(low_confidence_size);
+  out_ogm.data.resize(out_ogm_size);
+  finalizePointCloud2(pointcloud, high_confidence);
+  finalizePointCloud2(pointcloud, low_confidence);
+  finalizePointCloud2(pointcloud, out_ogm);
 }
 
 OccupancyGridMapOutlierFilterComponent::Debugger::Debugger(
@@ -356,34 +484,31 @@ OccupancyGridMapOutlierFilterComponent::Debugger::Debugger(
 }
 
 void OccupancyGridMapOutlierFilterComponent::Debugger::publishOutlier(
-  const PclPointCloud & input, const Header & header)
+  const PointCloud2 & input, const Header & header)
 {
   auto output_ptr = std::make_unique<PointCloud2>();
   transformToBaseLink(input, header, *output_ptr);
   outlier_pointcloud_pub_->publish(std::move(output_ptr));
 }
 void OccupancyGridMapOutlierFilterComponent::Debugger::publishHighConfidence(
-  const PclPointCloud & input, const Header & header)
+  const PointCloud2 & input, const Header & header)
 {
   auto output_ptr = std::make_unique<PointCloud2>();
   transformToBaseLink(input, header, *output_ptr);
   high_confidence_pointcloud_pub_->publish(std::move(output_ptr));
 }
 
 void OccupancyGridMapOutlierFilterComponent::Debugger::publishLowConfidence(
-  const PclPointCloud & input, const Header & header)
+  const PointCloud2 & input, const Header & header)
 {
   auto output_ptr = std::make_unique<PointCloud2>();
   transformToBaseLink(input, header, *output_ptr);
   low_confidence_pointcloud_pub_->publish(std::move(output_ptr));
 }
 
 void OccupancyGridMapOutlierFilterComponent::Debugger::transformToBaseLink(
-  const PclPointCloud & pcl_input, const Header & header, PointCloud2 & output)
+  const PointCloud2 & ros_input, [[maybe_unused]] const Header & header, PointCloud2 & output)
 {
-  PointCloud2 ros_input{};
-  pcl::toROSMsg(pcl_input, ros_input);
-  ros_input.header = header;
   transformPointcloud(ros_input, *(node_.tf2_), node_.base_link_frame_, output);
 }