diff --git a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp index 5d806b3c2e31b..725c3f612f85a 100644 --- a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp +++ b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "dummy_perception_publisher/node.hpp" +#include 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::Ptr ray_traced_merged_pointcloud_ptr( + new pcl::PointCloud); + pcl::VoxelGridOcclusionEstimation 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::Ptr ray_traced_pointcloud_ptr( + new pcl::PointCloud); + 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; }