ray tracing in dummy perception publisher is too heavy to publish objects in 10Hz

[satoshi-ota]

When ray tracing is enabled and I set multiple pedestrians by using rviz plugins, the topic /perception/object_recognition/detection/labeled_clusters may not be output at 10Hz due to heavy ray tracing processing.

I measured following process:

autoware.universe/simulator/dummy_perception_publisher/src/node.cpp

Lines 177 to 211 in 407c802

	// ray tracing
	if (enable_ray_tracing_) {
	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_ptr);
	// above this value raytrace won't be as expected
	const double leaf_size = 0.02;
	ray_tracing_filter.setLeafSize(leaf_size, leaf_size, leaf_size);
	ray_tracing_filter.initializeVoxelGrid();
	for (size_t i = 0; i < v_pointcloud.size(); ++i) {
	pcl::PointCloud<pcl::PointXYZ>::Ptr ray_traced_pointcloud_ptr(
	new pcl::PointCloud<pcl::PointXYZ>);
	for (size_t j = 0; j < v_pointcloud.at(i)->size(); ++j) {
	Eigen::Vector3i grid_coordinates = ray_tracing_filter.getGridCoordinates(
	v_pointcloud.at(i)->at(j).x, v_pointcloud.at(i)->at(j).y, v_pointcloud.at(i)->at(j).z);
	int grid_state;
	if (ray_tracing_filter.occlusionEstimation(grid_state, grid_coordinates) != 0) {
	RCLCPP_ERROR(get_logger(), "ray tracing failed");
	}
	if (grid_state == 1) { // occluded
	continue;
	} else { // not occluded
	ray_traced_pointcloud_ptr->push_back(v_pointcloud.at(i)->at(j));
	ray_traced_merged_pointcloud_ptr->push_back(v_pointcloud.at(i)->at(j));
	}
	}
	pcl::toROSMsg(
	*ray_traced_pointcloud_ptr,
	output_dynamic_object_msg.feature_objects.at(i).feature.cluster);
	output_dynamic_object_msg.feature_objects.at(i).feature.cluster.header.frame_id = "base_link";
	output_dynamic_object_msg.feature_objects.at(i).feature.cluster.header.stamp = current_time;
	}
	pcl::toROSMsg(*ray_traced_merged_pointcloud_ptr, output_pointcloud_msg);
	}

and when there were a large number of pedestrians, the processing time was actually increased as follows.

• no or few pedestrians

[dummy_perception_publisher_node-32] processing_time 0.000497871 ms                                                                                                                                            
[dummy_perception_publisher_node-32] processing_time 0.00054048 ms                                                                                                                                             
[dummy_perception_publisher_node-32] processing_time 0.00111424 ms                                                                                                                                             
[dummy_perception_publisher_node-32] processing_time 0.00144731 ms                                                                                                                                             
[dummy_perception_publisher_node-32] processing_time 0.000327956 ms                                                                                                                                            
[dummy_perception_publisher_node-32] processing_time 0.000686971 ms                                                                                                                                            

• many pedestrians

[dummy_perception_publisher_node-32] processing_time 0.16171 ms                                                                                                                                                
[dummy_perception_publisher_node-32] processing_time 0.157384 ms                                                                                                                                               
[dummy_perception_publisher_node-32] processing_time 0.300267 ms                                                                                                                                               
[dummy_perception_publisher_node-32] processing_time 0.299193 ms                                                                                                                                               
[dummy_perception_publisher_node-32] processing_time 0.28528 ms                                                                                                                                                
[dummy_perception_publisher_node-32] processing_time 0.313369 ms  

I think default reaf size should be changeable from launch file.

my environment

ThinkPad Extreme
Memory 32GB
CPU Intel® Core™ i7-10750H CPU @ 2.60GHz × 12
GPU NVIDIA Corporation / NVIDIA GeForce GTX 1650 Ti with Max-Q Design/PCIe/SSE2
OS Ubuntu20.04