From 05d091d952f03a44733ae21eb0d3a5b3236e4ca6 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Wed, 23 Aug 2023 12:57:05 +0900 Subject: [PATCH] fix: center point class (#4701) * feat(lidar_centerpoint): add build only option for tensorrt engine (#2677) * feat(lidar_centerpoint): add build only option for tensorrt engine Signed-off-by: yukke42 * fix typo Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * style(pre-commit): autofix * chore: add description Signed-off-by: yukke42 * chore: shutdown node Signed-off-by: yukke42 * feat: use tensorrt_commom Signed-off-by: yukke42 * fix: resolve the crash when shutting down the node Signed-off-by: yukke42 * chore: fix typo Co-authored-by: Kenzo Lobos Tsunekawa * style(pre-commit): autofix --------- Signed-off-by: yukke42 Signed-off-by: yukke42 Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenzo Lobos Tsunekawa * fix(lidar_centerpoint): fix class (#3934) * fix(lidar_centerpoint): fix class Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * feat(lidar_centerpoint): fix range in config for centerpoint Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * feat(lidar_centerpoint): fix the ranges and voxel size Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> --------- Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * Revert "fix: use centerpoint_tiny to centerpoint" This reverts commit efcf72623ff9f7f35dff2b1a9bef8ab4884a9446. --------- Signed-off-by: yukke42 Signed-off-by: yukke42 Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Co-authored-by: Yusuke Muramatsu Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenzo Lobos Tsunekawa Co-authored-by: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> --- .../launch/perception.launch.xml | 2 +- perception/lidar_centerpoint/README.md | 1 + .../config/centerpoint.param.yaml | 8 +-- .../config/centerpoint_tiny.param.yaml | 4 +- .../lidar_centerpoint/centerpoint_trt.hpp | 1 - .../lidar_centerpoint/network/network_trt.hpp | 4 +- .../network/tensorrt_wrapper.hpp | 44 +++------------ .../launch/lidar_centerpoint.launch.xml | 2 + .../lidar_centerpoint/lib/centerpoint_trt.cpp | 4 +- .../lib/network/network_trt.cpp | 5 +- .../lib/network/tensorrt_wrapper.cpp | 55 ++++++++++--------- perception/lidar_centerpoint/package.xml | 1 + perception/lidar_centerpoint/src/node.cpp | 5 ++ 13 files changed, 59 insertions(+), 77 deletions(-) diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index e62d45c5f32f3..ffc6f908983bb 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -9,7 +9,7 @@ - + diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index baaea06e59346..f03e5bb777299 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -43,6 +43,7 @@ We trained the models using . | `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 index 42a15067d55eb..0b29a87965b42 100644 --- a/perception/lidar_centerpoint/config/centerpoint.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint.param.yaml @@ -1,10 +1,10 @@ /**: 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 @@ -12,4 +12,4 @@ 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 index 92db4cd0b853e..8252fde8273d8 100644 --- 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 index 25891958f9194..fee17f1c0156a 100644 --- 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 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 index c2c2dbc179361..639aa0ba8bad3 100644 --- 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 & out_channel_sizes, const CenterPointConfig & config, - const bool verbose); + HeadTRT(const std::vector & 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 index 2cd05400bc9af..7f55ab6f5e414 100644 --- 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 +#include #include @@ -25,45 +26,18 @@ namespace centerpoint { -struct Deleter -{ - template - void operator()(T * obj) const - { - if (obj) { - delete obj; - } - } -}; - -template -using unique_ptr = std::unique_ptr; - -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 context_ = nullptr; + tensorrt_common::TrtUniquePtr context_{nullptr}; protected: virtual bool setProfile( @@ -71,7 +45,7 @@ class TensorRTWrapper 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 runtime_ = nullptr; - unique_ptr plan_ = nullptr; - unique_ptr engine_ = nullptr; + tensorrt_common::TrtUniquePtr runtime_{nullptr}; + tensorrt_common::TrtUniquePtr plan_{nullptr}; + tensorrt_common::TrtUniquePtr engine_{nullptr}; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index 522b488429c62..d552cb702b980 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -8,6 +8,7 @@ + @@ -21,6 +22,7 @@ + diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index a489be9e86538..9b01ea8f53675 100644 --- 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(config_); // encoder - encoder_trt_ptr_ = std::make_unique(config_, verbose_); + encoder_trt_ptr_ = std::make_unique(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 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(out_channel_sizes, config_, verbose_); + head_trt_ptr_ = std::make_unique(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 index ffc4ac4a2eef0..88319ff51fe35 100644 --- 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 & out_channel_sizes, const CenterPointConfig & config, - const bool verbose) -: TensorRTWrapper(config, verbose), out_channel_sizes_(out_channel_sizes) + const std::vector & 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 index a175079ab0348..079c41d06c6e0 100644 --- 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::createInferRuntime(logger_)); + runtime_ = + tensorrt_common::TrtUniquePtr(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(engine_->createExecutionContext()); + context_ = + tensorrt_common::TrtUniquePtr(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::createInferBuilder(logger_)); + auto builder = + tensorrt_common::TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); if (!builder) { std::cout << "Fail to create builder" << std::endl; return false; } - auto config = unique_ptr(builder->createBuilderConfig()); + auto config = + tensorrt_common::TrtUniquePtr(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(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - auto network = unique_ptr(builder->createNetworkV2(flag)); + auto network = + tensorrt_common::TrtUniquePtr(builder->createNetworkV2(flag)); if (!network) { std::cout << "Fail to create network" << std::endl; return false; } - auto parser = unique_ptr(nvonnxparser::createParser(*network, logger_)); + auto parser = tensorrt_common::TrtUniquePtr( + nvonnxparser::createParser(*network, logger_)); parser->parseFromFile(onnx_path.c_str(), static_cast(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(builder->buildSerializedNetwork(*network, *config)); + plan_ = tensorrt_common::TrtUniquePtr( + builder->buildSerializedNetwork(*network, *config)); if (!plan_) { std::cout << "Fail to create serialized network" << std::endl; return false; } - engine_ = unique_ptr( + engine_ = tensorrt_common::TrtUniquePtr( 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 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(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(runtime_->deserializeCudaEngine( + reinterpret_cast(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 index 970f69921c6e2..879013189b246 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -19,6 +19,7 @@ python3-open3d rclcpp rclcpp_components + tensorrt_common tf2_eigen tf2_geometry_msgs tier4_autoware_utils diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index da4000b450858..39683755bcaf0 100644 --- 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(