From afee3e1b307aeaa9bce790cee37aa65aab68bafc Mon Sep 17 00:00:00 2001 From: Hirokazu Ishida Date: Sun, 20 Mar 2022 07:57:40 +0900 Subject: [PATCH] [refactor] tmp --- .../dummy_perception_publisher/node.hpp | 22 ++--- .../dummy_perception_publisher/src/node.cpp | 92 ++++++++++--------- 2 files changed, 61 insertions(+), 53 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 cd7c335617050..8a1db6deab76c 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -39,7 +39,9 @@ struct ObjectInfo { ObjectInfo( - const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time); + const dummy_perception_publisher::msg::Object & object, size_t idx, + const rclcpp::Time & current_time); + size_t idx; double length; double width; double height; @@ -50,6 +52,14 @@ struct ObjectInfo tf2::Transform tf_map2moved_object; }; +void createObjectPointcloudObjectCentric( + const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud); + +void createObjectPointcloudVehicleCentric( + const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud); + class DummyPerceptionPublisherNode : public rclcpp::Node { private: @@ -72,16 +82,6 @@ class DummyPerceptionPublisherNode : public rclcpp::Node std::mt19937 random_generator_; void timerCallback(); - void createObjectPointcloudObjectCentric( - const double length, const double width, const double height, const double std_dev_x, - const double std_dev_y, const double std_dev_z, - const tf2::Transform & tf_base_link2moved_object, - pcl::PointCloud::Ptr & pointcloud_ptr); - void createObjectPointcloudVehicleCentric( - const double length, const double width, const double height, const double std_dev_x, - const double std_dev_y, const double std_dev_z, - const tf2::Transform & tf_base_link2moved_object, - pcl::PointCloud::Ptr & pointcloud_ptr); void objectCallback(const dummy_perception_publisher::msg::Object::ConstSharedPtr msg); public: diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 239c2a50f8b60..3591d5eba6cb9 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -49,8 +50,10 @@ pcl::PointXYZ getBaseLinkToPoint( }; ObjectInfo::ObjectInfo( - const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time) -: length(object.shape.dimensions.x), + const dummy_perception_publisher::msg::Object & object, size_t idx_, + const rclcpp::Time & current_time) +: idx(idx_), + length(object.shape.dimensions.x), width(object.shape.dimensions.y), height(object.shape.dimensions.z), std_dev_x(std::sqrt(object.initial_state.pose_covariance.covariance[0])), @@ -170,18 +173,16 @@ void DummyPerceptionPublisherNode::timerCallback() tf_map2moved_object = tf_map2object_origin * tf_object_origin2moved_object; tf2::toMsg(tf_map2moved_object, output_moved_object_pose.pose); + const auto obj_info = ObjectInfo(objects_.at(i), i, current_time); + // pointcloud pcl::PointCloud::Ptr pointcloud_ptr(new pcl::PointCloud); if (object_centric_pointcloud_) { createObjectPointcloudObjectCentric( - objects_.at(i).shape.dimensions.x, objects_.at(i).shape.dimensions.y, - objects_.at(i).shape.dimensions.z, std_dev_x, std_dev_y, std_dev_z, - tf_base_link2map * tf_map2moved_object, pointcloud_ptr); + obj_info, tf_base_link2map, random_generator_, pointcloud_ptr); } else { createObjectPointcloudVehicleCentric( - objects_.at(i).shape.dimensions.x, objects_.at(i).shape.dimensions.y, - objects_.at(i).shape.dimensions.z, std_dev_x, std_dev_y, std_dev_z, - tf_base_link2map * tf_map2moved_object, pointcloud_ptr); + obj_info, tf_base_link2map, random_generator_, pointcloud_ptr); } v_pointcloud.push_back(pointcloud_ptr); @@ -285,14 +286,13 @@ void DummyPerceptionPublisherNode::timerCallback() } } -void DummyPerceptionPublisherNode::createObjectPointcloudObjectCentric( - const double length, const double width, const double height, const double std_dev_x, - const double std_dev_y, const double std_dev_z, const tf2::Transform & tf_base_link2moved_object, - pcl::PointCloud::Ptr & pointcloud_ptr) +void createObjectPointcloudObjectCentric( + const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud) { - std::normal_distribution<> x_random(0.0, std_dev_x); - std::normal_distribution<> y_random(0.0, std_dev_y); - std::normal_distribution<> z_random(0.0, std_dev_z); + std::normal_distribution<> x_random(0.0, obj_info.std_dev_x); + std::normal_distribution<> y_random(0.0, obj_info.std_dev_y); + std::normal_distribution<> z_random(0.0, obj_info.std_dev_z); const double epsilon = 0.001; const double step = 0.05; const double vertical_theta_step = (1.0 / 180.0) * M_PI; @@ -302,34 +302,40 @@ void DummyPerceptionPublisherNode::createObjectPointcloudObjectCentric( const double horizontal_min_theta = (-180.0 / 180.0) * M_PI; const double horizontal_max_theta = (180.0 / 180.0) * M_PI; - const double min_z = -1.0 * (height / 2.0) + tf_base_link2moved_object.getOrigin().z(); - const double max_z = 1.0 * (height / 2.0) + tf_base_link2moved_object.getOrigin().z(); + const auto tf_base_link2moved_object = tf_base_link2map * obj_info.tf_map2moved_object; + + const double min_z = -1.0 * (obj_info.height / 2.0) + tf_base_link2moved_object.getOrigin().z(); + const double max_z = 1.0 * (obj_info.height / 2.0) + tf_base_link2moved_object.getOrigin().z(); pcl::PointCloud horizontal_candidate_pointcloud; pcl::PointCloud horizontal_pointcloud; { - const double y = -1.0 * (width / 2.0); - for (double x = -1.0 * (length / 2.0); x <= ((length / 2.0) + epsilon); x += step) { + const double y = -1.0 * (obj_info.height / 2.0); + for (double x = -1.0 * (obj_info.length / 2.0); x <= ((obj_info.length / 2.0) + epsilon); + x += step) { horizontal_candidate_pointcloud.push_back( getBaseLinkToPoint(tf_base_link2moved_object, x, y, 0.0)); } } { - const double y = 1.0 * (width / 2.0); - for (double x = -1.0 * (length / 2.0); x <= ((length / 2.0) + epsilon); x += step) { + const double y = 1.0 * (obj_info.height / 2.0); + for (double x = -1.0 * (obj_info.length / 2.0); x <= ((obj_info.length / 2.0) + epsilon); + x += step) { horizontal_candidate_pointcloud.push_back( getBaseLinkToPoint(tf_base_link2moved_object, x, y, 0.0)); } } { - const double x = -1.0 * (length / 2.0); - for (double y = -1.0 * (width / 2.0); y <= ((width / 2.0) + epsilon); y += step) { + const double x = -1.0 * (obj_info.length / 2.0); + for (double y = -1.0 * (obj_info.height / 2.0); y <= ((obj_info.height / 2.0) + epsilon); + y += step) { horizontal_candidate_pointcloud.push_back( getBaseLinkToPoint(tf_base_link2moved_object, x, y, 0.0)); } } { - const double x = 1.0 * (length / 2.0); - for (double y = -1.0 * (width / 2.0); y <= ((width / 2.0) + epsilon); y += step) { + const double x = 1.0 * (obj_info.length / 2.0); + for (double y = -1.0 * (obj_info.height / 2.0); y <= ((obj_info.height / 2.0) + epsilon); + y += step) { horizontal_candidate_pointcloud.push_back( getBaseLinkToPoint(tf_base_link2moved_object, x, y, 0.0)); } @@ -356,6 +362,7 @@ void DummyPerceptionPublisherNode::createObjectPointcloudObjectCentric( horizontal_ray_traced_pointcloud_indices.at(index) = i; } } + for (const auto & pointcloud_index : horizontal_ray_traced_pointcloud_indices) { if (pointcloud_index != no_data) { // generate vertical point @@ -369,30 +376,31 @@ void DummyPerceptionPublisherNode::createObjectPointcloudObjectCentric( if (min_z <= z && z <= max_z + epsilon) { pcl::PointXYZ point; point.x = - horizontal_candidate_pointcloud.at(pointcloud_index).x + x_random(random_generator_); + horizontal_candidate_pointcloud.at(pointcloud_index).x + x_random(random_generator); point.y = - horizontal_candidate_pointcloud.at(pointcloud_index).y + y_random(random_generator_); - point.z = z + z_random(random_generator_); - pointcloud_ptr->push_back(point); + horizontal_candidate_pointcloud.at(pointcloud_index).y + y_random(random_generator); + point.z = z + z_random(random_generator); + pointcloud->push_back(point); } } } } } -void DummyPerceptionPublisherNode::createObjectPointcloudVehicleCentric( - const double length, const double width, const double height, const double std_dev_x, - const double std_dev_y, const double std_dev_z, const tf2::Transform & tf_base_link2moved_object, - pcl::PointCloud::Ptr & pointcloud_ptr) +void createObjectPointcloudVehicleCentric( + const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud) { + const double horizontal_theta_step = 0.25 * M_PI / 180.0; + const auto tf_base_link2moved_object = tf_base_link2map * obj_info.tf_map2moved_object; const auto tf_moved_object2base_link = tf_base_link2moved_object.inverse(); const tf2::Transform tf_rotonly(tf_moved_object2base_link.getRotation()); // For vector rotation const auto sdf_box = [&](tf2::Vector3 p) { // As for signd distance field for a box, please refere: // https://www.iquilezles.org/www/articles/distfunctions/distfunctions.htm - const auto sd_val_x = std::abs(p.getX()) - 0.5 * length; - const auto sd_val_y = std::abs(p.getY()) - 0.5 * width; + const auto sd_val_x = std::abs(p.getX()) - 0.5 * obj_info.length; + const auto sd_val_y = std::abs(p.getY()) - 0.5 * obj_info.height; const auto positive_dist_x = std::max(sd_val_x, 0.0); const auto positive_dist_y = std::max(sd_val_y, 0.0); const auto positive_dist = std::hypot(positive_dist_x, positive_dist_y); @@ -420,27 +428,27 @@ void DummyPerceptionPublisherNode::createObjectPointcloudVehicleCentric( return std::numeric_limits::infinity(); }; - std::normal_distribution<> x_random(0.0, std_dev_x); - std::normal_distribution<> y_random(0.0, std_dev_y); - std::normal_distribution<> z_random(0.0, std_dev_z); + std::normal_distribution<> x_random(0.0, obj_info.std_dev_x); + std::normal_distribution<> y_random(0.0, obj_info.std_dev_y); + std::normal_distribution<> z_random(0.0, obj_info.std_dev_z); double angle = 0.0; - const size_t n_scan = static_cast(std::floor(2 * M_PI / angle_increment_)); + const size_t n_scan = static_cast(std::floor(2 * M_PI / horizontal_theta_step)); for (size_t i = 0; i < n_scan; ++i) { - angle += angle_increment_; + angle += horizontal_theta_step; const tf2::Vector3 dir_wrt_car(cos(angle), sin(angle), 0.0); const tf2::Vector3 dir_wrt_obj = tf_rotonly * dir_wrt_car; const auto start_ray_pos = tf_moved_object2base_link.getOrigin(); const tf2::Vector3 pos_noise( - -x_random(random_generator_), -y_random(random_generator_), -z_random(random_generator_)); + -x_random(random_generator), -y_random(random_generator), -z_random(random_generator)); const auto start_ray_pos_with_noise = start_ray_pos + pos_noise; const float ray_dist = compute_dist_by_spheretrace(start_ray_pos_with_noise, dir_wrt_obj); if (std::isfinite(ray_dist)) { const auto ray_tip = start_ray_pos_with_noise + ray_dist * dir_wrt_obj; const auto point = getBaseLinkToPoint( tf_base_link2moved_object, ray_tip.getX(), ray_tip.getY(), ray_tip.getZ()); - pointcloud_ptr->push_back(point); + pointcloud->push_back(point); } } }