[Tmp]

simulator/dummy_perception_publisher/src/pointcloud_creator.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "dummy_perception_publisher/node.hpp"
+#include <pcl/filters/voxel_grid_occlusion_estimation.h>
 
 pcl::PointXYZ getBaseLinkToPoint(
   const tf2::Transform & tf_base_link2moved_object, double x, double y, double z)
@@ -158,9 +159,38 @@ void ObjectCentricPointCloudCreator::create_multi(
     }
   }
 
-  pointclouds = pointclouds_tmp;
-  std::cout << merged_pointcloud_tmp.use_count() << std::endl;
-  merged_pointcloud = merged_pointcloud_tmp;
+  if (!enable_ray_tracing_) {
+    pointclouds = pointclouds_tmp;
+    merged_pointcloud = merged_pointcloud_tmp;
+    return;
+  }
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr ray_traced_merged_pointcloud_ptr(
+    new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::VoxelGridOcclusionEstimation<pcl::PointXYZ> ray_tracing_filter;
+  ray_tracing_filter.setInputCloud(merged_pointcloud_tmp);
+  ray_tracing_filter.setLeafSize(0.25, 0.25, 0.25);
+  ray_tracing_filter.initializeVoxelGrid();
+  for (size_t i = 0; i < pointclouds_tmp.size(); ++i) {
+    pcl::PointCloud<pcl::PointXYZ>::Ptr ray_traced_pointcloud_ptr(
+      new pcl::PointCloud<pcl::PointXYZ>);
+    for (size_t j = 0; j < pointclouds_tmp.at(i)->size(); ++j) {
+      Eigen::Vector3i grid_coordinates = ray_tracing_filter.getGridCoordinates(
+        pointclouds_tmp.at(i)->at(j).x, pointclouds_tmp.at(i)->at(j).y, pointclouds_tmp.at(i)->at(j).z);
+      int grid_state;
+      if (ray_tracing_filter.occlusionEstimation(grid_state, grid_coordinates) != 0) {
+        RCLCPP_ERROR(rclcpp::get_logger("dummy_perception_publisher"), "ray tracing failed");
+      }
+      if (grid_state == 1) {  // occluded
+        continue;
+      } else {  // not occluded
+        ray_traced_pointcloud_ptr->push_back(pointclouds_tmp.at(i)->at(j));
+        ray_traced_merged_pointcloud_ptr->push_back(pointclouds_tmp.at(i)->at(j));
+      }
+    }
+    pointclouds.push_back(ray_traced_pointcloud_ptr);
+  }
+  merged_pointcloud = ray_traced_merged_pointcloud_ptr;
 }