From 53829138b24f1e0b7cda78af50b139dba80d004e Mon Sep 17 00:00:00 2001 From: Hirokazu Ishida <38597814+HiroIshida@users.noreply.github.com> Date: Thu, 28 Apr 2022 22:37:50 +0900 Subject: [PATCH] feat(dummy_perception_publisher): publish realistic dummy pointcloud using raymarchig (#527) * Create pointcloud by raycasting from vehicle Signed-off-by: Hirokazu Ishida * [after-review] Use vector of ObjectInfo Signed-off-by: Hirokazu Ishida * [after-review] Implemented by strategy pattern Signed-off-by: Hirokazu Ishida * [after-review] split files Signed-off-by: Hirokazu Ishida * Use pcl raytracing Tmp Tmp Tmp Signed-off-by: Hirokazu Ishida * Add signed distance function lib Signed-off-by: Hirokazu Ishida * Use sdf library Signed-off-by: Hirokazu Ishida * Remove no longer used functions Signed-off-by: Hirokazu Ishida * Refactor Signed-off-by: Hirokazu Ishida * Simplify getPoint Signed-off-by: Hirokazu Ishida * Raytracing considering inter object relation Signed-off-by: Hirokazu Ishida * Add random noise Signed-off-by: Hirokazu Ishida * Default is object centric Signed-off-by: Hirokazu Ishida * Return if no objects are detected Signed-off-by: Hirokazu Ishida * Change definition of tf_global_to_local (same as other autoware codes) Signed-off-by: Hirokazu Ishida * Remove create method Signed-off-by: Hirokazu Ishida * Reaname: VehicleCentric -> EgoCentric Signed-off-by: Hirokazu Ishida * Refactor a bit Signed-off-by: Hirokazu Ishida * Tune parameter Signed-off-by: Hirokazu Ishida * Fix: Even if selected_idices is zero, pointclouds must be published Signed-off-by: Hirokazu Ishida * Fix launch file Signed-off-by: Hirokazu Ishida * Fix typo Signed-off-by: Hirokazu Ishida * Fix: create merged pointcloud when no idx is selected Signed-off-by: Hirokazu Ishida * Use ray-maching by default Signed-off-by: Hirokazu Ishida --- .../dummy_perception_publisher/CMakeLists.txt | 20 +- .../dummy_perception_publisher/node.hpp | 64 +++- .../signed_distance_function.hpp | 74 ++++ .../dummy_perception_publisher.launch.xml | 1 + .../dummy_perception_publisher/src/node.cpp | 326 ++++++------------ .../src/pointcloud_creator.cpp | 236 +++++++++++++ .../src/signed_distance_function.cpp | 84 +++++ .../src/test_signed_distance_function.cpp | 84 +++++ 8 files changed, 661 insertions(+), 228 deletions(-) create mode 100644 simulator/dummy_perception_publisher/include/dummy_perception_publisher/signed_distance_function.hpp create mode 100644 simulator/dummy_perception_publisher/src/pointcloud_creator.cpp create mode 100644 simulator/dummy_perception_publisher/src/signed_distance_function.cpp create mode 100644 simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp diff --git a/simulator/dummy_perception_publisher/CMakeLists.txt b/simulator/dummy_perception_publisher/CMakeLists.txt index 7e150cf96f087..78da0887e8d18 100644 --- a/simulator/dummy_perception_publisher/CMakeLists.txt +++ b/simulator/dummy_perception_publisher/CMakeLists.txt @@ -12,7 +12,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() # Dependencies for messages -find_package(ament_cmake REQUIRED) +find_package(ament_cmake_auto REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(autoware_auto_perception_msgs REQUIRED) find_package(tier4_perception_msgs REQUIRED) @@ -43,9 +43,18 @@ set(${PROJECT_NAME}_DEPENDENCIES tf2_ros ) +ament_auto_add_library(signed_distance_function SHARED + src/signed_distance_function.cpp +) + ament_auto_add_executable(dummy_perception_publisher_node src/main.cpp src/node.cpp + src/pointcloud_creator.cpp +) + +target_link_libraries(dummy_perception_publisher_node + signed_distance_function ) ament_target_dependencies(dummy_perception_publisher_node ${${PROJECT_NAME}_DEPENDENCIES}) @@ -85,6 +94,15 @@ ament_auto_add_executable(empty_objects_publisher if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(signed_distance_function-test + test/src/test_signed_distance_function.cpp + ) + target_link_libraries(signed_distance_function-test + signed_distance_function + ) endif() ament_auto_package( 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 ccbbf2308a310..07c6d2d848a4e 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -33,9 +33,64 @@ #include #include +#include #include #include +struct ObjectInfo +{ + ObjectInfo( + const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time); + double length; + double width; + double height; + double std_dev_x; + double std_dev_y; + double std_dev_z; + double std_dev_yaw; + tf2::Transform tf_map2moved_object; +}; + +class PointCloudCreator +{ +public: + virtual ~PointCloudCreator() {} + + virtual std::vector::Ptr> create_pointclouds( + const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, + pcl::PointCloud::Ptr & merged_pointcloud) const = 0; +}; + +class ObjectCentricPointCloudCreator : public PointCloudCreator +{ +public: + explicit ObjectCentricPointCloudCreator(bool enable_ray_tracing) + : enable_ray_tracing_(enable_ray_tracing) + { + } + + std::vector::Ptr> create_pointclouds( + const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, + pcl::PointCloud::Ptr & merged_pointcloud) const override; + +private: + void create_object_pointcloud( + const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud) const; + + bool enable_ray_tracing_; +}; + +class EgoCentricPointCloudCreator : public PointCloudCreator +{ + std::vector::Ptr> create_pointclouds( + const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, + pcl::PointCloud::Ptr & merged_pointcloud) const override; +}; + class DummyPerceptionPublisherNode : public rclcpp::Node { private: @@ -52,13 +107,12 @@ class DummyPerceptionPublisherNode : public rclcpp::Node bool enable_ray_tracing_; bool use_object_recognition_; bool use_real_param_; + std::unique_ptr pointcloud_creator_; + + double angle_increment_; + std::mt19937 random_generator_; void timerCallback(); - void createObjectPointcloud( - 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/include/dummy_perception_publisher/signed_distance_function.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/signed_distance_function.hpp new file mode 100644 index 0000000000000..a3768394fd1e9 --- /dev/null +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/signed_distance_function.hpp @@ -0,0 +1,74 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DUMMY_PERCEPTION_PUBLISHER__SIGNED_DISTANCE_FUNCTION_HPP_ +#define DUMMY_PERCEPTION_PUBLISHER__SIGNED_DISTANCE_FUNCTION_HPP_ + +#include + +#include +#include +#include +#include + +namespace signed_distance_function +{ + +class AbstractSignedDistanceFunction +{ +public: + virtual double operator()(double x, double y) const = 0; + double getSphereTracingDist( + double x_start, double y_start, double angle, double eps = 1e-2) const; + virtual ~AbstractSignedDistanceFunction() {} +}; + +class BoxSDF : public AbstractSignedDistanceFunction +{ +public: + BoxSDF(double length, double width, tf2::Transform tf_global_to_local) + : length_(length), width_(width), tf_global_to_local_(tf_global_to_local) + { + } + double operator()(double x, double y) const override; + +private: + double length_; + double width_; + // tf_global_to_local_ is NOT a transfrom of basis, rather just a coordinate of local w.r.t. + // global + tf2::Transform tf_global_to_local_; +}; + +class CompisiteSDF : public AbstractSignedDistanceFunction +{ +public: + explicit CompisiteSDF(std::vector> sdf_ptrs) + : sdf_ptrs_(std::move(sdf_ptrs)) + { + if (sdf_ptrs_.empty()) { + throw std::runtime_error("sdf_ptrs must not be empty"); + } + } + double operator()(double x, double y) const override; + + size_t nearest_sdf_index(double x, double y) const; + +private: + std::vector> sdf_ptrs_; +}; + +} // namespace signed_distance_function + +#endif // DUMMY_PERCEPTION_PUBLISHER__SIGNED_DISTANCE_FUNCTION_HPP_ diff --git a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml index 3d013a0ab582a..0d17feffed21f 100644 --- a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml +++ b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml @@ -18,6 +18,7 @@ + diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index cf4caf61d01e8..4fc9f2873a684 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -16,6 +16,8 @@ #include #include +#include +#include #include #include @@ -25,6 +27,34 @@ #include #include +ObjectInfo::ObjectInfo( + const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time) +: 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])), + std_dev_y(std::sqrt(object.initial_state.pose_covariance.covariance[7])), + std_dev_z(std::sqrt(object.initial_state.pose_covariance.covariance[14])), + std_dev_yaw(std::sqrt(object.initial_state.pose_covariance.covariance[35])) +{ + const double move_distance = + object.initial_state.twist_covariance.twist.linear.x * + (current_time.seconds() - rclcpp::Time(object.header.stamp).seconds()); + tf2::Transform tf_object_origin2moved_object; + tf2::Transform tf_map2object_origin; + { + geometry_msgs::msg::Transform ros_object_origin2moved_object; + ros_object_origin2moved_object.translation.x = move_distance; + ros_object_origin2moved_object.rotation.x = 0; + ros_object_origin2moved_object.rotation.y = 0; + ros_object_origin2moved_object.rotation.z = 0; + ros_object_origin2moved_object.rotation.w = 1; + tf2::fromMsg(ros_object_origin2moved_object, tf_object_origin2moved_object); + } + tf2::fromMsg(object.initial_state.pose_covariance.pose, tf_map2object_origin); + this->tf_map2moved_object = tf_map2object_origin * tf_object_origin2moved_object; +} + DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() : Node("dummy_perception_publisher"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { @@ -33,6 +63,19 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() enable_ray_tracing_ = this->declare_parameter("enable_ray_tracing", true); use_object_recognition_ = this->declare_parameter("use_object_recognition", true); + const bool object_centric_pointcloud = + this->declare_parameter("object_centric_pointcloud", false); + + if (object_centric_pointcloud) { + pointcloud_creator_ = + std::unique_ptr(new ObjectCentricPointCloudCreator(enable_ray_tracing_)); + } else { + pointcloud_creator_ = std::unique_ptr(new EgoCentricPointCloudCreator()); + } + + // parameters for vehicle centric point cloud generation + angle_increment_ = this->declare_parameter("angle_increment", 0.25 * M_PI / 180.0); + std::random_device seed_gen; random_generator_.seed(seed_gen()); @@ -84,128 +127,77 @@ void DummyPerceptionPublisherNode::timerCallback() return; } - std::vector::Ptr> v_pointcloud; - std::vector delete_idxs; + std::vector selected_indices{}; static std::uniform_real_distribution<> detection_successful_random(0.0, 1.0); for (size_t i = 0; i < objects_.size(); ++i) { - if (detection_successful_rate_ < detection_successful_random(random_generator_)) { - continue; + if (detection_successful_rate_ >= detection_successful_random(random_generator_)) { + selected_indices.push_back(i); } - const double std_dev_x = std::sqrt(objects_.at(i).initial_state.pose_covariance.covariance[0]); - const double std_dev_y = std::sqrt(objects_.at(i).initial_state.pose_covariance.covariance[7]); - const double std_dev_z = std::sqrt(objects_.at(i).initial_state.pose_covariance.covariance[14]); - const double std_dev_yaw = - std::sqrt(objects_.at(i).initial_state.pose_covariance.covariance[35]); - const double move_distance = - objects_.at(i).initial_state.twist_covariance.twist.linear.x * - (current_time.seconds() - rclcpp::Time(objects_.at(i).header.stamp).seconds()); - tf2::Transform tf_object_origin2moved_object; - tf2::Transform tf_map2object_origin; - tf2::Transform tf_map2moved_object; - { - geometry_msgs::msg::Transform ros_object_origin2moved_object; - ros_object_origin2moved_object.translation.x = move_distance; - ros_object_origin2moved_object.rotation.x = 0; - ros_object_origin2moved_object.rotation.y = 0; - ros_object_origin2moved_object.rotation.z = 0; - ros_object_origin2moved_object.rotation.w = 1; - tf2::fromMsg(ros_object_origin2moved_object, tf_object_origin2moved_object); - } - tf2::fromMsg(objects_.at(i).initial_state.pose_covariance.pose, tf_map2object_origin); - tf_map2moved_object = tf_map2object_origin * tf_object_origin2moved_object; - tf2::toMsg(tf_map2moved_object, output_moved_object_pose.pose); + } - // pointcloud - pcl::PointCloud::Ptr pointcloud_ptr(new pcl::PointCloud); - createObjectPointcloud( - 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); - v_pointcloud.push_back(pointcloud_ptr); + if (selected_indices.empty()) { + pcl::PointCloud::Ptr merged_pointcloud_ptr(new pcl::PointCloud); + pcl::toROSMsg(*merged_pointcloud_ptr, output_pointcloud_msg); + } else { + std::vector obj_infos; + for (const auto selected_idx : selected_indices) { + const auto obj_info = ObjectInfo(objects_.at(selected_idx), current_time); + tf2::toMsg(obj_info.tf_map2moved_object, output_moved_object_pose.pose); + obj_infos.push_back(obj_info); + } - // dynamic object - std::normal_distribution<> x_random(0.0, std_dev_x); - std::normal_distribution<> y_random(0.0, std_dev_y); - std::normal_distribution<> yaw_random(0.0, std_dev_yaw); - tf2::Quaternion noised_quat; - noised_quat.setRPY(0, 0, yaw_random(random_generator_)); - tf2::Transform tf_moved_object2noised_moved_object( - noised_quat, tf2::Vector3(x_random(random_generator_), y_random(random_generator_), 0.0)); - tf2::Transform tf_base_link2noised_moved_object; - tf_base_link2noised_moved_object = - tf_base_link2map * tf_map2moved_object * tf_moved_object2noised_moved_object; - tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object; - feature_object.object.classification.push_back(objects_.at(i).classification); - feature_object.object.kinematics.pose_with_covariance = - objects_.at(i).initial_state.pose_covariance; - feature_object.object.kinematics.twist_with_covariance = - objects_.at(i).initial_state.twist_covariance; - feature_object.object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - feature_object.object.kinematics.has_twist = false; - tf2::toMsg( - tf_base_link2noised_moved_object, feature_object.object.kinematics.pose_with_covariance.pose); - feature_object.object.shape = objects_.at(i).shape; - pcl::toROSMsg(*pointcloud_ptr, feature_object.feature.cluster); - output_dynamic_object_msg.feature_objects.push_back(feature_object); + pcl::PointCloud::Ptr merged_pointcloud_ptr(new pcl::PointCloud); + const auto pointclouds = pointcloud_creator_->create_pointclouds( + obj_infos, tf_base_link2map, random_generator_, merged_pointcloud_ptr); + pcl::toROSMsg(*merged_pointcloud_ptr, output_pointcloud_msg); - // check delete idx - tf2::Transform tf_base_link2moved_object; - tf_base_link2moved_object = tf_base_link2map * tf_map2moved_object; - double dist = std::sqrt( - tf_base_link2moved_object.getOrigin().x() * tf_base_link2moved_object.getOrigin().x() + - tf_base_link2moved_object.getOrigin().y() * tf_base_link2moved_object.getOrigin().y()); - if (visible_range_ < dist) { - delete_idxs.push_back(i); - } - } - // delete - for (int delete_idx = delete_idxs.size() - 1; 0 <= delete_idx; --delete_idx) { - objects_.erase(objects_.begin() + delete_idxs.at(delete_idx)); - } + std::vector delete_idxs; + for (size_t i = 0; i < selected_indices.size(); ++i) { + const auto pointcloud = pointclouds[i]; + const size_t selected_idx = selected_indices[i]; + const auto & object = objects_.at(selected_idx); + const auto object_info = ObjectInfo(object, current_time); + // dynamic object + std::normal_distribution<> x_random(0.0, object_info.std_dev_x); + std::normal_distribution<> y_random(0.0, object_info.std_dev_y); + std::normal_distribution<> yaw_random(0.0, object_info.std_dev_yaw); + tf2::Quaternion noised_quat; + noised_quat.setRPY(0, 0, yaw_random(random_generator_)); + tf2::Transform tf_moved_object2noised_moved_object( + noised_quat, tf2::Vector3(x_random(random_generator_), y_random(random_generator_), 0.0)); + tf2::Transform tf_base_link2noised_moved_object; + tf_base_link2noised_moved_object = + tf_base_link2map * object_info.tf_map2moved_object * tf_moved_object2noised_moved_object; + tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object; + feature_object.object.classification.push_back(object.classification); + feature_object.object.kinematics.pose_with_covariance = object.initial_state.pose_covariance; + feature_object.object.kinematics.twist_with_covariance = + object.initial_state.twist_covariance; + feature_object.object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + feature_object.object.kinematics.has_twist = false; + tf2::toMsg( + tf_base_link2noised_moved_object, + feature_object.object.kinematics.pose_with_covariance.pose); + feature_object.object.shape = object.shape; + pcl::toROSMsg(*pointcloud, feature_object.feature.cluster); + output_dynamic_object_msg.feature_objects.push_back(feature_object); - // merge all pointcloud - pcl::PointCloud::Ptr merged_pointcloud_ptr(new pcl::PointCloud); - for (size_t i = 0; i < v_pointcloud.size(); ++i) { - for (size_t j = 0; j < v_pointcloud.at(i)->size(); ++j) { - merged_pointcloud_ptr->push_back(v_pointcloud.at(i)->at(j)); + // check delete idx + tf2::Transform tf_base_link2moved_object; + tf_base_link2moved_object = tf_base_link2map * object_info.tf_map2moved_object; + double dist = std::sqrt( + tf_base_link2moved_object.getOrigin().x() * tf_base_link2moved_object.getOrigin().x() + + tf_base_link2moved_object.getOrigin().y() * tf_base_link2moved_object.getOrigin().y()); + if (visible_range_ < dist) { + delete_idxs.push_back(selected_idx); + } } - } - // no ground - pcl::toROSMsg(*merged_pointcloud_ptr, output_pointcloud_msg); - // ray tracing - if (enable_ray_tracing_) { - pcl::PointCloud::Ptr ray_traced_merged_pointcloud_ptr( - new pcl::PointCloud); - pcl::VoxelGridOcclusionEstimation ray_tracing_filter; - ray_tracing_filter.setInputCloud(merged_pointcloud_ptr); - ray_tracing_filter.setLeafSize(0.25, 0.25, 0.25); - ray_tracing_filter.initializeVoxelGrid(); - for (size_t i = 0; i < v_pointcloud.size(); ++i) { - pcl::PointCloud::Ptr ray_traced_pointcloud_ptr( - new pcl::PointCloud); - 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; + // delete + for (int delete_idx = delete_idxs.size() - 1; 0 <= delete_idx; --delete_idx) { + objects_.erase(objects_.begin() + delete_idxs.at(delete_idx)); } - pcl::toROSMsg(*ray_traced_merged_pointcloud_ptr, output_pointcloud_msg); } // create output header @@ -223,116 +215,6 @@ void DummyPerceptionPublisherNode::timerCallback() } } -void DummyPerceptionPublisherNode::createObjectPointcloud( - 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) -{ - 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); - auto getBaseLinkTo2DPoint = [tf_base_link2moved_object](double x, double y) -> pcl::PointXYZ { - tf2::Transform tf_moved_object2point; - tf2::Transform tf_base_link2point; - geometry_msgs::msg::Transform ros_moved_object2point; - ros_moved_object2point.translation.x = x; - ros_moved_object2point.translation.y = y; - ros_moved_object2point.translation.z = 0.0; - ros_moved_object2point.rotation.x = 0; - ros_moved_object2point.rotation.y = 0; - ros_moved_object2point.rotation.z = 0; - ros_moved_object2point.rotation.w = 1; - tf2::fromMsg(ros_moved_object2point, tf_moved_object2point); - tf_base_link2point = tf_base_link2moved_object * tf_moved_object2point; - pcl::PointXYZ point; - point.x = tf_base_link2point.getOrigin().x(); - point.y = tf_base_link2point.getOrigin().y(); - point.z = tf_base_link2point.getOrigin().z(); - return point; - }; - const double epsilon = 0.001; - const double step = 0.05; - const double vertical_theta_step = (1.0 / 180.0) * M_PI; - const double vertical_min_theta = (-15.0 / 180.0) * M_PI; - const double vertical_max_theta = (15.0 / 180.0) * M_PI; - const double horizontal_theta_step = (0.1 / 180.0) * M_PI; - 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(); - 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) { - horizontal_candidate_pointcloud.push_back(getBaseLinkTo2DPoint(x, y)); - } - } - { - const double y = 1.0 * (width / 2.0); - for (double x = -1.0 * (length / 2.0); x <= ((length / 2.0) + epsilon); x += step) { - horizontal_candidate_pointcloud.push_back(getBaseLinkTo2DPoint(x, y)); - } - } - { - const double x = -1.0 * (length / 2.0); - for (double y = -1.0 * (width / 2.0); y <= ((width / 2.0) + epsilon); y += step) { - horizontal_candidate_pointcloud.push_back(getBaseLinkTo2DPoint(x, y)); - } - } - { - const double x = 1.0 * (length / 2.0); - for (double y = -1.0 * (width / 2.0); y <= ((width / 2.0) + epsilon); y += step) { - horizontal_candidate_pointcloud.push_back(getBaseLinkTo2DPoint(x, y)); - } - } - // 2D ray tracing - size_t ranges_size = - std::ceil((horizontal_max_theta - horizontal_min_theta) / horizontal_theta_step); - std::vector horizontal_ray_traced_2d_pointcloud; - horizontal_ray_traced_2d_pointcloud.assign(ranges_size, std::numeric_limits::infinity()); - const int no_data = -1; - std::vector horizontal_ray_traced_pointcloud_indices; - horizontal_ray_traced_pointcloud_indices.assign(ranges_size, no_data); - for (size_t i = 0; i < horizontal_candidate_pointcloud.points.size(); ++i) { - double angle = - std::atan2(horizontal_candidate_pointcloud.at(i).y, horizontal_candidate_pointcloud.at(i).x); - double range = - std::hypot(horizontal_candidate_pointcloud.at(i).y, horizontal_candidate_pointcloud.at(i).x); - if (angle < horizontal_min_theta || angle > horizontal_max_theta) { - continue; - } - int index = (angle - horizontal_min_theta) / horizontal_theta_step; - if (range < horizontal_ray_traced_2d_pointcloud[index]) { - horizontal_ray_traced_2d_pointcloud[index] = range; - 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 - horizontal_pointcloud.push_back(horizontal_candidate_pointcloud.at(pointcloud_index)); - const double distance = std::hypot( - horizontal_candidate_pointcloud.at(pointcloud_index).x, - horizontal_candidate_pointcloud.at(pointcloud_index).y); - for (double vertical_theta = vertical_min_theta; - vertical_theta <= vertical_max_theta + epsilon; vertical_theta += vertical_theta_step) { - const double z = distance * std::tan(vertical_theta); - if (min_z <= z && z <= max_z + epsilon) { - pcl::PointXYZ point; - point.x = - 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); - } - } - } - } -} - void DummyPerceptionPublisherNode::objectCallback( const dummy_perception_publisher::msg::Object::ConstSharedPtr msg) { diff --git a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp new file mode 100644 index 0000000000000..27105829510bf --- /dev/null +++ b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp @@ -0,0 +1,236 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "dummy_perception_publisher/node.hpp" +#include "dummy_perception_publisher/signed_distance_function.hpp" + +#include + +#include +#include +#include + +#include +#include +#include + +pcl::PointXYZ getPointWrtBaseLink( + const tf2::Transform & tf_base_link2moved_object, double x, double y, double z) +{ + const auto p_wrt_base = tf_base_link2moved_object(tf2::Vector3(x, y, z)); + return pcl::PointXYZ(p_wrt_base.x(), p_wrt_base.y(), p_wrt_base.z()); +}; + +void ObjectCentricPointCloudCreator::create_object_pointcloud( + const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud) const +{ + 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; + const double vertical_min_theta = (-15.0 / 180.0) * M_PI; + const double vertical_max_theta = (15.0 / 180.0) * M_PI; + const double horizontal_theta_step = (0.1 / 180.0) * M_PI; + const double horizontal_min_theta = (-180.0 / 180.0) * M_PI; + const double horizontal_max_theta = (180.0 / 180.0) * M_PI; + + 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 * (obj_info.width / 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( + getPointWrtBaseLink(tf_base_link2moved_object, x, y, 0.0)); + } + } + { + const double y = 1.0 * (obj_info.width / 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( + getPointWrtBaseLink(tf_base_link2moved_object, x, y, 0.0)); + } + } + { + const double x = -1.0 * (obj_info.length / 2.0); + for (double y = -1.0 * (obj_info.width / 2.0); y <= ((obj_info.width / 2.0) + epsilon); + y += step) { + horizontal_candidate_pointcloud.push_back( + getPointWrtBaseLink(tf_base_link2moved_object, x, y, 0.0)); + } + } + { + const double x = 1.0 * (obj_info.length / 2.0); + for (double y = -1.0 * (obj_info.width / 2.0); y <= ((obj_info.width / 2.0) + epsilon); + y += step) { + horizontal_candidate_pointcloud.push_back( + getPointWrtBaseLink(tf_base_link2moved_object, x, y, 0.0)); + } + } + // 2D ray tracing + size_t ranges_size = + std::ceil((horizontal_max_theta - horizontal_min_theta) / horizontal_theta_step); + std::vector horizontal_ray_traced_2d_pointcloud; + horizontal_ray_traced_2d_pointcloud.assign(ranges_size, std::numeric_limits::infinity()); + const int no_data = -1; + std::vector horizontal_ray_traced_pointcloud_indices; + horizontal_ray_traced_pointcloud_indices.assign(ranges_size, no_data); + for (size_t i = 0; i < horizontal_candidate_pointcloud.points.size(); ++i) { + double angle = + std::atan2(horizontal_candidate_pointcloud.at(i).y, horizontal_candidate_pointcloud.at(i).x); + double range = + std::hypot(horizontal_candidate_pointcloud.at(i).y, horizontal_candidate_pointcloud.at(i).x); + if (angle < horizontal_min_theta || angle > horizontal_max_theta) { + continue; + } + int index = (angle - horizontal_min_theta) / horizontal_theta_step; + if (range < horizontal_ray_traced_2d_pointcloud[index]) { + horizontal_ray_traced_2d_pointcloud[index] = range; + 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 + horizontal_pointcloud.push_back(horizontal_candidate_pointcloud.at(pointcloud_index)); + const double distance = std::hypot( + horizontal_candidate_pointcloud.at(pointcloud_index).x, + horizontal_candidate_pointcloud.at(pointcloud_index).y); + for (double vertical_theta = vertical_min_theta; + vertical_theta <= vertical_max_theta + epsilon; vertical_theta += vertical_theta_step) { + const double z = distance * std::tan(vertical_theta); + if (min_z <= z && z <= max_z + epsilon) { + pcl::PointXYZ point; + point.x = + 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->push_back(point); + } + } + } + } +} + +std::vector::Ptr> ObjectCentricPointCloudCreator::create_pointclouds( + const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, pcl::PointCloud::Ptr & merged_pointcloud) const +{ + std::vector::Ptr> pointclouds_tmp; + pcl::PointCloud::Ptr merged_pointcloud_tmp(new pcl::PointCloud); + + for (const auto & obj_info : obj_infos) { + pcl::PointCloud::Ptr pointcloud_shared_ptr(new pcl::PointCloud); + this->create_object_pointcloud( + obj_info, tf_base_link2map, random_generator, pointcloud_shared_ptr); + pointclouds_tmp.push_back(pointcloud_shared_ptr); + } + + for (const auto & cloud : pointclouds_tmp) { + for (const auto & pt : *cloud) { + merged_pointcloud_tmp->push_back(pt); + } + } + + if (!enable_ray_tracing_) { + merged_pointcloud = merged_pointcloud_tmp; + return pointclouds_tmp; + } + + 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(); + std::vector::Ptr> pointclouds; + 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; + return pointclouds; +} + +std::vector::Ptr> EgoCentricPointCloudCreator::create_pointclouds( + const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, pcl::PointCloud::Ptr & merged_pointcloud) const +{ + std::vector> sdf_ptrs; + for (const auto & obj_info : obj_infos) { + const auto sdf_ptr = std::make_shared( + obj_info.length, obj_info.width, tf_base_link2map * obj_info.tf_map2moved_object); + sdf_ptrs.push_back(sdf_ptr); + } + const auto composite_sdf = signed_distance_function::CompisiteSDF(sdf_ptrs); + + std::vector::Ptr> pointclouds(obj_infos.size()); + for (size_t i = 0; i < obj_infos.size(); ++i) { + pointclouds.at(i) = (pcl::PointCloud::Ptr(new pcl::PointCloud)); + } + + const double horizontal_theta_step = 0.25 * M_PI / 180.0; + double angle = 0.0; + const auto n_scan = static_cast(std::floor(2 * M_PI / horizontal_theta_step)); + for (size_t i = 0; i < n_scan; ++i) { + angle += horizontal_theta_step; + const auto dist = composite_sdf.getSphereTracingDist(0.0, 0.0, angle); + + if (std::isfinite(dist)) { + const auto x_hit = dist * cos(angle); + const auto y_hit = dist * sin(angle); + const auto idx_hit = composite_sdf.nearest_sdf_index(x_hit, y_hit); + + std::normal_distribution<> x_random(0.0, obj_infos.at(idx_hit).std_dev_x); + std::normal_distribution<> y_random(0.0, obj_infos.at(idx_hit).std_dev_y); + std::normal_distribution<> z_random(0.0, obj_infos.at(idx_hit).std_dev_z); + pointclouds.at(idx_hit)->push_back(pcl::PointXYZ( + x_hit + x_random(random_generator), y_hit + y_random(random_generator), + z_random(random_generator))); + } + } + + for (const auto & cloud : pointclouds) { + for (const auto & pt : *cloud) { + merged_pointcloud->push_back(pt); + } + } + return pointclouds; +} diff --git a/simulator/dummy_perception_publisher/src/signed_distance_function.cpp b/simulator/dummy_perception_publisher/src/signed_distance_function.cpp new file mode 100644 index 0000000000000..f44b1f2b73855 --- /dev/null +++ b/simulator/dummy_perception_publisher/src/signed_distance_function.cpp @@ -0,0 +1,84 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "dummy_perception_publisher/signed_distance_function.hpp" + +#include + +#include +#include + +namespace signed_distance_function +{ + +double AbstractSignedDistanceFunction::getSphereTracingDist( + double x_start, double y_start, double angle, double eps) const +{ + // https://computergraphics.stackexchange.com/questions/161/what-is-ray-marching-is-sphere-tracing-the-same-thing/163 + tf2::Vector3 direction(cos(angle), sin(angle), 0.0); + + const size_t max_iter = 40; + + const auto pos_start = tf2::Vector3(x_start, y_start, 0.0); + + auto ray_tip = pos_start; + for (size_t itr = 0; itr < max_iter; ++itr) { + const auto dist = this->operator()(ray_tip.getX(), ray_tip.getY()); + ray_tip = ray_tip + dist * direction; + bool almost_on_surface = std::abs(dist) < eps; + if (almost_on_surface) { + return tf2::tf2Distance(ray_tip, pos_start); + } + } + // ray did not hit the surface. + return std::numeric_limits::infinity(); +} + +double BoxSDF::operator()(double x, double y) const +{ + const auto vec_global = tf2::Vector3(x, y, 0.0); + const auto vec_local = tf_global_to_local_.inverse()(vec_global); + + // As for signed distance field for a box, please refere: + // https://www.iquilezles.org/www/articles/distfunctions/distfunctions.htm + const auto sd_val_x = std::abs(vec_local.getX()) - 0.5 * length_; + const auto sd_val_y = std::abs(vec_local.getY()) - 0.5 * width_; + 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); + const auto negative_dist = std::min(std::max(sd_val_x, sd_val_y), 0.0); + return positive_dist + negative_dist; +} + +double CompisiteSDF::operator()(double x, double y) const +{ + const size_t nearest_idx = nearest_sdf_index(x, y); + return sdf_ptrs_.at(nearest_idx)->operator()(x, y); +} + +size_t CompisiteSDF::nearest_sdf_index(double x, double y) const +{ + double min_value = std::numeric_limits::infinity(); + size_t idx_min; + for (size_t i = 0; i < sdf_ptrs_.size(); ++i) { + const auto value = sdf_ptrs_.at(i)->operator()(x, y); + if (value < min_value) { + min_value = value; + idx_min = i; + } + } + return idx_min; +} + +} // namespace signed_distance_function diff --git a/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp b/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp new file mode 100644 index 0000000000000..3758943d05a9d --- /dev/null +++ b/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp @@ -0,0 +1,84 @@ +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "dummy_perception_publisher/signed_distance_function.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +namespace sdf = signed_distance_function; + +TEST(SignedDistanceFunctionTest, BoxSDF) +{ + const double eps = 1e-5; + + { + // test with identity transform + const auto q = tf2::Quaternion(tf2::Vector3(0, 0, 1.0), 0.0); + const auto tf_global2local = tf2::Transform(q); + const auto func = sdf::BoxSDF(1., 2., tf_global2local); + ASSERT_NEAR(func(0.0, 0.0), -0.5, eps); + ASSERT_NEAR(func(0.0, 1.0), 0.0, eps); + ASSERT_NEAR(func(0.0, 1.5), 0.5, eps); + ASSERT_NEAR(func(0.5, 0.0), 0.0, eps); + ASSERT_NEAR(func(1.0, 0.0), 0.5, eps); + ASSERT_NEAR(func(1.0, 2.0), sqrt(0.5 * 0.5 + 1.0 * 1.0), eps); + + ASSERT_NEAR(func.getSphereTracingDist(2.0, 0.0, M_PI * -1.0), 1.5, eps); + ASSERT_NEAR(func.getSphereTracingDist(1.0, 1.5, M_PI * 1.25), sqrt(2.0) * 0.5, eps); + ASSERT_TRUE(std::isinf(func.getSphereTracingDist(2.0, 0.0, 0.0))); + } + + { + // test with rotation (90 deg) and translation + const auto q = tf2::Quaternion(tf2::Vector3(0, 0, 1.0), M_PI * 0.5); + const auto tf_global2local = tf2::Transform(q, tf2::Vector3(1.0, 1.0, 0.0)); + const auto func = sdf::BoxSDF(1., 2., tf_global2local); + ASSERT_NEAR(func(1.0, 1.0), -0.5, eps); + ASSERT_NEAR(func(0.0, 0.0), 0.5, eps); + + ASSERT_NEAR(func.getSphereTracingDist(0.0, 0.0, M_PI * 0.25, eps), sqrt(2.0) * 0.5, eps); + } +} + +TEST(SignedDistanceFunctionTest, CompositeSDF) +{ + const double eps = 1e-5; + const auto q_identity = tf2::Quaternion(tf2::Vector3(0, 0, 1.0), 0.0); + const auto f1 = + std::make_shared(1., 1., tf2::Transform(q_identity, tf2::Vector3(0, 0, 0))); + const auto f2 = + std::make_shared(1., 1., tf2::Transform(q_identity, tf2::Vector3(0, 2.0, 0))); + const auto func = + sdf::CompisiteSDF(std::vector>{f1, f2}); + ASSERT_NEAR(func(0.0, 0.9), 0.4, eps); + ASSERT_NEAR(func(0.0, 1.1), 0.4, eps); + ASSERT_NEAR(func(0.0, 0.1), -0.4, eps); + ASSERT_NEAR(func(0.0, 1.6), -0.1, eps); + + ASSERT_NEAR(func.getSphereTracingDist(0.0, 1.0, M_PI * 0.5, eps), 0.5, eps); + ASSERT_NEAR(func.getSphereTracingDist(0.0, 1.0, M_PI * -0.5, eps), 0.5, eps); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}