fix: center point class (autowarefoundation#4701)

* feat(lidar_centerpoint): add build only option for tensorrt engine (autowarefoundation#2677) * feat(lidar_centerpoint): add build only option for tensorrt engine Signed-off-by: yukke42 <[email protected]> * fix typo Co-authored-by: Daisuke Nishimatsu <[email protected]> * style(pre-commit): autofix * chore: add description Signed-off-by: yukke42 <[email protected]> * chore: shutdown node Signed-off-by: yukke42 <[email protected]> * feat: use tensorrt_commom Signed-off-by: yukke42 <[email protected]> * fix: resolve the crash when shutting down the node Signed-off-by: yukke42 <[email protected]> * chore: fix typo Co-authored-by: Kenzo Lobos Tsunekawa <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: yukke42 <[email protected]> Signed-off-by: yukke42 <[email protected]> Co-authored-by: Daisuke Nishimatsu <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenzo Lobos Tsunekawa <[email protected]> * fix(lidar_centerpoint): fix class (autowarefoundation#3934) * fix(lidar_centerpoint): fix class Signed-off-by: Shin-kyoto <[email protected]> * feat(lidar_centerpoint): fix range in config for centerpoint Signed-off-by: Shin-kyoto <[email protected]> * feat(lidar_centerpoint): fix the ranges and voxel size Signed-off-by: Shin-kyoto <[email protected]> --------- Signed-off-by: Shin-kyoto <[email protected]> * Revert "fix: use centerpoint_tiny to centerpoint" This reverts commit efcf726. --------- Signed-off-by: yukke42 <[email protected]> Signed-off-by: yukke42 <[email protected]> Signed-off-by: Shin-kyoto <[email protected]> Co-authored-by: Yusuke Muramatsu <[email protected]> Co-authored-by: Daisuke Nishimatsu <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenzo Lobos Tsunekawa <[email protected]> Co-authored-by: Shintaro Tomie <[email protected]>
AutomotiveAIChallenge · Aug 23, 2023 · 05d091d · 05d091d
1 parent 7814305
commit 05d091d
Show file tree

Hide file tree

Showing 13 changed files with 59 additions and 77 deletions.
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -9,7 +9,7 @@
   <arg name="obstacle_segmentation_ground_segmentation_elevation_map_param_path"/>
 
   <!-- centerpoint model configs -->
-  <arg name="centerpoint_model_name" default="centerpoint" description="options: `centerpoint` or `centerpoint_tiny`"/>
+  <arg name="centerpoint_model_name" default="centerpoint_tiny" description="options: `centerpoint` or `centerpoint_tiny`"/>
   <arg name="centerpoint_model_path" default="$(find-pkg-share lidar_centerpoint)/data"/>
   <arg name="centerpoint_model_param_path" default="$(find-pkg-share lidar_centerpoint)/config/$(var centerpoint_model_name).param.yaml"/>
 

diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md
@@ -43,6 +43,7 @@ We trained the models using <https://github.com/open-mmlab/mmdetection3d>.
 | `nms_iou_target_class_names`    | list[string] | -             | target classes for IoU-based Non Maximum Suppression          |
 | `nms_iou_search_distance_2d`    | double       | -             | If two objects are farther than the value, NMS isn't applied. |
 | `nms_iou_threshold`             | double       | -             | IoU threshold for the IoU-based Non Maximum Suppression       |
+| `build_only`                    | bool         | `false`       | shutdown the node after TensorRT engine file is built         |
 
 ## Assumptions / Known limits
 

diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/lidar_centerpoint/config/centerpoint.param.yaml
@@ -1,15 +1,15 @@
 /**:
   ros__parameters:
-    class_names: ["CAR", "PEDESTRIAN", "BICYCLE"]
+    class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"]
     point_feature_size: 4
     max_voxel_size: 40000
-    point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0]
-    voxel_size: [0.32, 0.32, 8.0]
+    point_cloud_range: [-76.8, -76.8, -4.0, 76.8, 76.8, 6.0]
+    voxel_size: [0.32, 0.32, 10.0]
     downsample_factor: 1
     encoder_in_feature_size: 9
     # post-process params
     circle_nms_dist_threshold: 0.5
     iou_nms_target_class_names: ["CAR"]
     iou_nms_search_distance_2d: 10.0
     iou_nms_threshold: 0.1
-    yaw_norm_thresholds: [0.3, 0.0, 0.3]
+    yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0]
diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml
@@ -3,8 +3,8 @@
     class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"]
     point_feature_size: 4
     max_voxel_size: 40000
-    point_cloud_range: [-76.8, -76.8, -2.0, 76.8, 76.8, 4.0]
-    voxel_size: [0.32, 0.32, 6.0]
+    point_cloud_range: [-76.8, -76.8, -4.0, 76.8, 76.8, 6.0]
+    voxel_size: [0.32, 0.32, 10.0]
     downsample_factor: 2
     encoder_in_feature_size: 9
     # post-process params

diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp
@@ -81,7 +81,6 @@ class CenterPointTRT
   std::unique_ptr<PostProcessCUDA> post_proc_ptr_{nullptr};
   cudaStream_t stream_{nullptr};
 
-  bool verbose_{false};
   std::size_t class_size_{0};
   CenterPointConfig config_;
   std::size_t num_voxels_{0};

diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp
@@ -38,9 +38,7 @@ class HeadTRT : public TensorRTWrapper
 public:
   using TensorRTWrapper::TensorRTWrapper;
 
-  HeadTRT(
-    const std::vector<std::size_t> & out_channel_sizes, const CenterPointConfig & config,
-    const bool verbose);
+  HeadTRT(const std::vector<std::size_t> & out_channel_sizes, const CenterPointConfig & config);
 
 protected:
   bool setProfile(

diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp
@@ -16,6 +16,7 @@
 #define LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_
 
 #include <lidar_centerpoint/centerpoint_config.hpp>
+#include <tensorrt_common/tensorrt_common.hpp>
 
 #include <NvInfer.h>
 
@@ -25,53 +26,26 @@
 
 namespace centerpoint
 {
-struct Deleter
-{
-  template <typename T>
-  void operator()(T * obj) const
-  {
-    if (obj) {
-      delete obj;
-    }
-  }
-};
-
-template <typename T>
-using unique_ptr = std::unique_ptr<T, Deleter>;
-
-class Logger : public nvinfer1::ILogger
-{
-public:
-  explicit Logger(bool verbose) : verbose_(verbose) {}
-
-  void log(Severity severity, const char * msg) noexcept override
-  {
-    if (verbose_ || ((severity != Severity::kINFO) && (severity != Severity::kVERBOSE))) {
-      std::cout << msg << std::endl;
-    }
-  }
-
-private:
-  bool verbose_{false};
-};
 
 class TensorRTWrapper
 {
 public:
-  explicit TensorRTWrapper(const CenterPointConfig & config, const bool verbose);
+  explicit TensorRTWrapper(const CenterPointConfig & config);
+
+  ~TensorRTWrapper();
 
   bool init(
     const std::string & onnx_path, const std::string & engine_path, const std::string & precision);
 
-  unique_ptr<nvinfer1::IExecutionContext> context_ = nullptr;
+  tensorrt_common::TrtUniquePtr<nvinfer1::IExecutionContext> context_{nullptr};
 
 protected:
   virtual bool setProfile(
     nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network,
     nvinfer1::IBuilderConfig & config) = 0;
 
   CenterPointConfig config_;
-  Logger logger_;
+  tensorrt_common::Logger logger_;
 
 private:
   bool parseONNX(
@@ -84,9 +58,9 @@ class TensorRTWrapper
 
   bool createContext();
 
-  unique_ptr<nvinfer1::IRuntime> runtime_ = nullptr;
-  unique_ptr<nvinfer1::IHostMemory> plan_ = nullptr;
-  unique_ptr<nvinfer1::ICudaEngine> engine_ = nullptr;
+  tensorrt_common::TrtUniquePtr<nvinfer1::IRuntime> runtime_{nullptr};
+  tensorrt_common::TrtUniquePtr<nvinfer1::IHostMemory> plan_{nullptr};
+  tensorrt_common::TrtUniquePtr<nvinfer1::ICudaEngine> engine_{nullptr};
 };
 
 }  // namespace centerpoint

diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml
@@ -8,6 +8,7 @@
   <arg name="class_remapper_param_path" default="$(find-pkg-share lidar_centerpoint)/config/detection_class_remapper.param.yaml"/>
   <arg name="score_threshold" default="0.35"/>
   <arg name="has_twist" default="false"/>
+  <arg name="build_only" default="false" description="shutdown node after TensorRT engine file is built"/>
 
   <node pkg="lidar_centerpoint" exec="lidar_centerpoint_node" name="lidar_centerpoint" output="screen">
     <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
@@ -21,6 +22,7 @@
     <param name="encoder_engine_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).engine"/>
     <param name="head_onnx_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"/>
     <param name="head_engine_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"/>
+    <param name="build_only" value="$(var build_only)"/>
     <param from="$(var model_param_path)"/>
     <param from="$(var class_remapper_param_path)"/>
   </node>

diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp
@@ -35,7 +35,7 @@ CenterPointTRT::CenterPointTRT(
   post_proc_ptr_ = std::make_unique<PostProcessCUDA>(config_);
 
   // encoder
-  encoder_trt_ptr_ = std::make_unique<VoxelEncoderTRT>(config_, verbose_);
+  encoder_trt_ptr_ = std::make_unique<VoxelEncoderTRT>(config_);
   encoder_trt_ptr_->init(
     encoder_param.onnx_path(), encoder_param.engine_path(), encoder_param.trt_precision());
   encoder_trt_ptr_->context_->setBindingDimensions(
@@ -47,7 +47,7 @@ CenterPointTRT::CenterPointTRT(
   std::vector<std::size_t> out_channel_sizes = {
     config_.class_size_,        config_.head_out_offset_size_, config_.head_out_z_size_,
     config_.head_out_dim_size_, config_.head_out_rot_size_,    config_.head_out_vel_size_};
-  head_trt_ptr_ = std::make_unique<HeadTRT>(out_channel_sizes, config_, verbose_);
+  head_trt_ptr_ = std::make_unique<HeadTRT>(out_channel_sizes, config_);
   head_trt_ptr_->init(head_param.onnx_path(), head_param.engine_path(), head_param.trt_precision());
   head_trt_ptr_->context_->setBindingDimensions(
     0, nvinfer1::Dims4(

diff --git a/perception/lidar_centerpoint/lib/network/network_trt.cpp b/perception/lidar_centerpoint/lib/network/network_trt.cpp
@@ -39,9 +39,8 @@ bool VoxelEncoderTRT::setProfile(
 }
 
 HeadTRT::HeadTRT(
-  const std::vector<std::size_t> & out_channel_sizes, const CenterPointConfig & config,
-  const bool verbose)
-: TensorRTWrapper(config, verbose), out_channel_sizes_(out_channel_sizes)
+  const std::vector<std::size_t> & out_channel_sizes, const CenterPointConfig & config)
+: TensorRTWrapper(config), out_channel_sizes_(out_channel_sizes)
 {
 }
 

diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp
@@ -22,15 +22,21 @@
 
 namespace centerpoint
 {
-TensorRTWrapper::TensorRTWrapper(const CenterPointConfig & config, const bool verbose)
-: config_(config), logger_(Logger(verbose))
+TensorRTWrapper::TensorRTWrapper(const CenterPointConfig & config) : config_(config) {}
+
+TensorRTWrapper::~TensorRTWrapper()
 {
+  context_.reset();
+  runtime_.reset();
+  plan_.reset();
+  engine_.reset();
 }
 
 bool TensorRTWrapper::init(
   const std::string & onnx_path, const std::string & engine_path, const std::string & precision)
 {
-  runtime_ = unique_ptr<nvinfer1::IRuntime>(nvinfer1::createInferRuntime(logger_));
+  runtime_ =
+    tensorrt_common::TrtUniquePtr<nvinfer1::IRuntime>(nvinfer1::createInferRuntime(logger_));
   if (!runtime_) {
     std::cout << "Fail to create runtime" << std::endl;
     return false;
@@ -55,7 +61,8 @@ bool TensorRTWrapper::createContext()
     return false;
   }
 
-  context_ = unique_ptr<nvinfer1::IExecutionContext>(engine_->createExecutionContext());
+  context_ =
+    tensorrt_common::TrtUniquePtr<nvinfer1::IExecutionContext>(engine_->createExecutionContext());
   if (!context_) {
     std::cout << "Fail to create context" << std::endl;
     return false;
@@ -68,13 +75,15 @@ bool TensorRTWrapper::parseONNX(
   const std::string & onnx_path, const std::string & engine_path, const std::string & precision,
   const size_t workspace_size)
 {
-  auto builder = unique_ptr<nvinfer1::IBuilder>(nvinfer1::createInferBuilder(logger_));
+  auto builder =
+    tensorrt_common::TrtUniquePtr<nvinfer1::IBuilder>(nvinfer1::createInferBuilder(logger_));
   if (!builder) {
     std::cout << "Fail to create builder" << std::endl;
     return false;
   }
 
-  auto config = unique_ptr<nvinfer1::IBuilderConfig>(builder->createBuilderConfig());
+  auto config =
+    tensorrt_common::TrtUniquePtr<nvinfer1::IBuilderConfig>(builder->createBuilderConfig());
   if (!config) {
     std::cout << "Fail to create config" << std::endl;
     return false;
@@ -95,13 +104,15 @@ bool TensorRTWrapper::parseONNX(
 
   const auto flag =
     1U << static_cast<uint32_t>(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH);
-  auto network = unique_ptr<nvinfer1::INetworkDefinition>(builder->createNetworkV2(flag));
+  auto network =
+    tensorrt_common::TrtUniquePtr<nvinfer1::INetworkDefinition>(builder->createNetworkV2(flag));
   if (!network) {
     std::cout << "Fail to create network" << std::endl;
     return false;
   }
 
-  auto parser = unique_ptr<nvonnxparser::IParser>(nvonnxparser::createParser(*network, logger_));
+  auto parser = tensorrt_common::TrtUniquePtr<nvonnxparser::IParser>(
+    nvonnxparser::createParser(*network, logger_));
   parser->parseFromFile(onnx_path.c_str(), static_cast<int>(nvinfer1::ILogger::Severity::kERROR));
 
   if (!setProfile(*builder, *network, *config)) {
@@ -111,12 +122,13 @@ bool TensorRTWrapper::parseONNX(
 
   std::cout << "Applying optimizations and building TRT CUDA engine (" << onnx_path << ") ..."
             << std::endl;
-  plan_ = unique_ptr<nvinfer1::IHostMemory>(builder->buildSerializedNetwork(*network, *config));
+  plan_ = tensorrt_common::TrtUniquePtr<nvinfer1::IHostMemory>(
+    builder->buildSerializedNetwork(*network, *config));
   if (!plan_) {
     std::cout << "Fail to create serialized network" << std::endl;
     return false;
   }
-  engine_ = unique_ptr<nvinfer1::ICudaEngine>(
+  engine_ = tensorrt_common::TrtUniquePtr<nvinfer1::ICudaEngine>(
     runtime_->deserializeCudaEngine(plan_->data(), plan_->size()));
   if (!engine_) {
     std::cout << "Fail to create engine" << std::endl;
@@ -136,22 +148,13 @@ bool TensorRTWrapper::saveEngine(const std::string & engine_path)
 
 bool TensorRTWrapper::loadEngine(const std::string & engine_path)
 {
-  std::ifstream file(engine_path, std::ios::in | std::ios::binary);
-  file.seekg(0, std::ifstream::end);
-  const size_t size = file.tellg();
-  file.seekg(0, std::ifstream::beg);
-
-  std::unique_ptr<char[]> buffer{new char[size]};
-  file.read(buffer.get(), size);
-  file.close();
-
-  if (!runtime_) {
-    std::cout << "Fail to load engine: Runtime isn't created" << std::endl;
-    return false;
-  }
-
-  std::cout << "Loading from " << engine_path << std::endl;
-  engine_ = unique_ptr<nvinfer1::ICudaEngine>(runtime_->deserializeCudaEngine(buffer.get(), size));
+  std::ifstream engine_file(engine_path);
+  std::stringstream engine_buffer;
+  engine_buffer << engine_file.rdbuf();
+  std::string engine_str = engine_buffer.str();
+  engine_ = tensorrt_common::TrtUniquePtr<nvinfer1::ICudaEngine>(runtime_->deserializeCudaEngine(
+    reinterpret_cast<const void *>(engine_str.data()), engine_str.size()));
+  std::cout << "Loaded engine from " << engine_path << std::endl;
   return true;
 }
 

diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml
@@ -19,6 +19,7 @@
   <depend>python3-open3d</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tensorrt_common</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tier4_autoware_utils</depend>

diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp
@@ -121,6 +121,11 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
     stop_watch_ptr_->tic("cyclic_time");
     stop_watch_ptr_->tic("processing_time");
   }
+
+  if (this->declare_parameter("build_only", false)) {
+    RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node.");
+    rclcpp::shutdown();
+  }
 }
 
 void LidarCenterPointNode::pointCloudCallback(