From d7d05ba3884689d23bbd041340793fb18d38b517 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 17 Mar 2023 11:38:44 +0900 Subject: [PATCH 1/6] Add a check for the class size between the model and the config Signed-off-by: Kenzo Lobos-Tsunekawa --- perception/lidar_centerpoint/lib/network/network_trt.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/perception/lidar_centerpoint/lib/network/network_trt.cpp b/perception/lidar_centerpoint/lib/network/network_trt.cpp index 88319ff51fe35..79430e42c4869 100644 --- a/perception/lidar_centerpoint/lib/network/network_trt.cpp +++ b/perception/lidar_centerpoint/lib/network/network_trt.cpp @@ -14,6 +14,8 @@ #include "lidar_centerpoint/network/network_trt.hpp" +#include + namespace centerpoint { bool VoxelEncoderTRT::setProfile( @@ -59,6 +61,13 @@ bool HeadTRT::setProfile( for (std::size_t ci = 0; ci < out_channel_sizes_.size(); ci++) { auto out_name = network.getOutput(ci)->getName(); + + if ( + out_name == std::string("heatmap") && + network.getOutput(ci)->getDimensions().d[1] != static_cast(out_channel_sizes_[ci])) { + std::cout << "Expected and actual number of classes do not match" << std::endl; + return false; + } auto out_dims = nvinfer1::Dims4( config_.batch_size_, out_channel_sizes_[ci], config_.down_grid_size_y_, config_.down_grid_size_x_); From f0f0d380370824f731d20277a5df5511f67d4d39 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 17 Mar 2023 14:52:31 +0900 Subject: [PATCH 2/6] Changed std::cout for logger calls Fixed typos Signed-off-by: Kenzo Lobos-Tsunekawa --- .../tensorrt_common/tensorrt_common.hpp | 2 ++ .../lib/network/network_trt.cpp | 4 ++- .../lib/network/tensorrt_wrapper.cpp | 29 +++++++++++-------- 3 files changed, 22 insertions(+), 13 deletions(-) diff --git a/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp b/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp index bb271f0689002..8d322d4190a7d 100644 --- a/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp +++ b/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp @@ -68,6 +68,8 @@ class Logger : public nvinfer1::ILogger // NOLINT } } + void log(Severity severity, const std::string & msg) noexcept { log(severity, msg.c_str()); } + Severity reportable_severity_{Severity::kWARNING}; rclcpp::Logger logger_{rclcpp::get_logger("tensorrt_common")}; }; diff --git a/perception/lidar_centerpoint/lib/network/network_trt.cpp b/perception/lidar_centerpoint/lib/network/network_trt.cpp index 79430e42c4869..f0504472b029c 100644 --- a/perception/lidar_centerpoint/lib/network/network_trt.cpp +++ b/perception/lidar_centerpoint/lib/network/network_trt.cpp @@ -50,6 +50,8 @@ bool HeadTRT::setProfile( nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, nvinfer1::IBuilderConfig & config) { + using Severity = tensorrt_common::Logger::Severity; + auto profile = builder.createOptimizationProfile(); auto in_name = network.getInput(0)->getName(); auto in_dims = nvinfer1::Dims4( @@ -65,7 +67,7 @@ bool HeadTRT::setProfile( if ( out_name == std::string("heatmap") && network.getOutput(ci)->getDimensions().d[1] != static_cast(out_channel_sizes_[ci])) { - std::cout << "Expected and actual number of classes do not match" << std::endl; + logger_.log(Severity::kERROR, "Expected and actual number of classes do not match"); return false; } auto out_dims = nvinfer1::Dims4( diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index 079c41d06c6e0..18a27491dd308 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -75,17 +75,19 @@ bool TensorRTWrapper::parseONNX( const std::string & onnx_path, const std::string & engine_path, const std::string & precision, const size_t workspace_size) { + using Severity = tensorrt_common::Logger::Severity; + auto builder = tensorrt_common::TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); if (!builder) { - std::cout << "Fail to create builder" << std::endl; + logger_.log(Severity::kERROR, "Failed to create builder"); return false; } auto config = tensorrt_common::TrtUniquePtr(builder->createBuilderConfig()); if (!config) { - std::cout << "Fail to create config" << std::endl; + logger_.log(Severity::kERROR, "Failed to create config"); return false; } #if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 @@ -95,10 +97,10 @@ bool TensorRTWrapper::parseONNX( #endif if (precision == "fp16") { if (builder->platformHasFastFp16()) { - std::cout << "use TensorRT FP16 Inference" << std::endl; + logger_.log(Severity::kINFO, "Using TensorRT FP16 Inference"); config->setFlag(nvinfer1::BuilderFlag::kFP16); } else { - std::cout << "TensorRT FP16 Inference isn't supported in this environment" << std::endl; + logger_.log(Severity::kINFO, "TensorRT FP16 Inference isn't supported in this environment"); } } @@ -107,7 +109,7 @@ bool TensorRTWrapper::parseONNX( auto network = tensorrt_common::TrtUniquePtr(builder->createNetworkV2(flag)); if (!network) { - std::cout << "Fail to create network" << std::endl; + logger_.log(Severity::kERROR, "Failed to create network"); return false; } @@ -116,22 +118,23 @@ bool TensorRTWrapper::parseONNX( parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); if (!setProfile(*builder, *network, *config)) { - std::cout << "Fail to set profile" << std::endl; + logger_.log(Severity::kERROR, "Failed to set profile"); return false; } - std::cout << "Applying optimizations and building TRT CUDA engine (" << onnx_path << ") ..." - << std::endl; + logger_.log( + Severity::kINFO, std::string("Applying optimizations and building TRT CUDA engine (") + + onnx_path + std::string(") ...")); plan_ = tensorrt_common::TrtUniquePtr( builder->buildSerializedNetwork(*network, *config)); if (!plan_) { - std::cout << "Fail to create serialized network" << std::endl; + logger_.log(Severity::kERROR, "Failed to create serialized network"); return false; } engine_ = tensorrt_common::TrtUniquePtr( runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!engine_) { - std::cout << "Fail to create engine" << std::endl; + logger_.log(Severity::kERROR, "Failed to create engine"); return false; } @@ -140,7 +143,8 @@ bool TensorRTWrapper::parseONNX( bool TensorRTWrapper::saveEngine(const std::string & engine_path) { - std::cout << "Writing to " << engine_path << std::endl; + using Severity = tensorrt_common::Logger::Severity; + logger_.log(Severity::kINFO, std::string("Writing to ") + engine_path); std::ofstream file(engine_path, std::ios::out | std::ios::binary); file.write(reinterpret_cast(plan_->data()), plan_->size()); return true; @@ -148,13 +152,14 @@ bool TensorRTWrapper::saveEngine(const std::string & engine_path) bool TensorRTWrapper::loadEngine(const std::string & engine_path) { + using Severity = tensorrt_common::Logger::Severity; 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; + logger_.log(Severity::kINFO, std::string("Loaded engine from ") + engine_path); return true; } From cfadced301f3bb8bdb1feb860a43d8f4547c010f Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 17 Mar 2023 16:25:54 +0900 Subject: [PATCH 3/6] Missed some couts Signed-off-by: Kenzo Lobos-Tsunekawa --- perception/lidar_centerpoint/lib/network/network_trt.cpp | 2 -- .../lidar_centerpoint/lib/network/tensorrt_wrapper.cpp | 9 +++++++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/perception/lidar_centerpoint/lib/network/network_trt.cpp b/perception/lidar_centerpoint/lib/network/network_trt.cpp index f0504472b029c..2baf909448caa 100644 --- a/perception/lidar_centerpoint/lib/network/network_trt.cpp +++ b/perception/lidar_centerpoint/lib/network/network_trt.cpp @@ -14,8 +14,6 @@ #include "lidar_centerpoint/network/network_trt.hpp" -#include - namespace centerpoint { bool VoxelEncoderTRT::setProfile( diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index 18a27491dd308..22c116e9bf785 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -35,10 +35,12 @@ TensorRTWrapper::~TensorRTWrapper() bool TensorRTWrapper::init( const std::string & onnx_path, const std::string & engine_path, const std::string & precision) { + using Severity = tensorrt_common::Logger::Severity; + runtime_ = tensorrt_common::TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); if (!runtime_) { - std::cout << "Fail to create runtime" << std::endl; + logger_.log(Severity::kERROR, "Failed to create runtime"); return false; } @@ -56,8 +58,10 @@ bool TensorRTWrapper::init( bool TensorRTWrapper::createContext() { + using Severity = tensorrt_common::Logger::Severity; + if (!engine_) { - std::cout << "Fail to create context: Engine isn't created" << std::endl; + logger_.log(Severity::kERROR, "Failed to create context: Engine was not created"); return false; } @@ -65,6 +69,7 @@ bool TensorRTWrapper::createContext() tensorrt_common::TrtUniquePtr(engine_->createExecutionContext()); if (!context_) { std::cout << "Fail to create context" << std::endl; + logger_.log(Severity::kERROR, "Failed to create builder"); return false; } From 433168e0505c6cf46e4f1d8875b407f59dea1d0b Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 28 Mar 2023 01:28:30 +0900 Subject: [PATCH 4/6] Changed to the standard rclcpp logger Signed-off-by: Kenzo Lobos-Tsunekawa --- .../lib/network/network_trt.cpp | 8 ++-- .../lib/network/tensorrt_wrapper.cpp | 45 +++++++++---------- 2 files changed, 26 insertions(+), 27 deletions(-) diff --git a/perception/lidar_centerpoint/lib/network/network_trt.cpp b/perception/lidar_centerpoint/lib/network/network_trt.cpp index 2baf909448caa..2d841d22c2eb1 100644 --- a/perception/lidar_centerpoint/lib/network/network_trt.cpp +++ b/perception/lidar_centerpoint/lib/network/network_trt.cpp @@ -14,6 +14,8 @@ #include "lidar_centerpoint/network/network_trt.hpp" +#include + namespace centerpoint { bool VoxelEncoderTRT::setProfile( @@ -48,8 +50,6 @@ bool HeadTRT::setProfile( nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, nvinfer1::IBuilderConfig & config) { - using Severity = tensorrt_common::Logger::Severity; - auto profile = builder.createOptimizationProfile(); auto in_name = network.getInput(0)->getName(); auto in_dims = nvinfer1::Dims4( @@ -65,7 +65,9 @@ bool HeadTRT::setProfile( if ( out_name == std::string("heatmap") && network.getOutput(ci)->getDimensions().d[1] != static_cast(out_channel_sizes_[ci])) { - logger_.log(Severity::kERROR, "Expected and actual number of classes do not match"); + RCLCPP_ERROR( + rclcpp::get_logger("lidar_centerpoint"), + "Expected and actual number of classes do not match"); return false; } auto out_dims = nvinfer1::Dims4( diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index 22c116e9bf785..f632b3a60c3b2 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -14,6 +14,8 @@ #include "lidar_centerpoint/network/tensorrt_wrapper.hpp" +#include + #include #include @@ -35,12 +37,10 @@ TensorRTWrapper::~TensorRTWrapper() bool TensorRTWrapper::init( const std::string & onnx_path, const std::string & engine_path, const std::string & precision) { - using Severity = tensorrt_common::Logger::Severity; - runtime_ = tensorrt_common::TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); if (!runtime_) { - logger_.log(Severity::kERROR, "Failed to create runtime"); + RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create runtime"); return false; } @@ -58,10 +58,9 @@ bool TensorRTWrapper::init( bool TensorRTWrapper::createContext() { - using Severity = tensorrt_common::Logger::Severity; - if (!engine_) { - logger_.log(Severity::kERROR, "Failed to create context: Engine was not created"); + RCLCPP_ERROR( + rclcpp::get_logger("lidar_centerpoint"), "Failed to create context: Engine was not created"); return false; } @@ -69,7 +68,7 @@ bool TensorRTWrapper::createContext() tensorrt_common::TrtUniquePtr(engine_->createExecutionContext()); if (!context_) { std::cout << "Fail to create context" << std::endl; - logger_.log(Severity::kERROR, "Failed to create builder"); + RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create builder"); return false; } @@ -80,19 +79,17 @@ bool TensorRTWrapper::parseONNX( const std::string & onnx_path, const std::string & engine_path, const std::string & precision, const size_t workspace_size) { - using Severity = tensorrt_common::Logger::Severity; - auto builder = tensorrt_common::TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); if (!builder) { - logger_.log(Severity::kERROR, "Failed to create builder"); + RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create builder"); return false; } auto config = tensorrt_common::TrtUniquePtr(builder->createBuilderConfig()); if (!config) { - logger_.log(Severity::kERROR, "Failed to create config"); + RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create config"); return false; } #if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 @@ -102,10 +99,12 @@ bool TensorRTWrapper::parseONNX( #endif if (precision == "fp16") { if (builder->platformHasFastFp16()) { - logger_.log(Severity::kINFO, "Using TensorRT FP16 Inference"); + RCLCPP_INFO(rclcpp::get_logger("lidar_centerpoint"), "Using TensorRT FP16 Inference"); config->setFlag(nvinfer1::BuilderFlag::kFP16); } else { - logger_.log(Severity::kINFO, "TensorRT FP16 Inference isn't supported in this environment"); + RCLCPP_INFO( + rclcpp::get_logger("lidar_centerpoint"), + "TensorRT FP16 Inference isn't supported in this environment"); } } @@ -114,7 +113,7 @@ bool TensorRTWrapper::parseONNX( auto network = tensorrt_common::TrtUniquePtr(builder->createNetworkV2(flag)); if (!network) { - logger_.log(Severity::kERROR, "Failed to create network"); + RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create network"); return false; } @@ -123,23 +122,23 @@ bool TensorRTWrapper::parseONNX( parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); if (!setProfile(*builder, *network, *config)) { - logger_.log(Severity::kERROR, "Failed to set profile"); + RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to set profile"); return false; } - logger_.log( - Severity::kINFO, std::string("Applying optimizations and building TRT CUDA engine (") + - onnx_path + std::string(") ...")); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("lidar_centerpoint"), + "Applying optimizations and building TRT CUDA engine (" << onnx_path << ") ..."); plan_ = tensorrt_common::TrtUniquePtr( builder->buildSerializedNetwork(*network, *config)); if (!plan_) { - logger_.log(Severity::kERROR, "Failed to create serialized network"); + RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create serialized network"); return false; } engine_ = tensorrt_common::TrtUniquePtr( runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!engine_) { - logger_.log(Severity::kERROR, "Failed to create engine"); + RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create engine"); return false; } @@ -148,8 +147,7 @@ bool TensorRTWrapper::parseONNX( bool TensorRTWrapper::saveEngine(const std::string & engine_path) { - using Severity = tensorrt_common::Logger::Severity; - logger_.log(Severity::kINFO, std::string("Writing to ") + engine_path); + RCLCPP_INFO_STREAM(rclcpp::get_logger("lidar_centerpoint"), "Writing to " << engine_path); std::ofstream file(engine_path, std::ios::out | std::ios::binary); file.write(reinterpret_cast(plan_->data()), plan_->size()); return true; @@ -157,14 +155,13 @@ bool TensorRTWrapper::saveEngine(const std::string & engine_path) bool TensorRTWrapper::loadEngine(const std::string & engine_path) { - using Severity = tensorrt_common::Logger::Severity; 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())); - logger_.log(Severity::kINFO, std::string("Loaded engine from ") + engine_path); + RCLCPP_INFO_STREAM(rclcpp::get_logger("lidar_centerpoint"), "Loaded engine from " << engine_path); return true; } From c686a8335422218b711d1bc98d36a43f39eeffe4 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 28 Mar 2023 01:31:23 +0900 Subject: [PATCH 5/6] Deleted unused logic Signed-off-by: Kenzo Lobos-Tsunekawa --- .../tensorrt_common/include/tensorrt_common/tensorrt_common.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp b/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp index 8d322d4190a7d..bb271f0689002 100644 --- a/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp +++ b/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp @@ -68,8 +68,6 @@ class Logger : public nvinfer1::ILogger // NOLINT } } - void log(Severity severity, const std::string & msg) noexcept { log(severity, msg.c_str()); } - Severity reportable_severity_{Severity::kWARNING}; rclcpp::Logger logger_{rclcpp::get_logger("tensorrt_common")}; }; From 2a6b2411dc77ee33093ebbaf4b65c85d308dcd40 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Tue, 28 Mar 2023 19:31:02 +0900 Subject: [PATCH 6/6] Update perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp Co-authored-by: Yusuke Muramatsu --- perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index f632b3a60c3b2..4840b63940df1 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -67,8 +67,7 @@ bool TensorRTWrapper::createContext() context_ = tensorrt_common::TrtUniquePtr(engine_->createExecutionContext()); if (!context_) { - std::cout << "Fail to create context" << std::endl; - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create builder"); + RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create context"); return false; }