fix: update the class

Signed-off-by: tzhong518 <sworgun@gmail.com>

perception/image_projection_based_fusion/CMakeLists.txt

@@ -66,88 +66,59 @@ else()
   set(CUDNN_AVAIL OFF)
 endif()
 
-if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
-
-  # find dependencies
-  find_package(ament_lint_auto REQUIRED)
-  find_package(ament_cmake_auto REQUIRED)
-  find_package(OpenCV REQUIRED)
-  find_package(Eigen3 REQUIRED)
-  find_package(PCL REQUIRED)
-  ament_auto_find_build_dependencies()
-
-  include_directories(
-    include
-    ${OpenCV_INCLUDE_DIRS}
-    ${EIGEN3_INCLUDE_DIR}
-    ${PCL_INCLUDE_DIRS}
-    ${CUDA_INCLUDE_DIRS}
-  )
-
-  ### node ###
-  ament_auto_add_library(${PROJECT_NAME} SHARED
-    src/fusion_node.cpp
-    src/debugger.cpp
-    src/utils/geometry.cpp
-    src/utils/utils.cpp
-    src/roi_cluster_fusion/node.cpp
-    src/roi_detected_object_fusion/node.cpp
-    src/pointpainting_fusion/node.cpp
-    src/pointpainting_fusion/pointpainting_trt.cpp
-    src/pointpainting_fusion/voxel_generator.cpp
-  )
-
-  cuda_add_library(pointpainting_cuda_lib SHARED
-    src/pointpainting_fusion/preprocess_kernel.cu
-  )
-
-  target_link_libraries(${PROJECT_NAME}
-    ${OpenCV_LIBRARIES}
-    ${EIGEN3_LIBRARIES}
-    ${PCL_LIBRARIES}
-    ${NVINFER}
-    ${CUDA_LIBRARIES}
-    ${CUBLAS_LIBRARIES}
-    ${CUDA_curand_LIBRARY}
-    ${CUDNN_LIBRARY}
-    pointpainting_cuda_lib
-  )
+find_package(OpenCV REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+include_directories(
+  include
+  ${OpenCV_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+)
 
-  rclcpp_components_register_node(${PROJECT_NAME}
-    PLUGIN "image_projection_based_fusion::RoiClusterFusionNode"
-    EXECUTABLE roi_cluster_fusion_node
-  )
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/fusion_node.cpp
+  src/debugger.cpp
+  src/utils/geometry.cpp
+  src/utils/utils.cpp
+  src/roi_cluster_fusion/node.cpp
+  src/roi_detected_object_fusion/node.cpp
+  src/pointpainting_fusion/node.cpp
+  src/pointpainting_fusion/pointpainting_trt.cpp
+  src/pointpainting_fusion/voxel_generator.cpp
+)
 
-  rclcpp_components_register_node(${PROJECT_NAME}
-    PLUGIN "image_projection_based_fusion::RoiDetectedObjectFusionNode"
-    EXECUTABLE roi_detected_object_fusion_node
-  )
+cuda_add_library(pointpainting_cuda_lib SHARED
+  src/pointpainting_fusion/preprocess_kernel.cu
+)
 
-  rclcpp_components_register_node(${PROJECT_NAME}
-    PLUGIN "image_projection_based_fusion::PointpaintingFusionNode"
-    EXECUTABLE pointpainting_fusion_node
-  )
+target_link_libraries(${PROJECT_NAME}
+  ${OpenCV_LIBRARIES}
+  ${EIGEN3_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${NVINFER}
+  ${CUDA_LIBRARIES}
+  ${CUBLAS_LIBRARIES}
+  ${CUDA_curand_LIBRARY}
+  ${CUDNN_LIBRARY}
+  pointpainting_cuda_lib
+)
 
-  if(BUILD_TESTING)
-    find_package(ament_lint_auto REQUIRED)
-  endif()
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "image_projection_based_fusion::RoiClusterFusionNode"
+  EXECUTABLE roi_cluster_fusion_node
+)
 
-  ament_auto_package(INSTALL_TO_SHARE
-      launch
-      data
-      config
-  )
-else()
-  find_package(ament_cmake_auto REQUIRED)
-  ament_auto_find_build_dependencies()
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "image_projection_based_fusion::RoiDetectedObjectFusionNode"
+  EXECUTABLE roi_detected_object_fusion_node
+)
 
-  if(BUILD_TESTING)
-    find_package(ament_lint_auto REQUIRED)
-    ament_lint_auto_find_test_dependencies()
-  endif()
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "image_projection_based_fusion::PointpaintingFusionNode"
+  EXECUTABLE pointpainting_fusion_node
+)
 
