From cbab6a59335894a391c90d906a85975debad05ae Mon Sep 17 00:00:00 2001 From: Hirokazu Ishida Date: Thu, 7 Apr 2022 03:27:22 +0900 Subject: [PATCH] Refactor --- .../dummy_perception_publisher/node.hpp | 12 ++++++------ .../dummy_perception_publisher/src/node.cpp | 5 ++--- .../src/pointcloud_creator.cpp | 18 ++++++++++-------- 3 files changed, 18 insertions(+), 17 deletions(-) diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp index 7536ff3650cef..910cd9724041f 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -59,9 +59,9 @@ class PointCloudCreator const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud) const = 0; - virtual void create_multi( + virtual std::vector::Ptr> create_pointclouds( const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, - std::mt19937 & random_generator, std::vector::Ptr> & pointclouds, + std::mt19937 & random_generator, pcl::PointCloud::Ptr & merged_pointcloud) const = 0; }; @@ -77,9 +77,9 @@ class ObjectCentricPointCloudCreator : public PointCloudCreator const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud) const override; - void create_multi( + std::vector::Ptr> create_pointclouds( const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, - std::mt19937 & random_generator, std::vector::Ptr> & pointclouds, + std::mt19937 & random_generator, pcl::PointCloud::Ptr & merged_pointcloud) const override; bool enable_ray_tracing_; @@ -91,9 +91,9 @@ class VehicleCentricPointCloudCreator : public PointCloudCreator const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud) const override; - void create_multi( + std::vector::Ptr> create_pointclouds( const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, - std::mt19937 & random_generator, std::vector::Ptr> & pointclouds, + std::mt19937 & random_generator, pcl::PointCloud::Ptr & merged_pointcloud) const override; }; diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index a663f5374df93..91dab0687b98c 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -142,10 +142,9 @@ void DummyPerceptionPublisherNode::timerCallback() obj_infos.push_back(obj_info); } - std::vector::Ptr> v_pointcloud; pcl::PointCloud::Ptr merged_pointcloud_ptr(new pcl::PointCloud); - pointcloud_creator_->create_multi( - obj_infos, tf_base_link2map, random_generator_, v_pointcloud, merged_pointcloud_ptr); + const auto v_pointcloud = pointcloud_creator_->create_pointclouds( + obj_infos, tf_base_link2map, random_generator_, merged_pointcloud_ptr); pcl::toROSMsg(*merged_pointcloud_ptr, output_pointcloud_msg); std::vector delete_idxs; diff --git a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp index 57d173ecc0cb6..a210e2d8ee41a 100644 --- a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp +++ b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp @@ -132,10 +132,9 @@ void ObjectCentricPointCloudCreator::create( } } -void ObjectCentricPointCloudCreator::create_multi( +std::vector::Ptr> ObjectCentricPointCloudCreator::create_pointclouds( const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, - std::mt19937 & random_generator, std::vector::Ptr> & pointclouds, - pcl::PointCloud::Ptr & merged_pointcloud) const + std::mt19937 & random_generator, pcl::PointCloud::Ptr & merged_pointcloud) const { std::vector::Ptr> pointclouds_tmp; pcl::PointCloud::Ptr merged_pointcloud_tmp(new pcl::PointCloud); @@ -153,9 +152,8 @@ void ObjectCentricPointCloudCreator::create_multi( } if (!enable_ray_tracing_) { - pointclouds = pointclouds_tmp; merged_pointcloud = merged_pointcloud_tmp; - return; + return pointclouds_tmp; } pcl::PointCloud::Ptr ray_traced_merged_pointcloud_ptr( @@ -164,6 +162,7 @@ void ObjectCentricPointCloudCreator::create_multi( ray_tracing_filter.setInputCloud(merged_pointcloud_tmp); ray_tracing_filter.setLeafSize(0.25, 0.25, 0.25); ray_tracing_filter.initializeVoxelGrid(); + std::vector::Ptr> pointclouds; for (size_t i = 0; i < pointclouds_tmp.size(); ++i) { pcl::PointCloud::Ptr ray_traced_pointcloud_ptr( new pcl::PointCloud); @@ -185,6 +184,7 @@ void ObjectCentricPointCloudCreator::create_multi( pointclouds.push_back(ray_traced_pointcloud_ptr); } merged_pointcloud = ray_traced_merged_pointcloud_ptr; + return pointclouds; } void VehicleCentricPointCloudCreator::create( @@ -213,11 +213,12 @@ void VehicleCentricPointCloudCreator::create( } } -void VehicleCentricPointCloudCreator::create_multi( +std::vector::Ptr> +VehicleCentricPointCloudCreator::create_pointclouds( const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, - std::mt19937 & random_generator, std::vector::Ptr> & pointclouds, - pcl::PointCloud::Ptr & merged_pointcloud) const + std::mt19937 & random_generator, pcl::PointCloud::Ptr & merged_pointcloud) const { + std::vector::Ptr> pointclouds; for (const auto & obj_info : obj_infos) { pcl::PointCloud::Ptr pointcloud_shared_ptr(new pcl::PointCloud); this->create(obj_info, tf_base_link2map, random_generator, pointcloud_shared_ptr); @@ -229,4 +230,5 @@ void VehicleCentricPointCloudCreator::create_multi( merged_pointcloud->push_back(pointclouds.at(i)->at(j)); } } + return pointclouds; }