feat(dummy_perception_publisher): publish realistic dummy pointcloud …

…using raymarchig (autowarefoundation#527) * Create pointcloud by raycasting from vehicle Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * [after-review] Use vector of ObjectInfo Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * [after-review] Implemented by strategy pattern Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * [after-review] split files Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Use pcl raytracing Tmp Tmp Tmp Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Add signed distance function lib Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Use sdf library Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Remove no longer used functions Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Refactor Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Simplify getPoint Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Raytracing considering inter object relation Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Add random noise Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Default is object centric Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Return if no objects are detected Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Change definition of tf_global_to_local (same as other autoware codes) Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Remove create method Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Reaname: VehicleCentric -> EgoCentric Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Refactor a bit Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Tune parameter Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Fix: Even if selected_idices is zero, pointclouds must be published Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Fix launch file Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Fix typo Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Fix: create merged pointcloud when no idx is selected Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp> * Use ray-maching by default Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>
HiroIshida · May 14, 2022 · 5382913 · 5382913
1 parent 5dbd9f5
commit 5382913
Show file tree

Hide file tree

Showing 8 changed files with 661 additions and 228 deletions.
diff --git 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
@@ -33,9 +33,64 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <memory>
 #include <random>
 #include <vector>
 
+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<pcl::PointCloud<pcl::PointXYZ>::Ptr> create_pointclouds(
+    const std::vector<ObjectInfo> & obj_infos, const tf2::Transform & tf_base_link2map,
+    std::mt19937 & random_generator,
+    pcl::PointCloud<pcl::PointXYZ>::Ptr & merged_pointcloud) const = 0;
+};
+
+class ObjectCentricPointCloudCreator : public PointCloudCreator
+{
+public:
+  explicit ObjectCentricPointCloudCreator(bool enable_ray_tracing)
+  : enable_ray_tracing_(enable_ray_tracing)
+  {
+  }
+
+  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> create_pointclouds(
+    const std::vector<ObjectInfo> & obj_infos, const tf2::Transform & tf_base_link2map,
+    std::mt19937 & random_generator,
+    pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ>::Ptr pointcloud) const;
+
+  bool enable_ray_tracing_;
+};
+
+class EgoCentricPointCloudCreator : public PointCloudCreator
+{
+  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> create_pointclouds(
+    const std::vector<ObjectInfo> & obj_infos, const tf2::Transform & tf_base_link2map,
+    std::mt19937 & random_generator,
+    pcl::PointCloud<pcl::PointXYZ>::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<PointCloudCreator> 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<pcl::PointXYZ>::Ptr & pointcloud_ptr);
   void objectCallback(const dummy_perception_publisher::msg::Object::ConstSharedPtr msg);
 
 public:

diff --git a/...ummy_perception_publisher/include/dummy_perception_publisher/signed_distance_function.hpp b/...ummy_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 <tf2/LinearMath/Transform.h>
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+#include <vector>
+
+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<std::shared_ptr<AbstractSignedDistanceFunction>> 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<std::shared_ptr<AbstractSignedDistanceFunction>> 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
@@ -18,6 +18,7 @@
         <param name="detection_successful_rate" value="0.999"/>
         <param name="enable_ray_tracing" value="false"/>
         <param name="use_object_recognition" value="$(var use_object_recognition)"/>
+        <param name="object_centric_pointcloud" value="false"/>
       </node>
       <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
         <arg name="input/objects" value="/perception/object_recognition/detection/labeled_clusters"/>