-  ament_auto_package(
-    INSTALL_TO_SHARE
-      launch
-  )
-endif()
+ament_auto_package(INSTALL_TO_SHARE
+    launch
+)

perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp

@@ -30,7 +30,7 @@ namespace image_projection_based_fusion
 {
 using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
-class PointpaintingFusionNode : public FusionNode<sensor_msgs::msg::PointCloud2>
+class PointpaintingFusionNode : public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects>
 {
 public:
   explicit PointpaintingFusionNode(const rclcpp::NodeOptions & options);
@@ -39,21 +39,23 @@ class PointpaintingFusionNode : public FusionNode<sensor_msgs::msg::PointCloud2>
   void preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
 
   void fuseOnSingleImage(
-    const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const int image_id,
+    const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
     const DetectedObjectsWithFeature & input_roi_msg,
     const sensor_msgs::msg::CameraInfo & camera_info,
     sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
 
   void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
 
-  rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr obj_pub_ptr_;
+  rclcpp::Publisher<DetectedObjects>::SharedPtr obj_pub_ptr_;
 
   float score_threshold_{0.0};
   std::vector<std::string> class_names_;
   bool rename_car_to_truck_and_bus_{false};
   bool has_twist_{false};
 
   std::unique_ptr<centerpoint::PointPaintingTRT> detector_ptr_{nullptr};
+
+  bool out_of_scope(const DetectedObjects & obj);
 };
 
 }  // namespace image_projection_based_fusion

perception/image_projection_based_fusion/package.xml

@@ -27,6 +27,7 @@
   <depend>tf2_sensor_msgs</depend>
   <depend>tier4_perception_msgs</depend>
   <depend>tier4_autoware_utils</depend>
+  <depend>lidar_centerpoint</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -27,7 +27,7 @@ namespace image_projection_based_fusion
 {
 
 PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & options)
-: FusionNode<sensor_msgs::msg::PointCloud2>("pointpainting_fusion", options)
+: FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects>("pointpainting_fusion", options)
 {
   const float score_threshold =
     static_cast<float>(this->declare_parameter<double>("score_threshold", 0.4));
@@ -65,14 +65,14 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt
   centerpoint::CenterPointConfig config(
     class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size,
     downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold);
+
   // create detector
   detector_ptr_ = std::make_unique<centerpoint::PointPaintingTRT>(
     encoder_param, head_param, densification_param, config);
 
   // sub and pub
   sub_.subscribe(this, "~/input/pointcloud", rmw_qos_profile_sensor_data);
-  obj_pub_ptr_ = this->create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
-    "~/output/objects", rclcpp::QoS{1});
+  obj_pub_ptr_ = this->create_publisher<DetectedObjects>("~/output/objects", rclcpp::QoS{1});
 }
 
 void PointpaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg)
@@ -120,8 +120,8 @@ void PointpaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted
 }
 
 void PointpaintingFusionNode::fuseOnSingleImage(
-  const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const int image_id,
-  const DetectedObjectsWithFeature & input_roi_msg,
+  __attribute__((unused)) const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg,
+  const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg,
   const sensor_msgs::msg::CameraInfo & camera_info,
   sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg)
 {
@@ -140,17 +140,6 @@ void PointpaintingFusionNode::fuseOnSingleImage(
     transform_stamped = transform_stamped_optional.value();
   }
 
-  // get transform from pointcloud frame id to camera optical frame id
-  // geometry_msgs::msg::TransformStamped transform_stamped;
-  // try {
-  //   transform_stamped = tf_buffer_.lookupTransform(
-  //     /*target*/ camera_info.header.frame_id,
-  //     /*src*/ painted_pointcloud_msg.header.frame_id, tf2::TimePointZero);
-  // } catch (tf2::TransformException & ex) {
-  //   RCLCPP_WARN(this->get_logger(), "%s", ex.what());
-  //   return;
-  // }
-
   // projection matrix
   Eigen::Matrix4d camera_projection;
   camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2),
@@ -253,6 +242,11 @@ void PointpaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte
   obj_pub_ptr_->publish(output_obj_msg);
 }
 
+bool PointpaintingFusionNode::out_of_scope(__attribute__((unused)) const DetectedObjects & obj)
+{
+  return false;
+}
+
 }  // namespace image_projection_based_fusion
 
 #include <rclcpp_components/register_node_macro.hpp>

perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp

@@ -25,7 +25,6 @@
 #include <vector>
 
 namespace centerpoint
-// image_projection_based_fusion
 {
 PointPaintingTRT::PointPaintingTRT(
   const NetworkParam & encoder_param, const NetworkParam & head_param,