14
14
15
15
#include " lidar_centerpoint/network/tensorrt_wrapper.hpp"
16
16
17
- #include < rclcpp/rclcpp.hpp>
18
-
19
17
#include < NvOnnxParser.h>
20
18
21
19
#include < fstream>
@@ -42,7 +40,7 @@ bool TensorRTWrapper::init(
42
40
runtime_ =
43
41
tensorrt_common::TrtUniquePtr<nvinfer1::IRuntime>(nvinfer1::createInferRuntime (logger_));
44
42
if (!runtime_) {
45
- RCLCPP_ERROR ( rclcpp::get_logger ( " lidar_centerpoint " ), " Failed to create runtime" ) ;
43
+ tensorrt_common::LOG_ERROR (logger_) << " Failed to create runtime" << std::endl ;
46
44
return false ;
47
45
}
48
46
@@ -51,7 +49,11 @@ bool TensorRTWrapper::init(
51
49
if (engine_file.is_open ()) {
52
50
success = loadEngine (engine_path);
53
51
} else {
52
+ auto log_thread = logger_.log_throttle (
53
+ nvinfer1::ILogger::Severity::kINFO ,
54
+ " Applying optimizations and building TRT CUDA engine. Please wait a minutes..." , 5 );
54
55
success = parseONNX (onnx_path, engine_path, precision);
56
+ logger_.stop_throttle (log_thread);
55
57
}
56
58
success &= createContext ();
57
59
@@ -61,15 +63,15 @@ bool TensorRTWrapper::init(
61
63
bool TensorRTWrapper::createContext ()
62
64
{
63
65
if (!engine_) {
64
- RCLCPP_ERROR (
65
- rclcpp::get_logger ( " lidar_centerpoint " ), " Failed to create context: Engine was not created" ) ;
66
+ tensorrt_common::LOG_ERROR (logger_)
67
+ << " Failed to create context: Engine was not created" << std::endl ;
66
68
return false ;
67
69
}
68
70
69
71
context_ =
70
72
tensorrt_common::TrtUniquePtr<nvinfer1::IExecutionContext>(engine_->createExecutionContext ());
71
73
if (!context_) {
72
- RCLCPP_ERROR ( rclcpp::get_logger ( " lidar_centerpoint " ), " Failed to create context" ) ;
74
+ tensorrt_common::LOG_ERROR (logger_) << " Failed to create context" << std::endl ;
73
75
return false ;
74
76
}
75
77
@@ -83,14 +85,14 @@ bool TensorRTWrapper::parseONNX(
83
85
auto builder =
84
86
tensorrt_common::TrtUniquePtr<nvinfer1::IBuilder>(nvinfer1::createInferBuilder (logger_));
85
87
if (!builder) {
86
- RCLCPP_ERROR ( rclcpp::get_logger ( " lidar_centerpoint " ), " Failed to create builder" ) ;
88
+ tensorrt_common::LOG_ERROR (logger_) << " Failed to create builder" << std::endl ;
87
89
return false ;
88
90
}
89
91
90
92
auto config =
91
93
tensorrt_common::TrtUniquePtr<nvinfer1::IBuilderConfig>(builder->createBuilderConfig ());
92
94
if (!config) {
93
- RCLCPP_ERROR ( rclcpp::get_logger ( " lidar_centerpoint " ), " Failed to create config" ) ;
95
+ tensorrt_common::LOG_ERROR (logger_) << " Failed to create config" << std::endl ;
94
96
return false ;
95
97
}
96
98
#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400
@@ -100,12 +102,11 @@ bool TensorRTWrapper::parseONNX(
100
102
#endif
101
103
if (precision == " fp16" ) {
102
104
if (builder->platformHasFastFp16 ()) {
103
- RCLCPP_INFO ( rclcpp::get_logger ( " lidar_centerpoint " ), " Using TensorRT FP16 Inference" ) ;
105
+ tensorrt_common::LOG_INFO (logger_) << " Using TensorRT FP16 Inference" << std::endl ;
104
106
config->setFlag (nvinfer1::BuilderFlag::kFP16 );
105
107
} else {
106
- RCLCPP_INFO (
107
- rclcpp::get_logger (" lidar_centerpoint" ),
108
- " TensorRT FP16 Inference isn't supported in this environment" );
108
+ tensorrt_common::LOG_INFO (logger_)
109
+ << " TensorRT FP16 Inference isn't supported in this environment" << std::endl;
109
110
}
110
111
}
111
112
@@ -114,7 +115,7 @@ bool TensorRTWrapper::parseONNX(
114
115
auto network =
115
116
tensorrt_common::TrtUniquePtr<nvinfer1::INetworkDefinition>(builder->createNetworkV2 (flag));
116
117
if (!network) {
117
- RCLCPP_ERROR ( rclcpp::get_logger ( " lidar_centerpoint " ), " Failed to create network" ) ;
118
+ tensorrt_common::LOG_ERROR (logger_) << " Failed to create network" << std::endl ;
118
119
return false ;
119
120
}
120
121
@@ -123,23 +124,20 @@ bool TensorRTWrapper::parseONNX(
123
124
parser->parseFromFile (onnx_path.c_str (), static_cast <int >(nvinfer1::ILogger::Severity::kERROR ));
124
125
125
126
if (!setProfile (*builder, *network, *config)) {
126
- RCLCPP_ERROR ( rclcpp::get_logger ( " lidar_centerpoint " ), " Failed to set profile" ) ;
127
+ tensorrt_common::LOG_ERROR (logger_) << " Failed to set profile" << std::endl ;
127
128
return false ;
128
129
}
129
130
130
- RCLCPP_INFO_STREAM (
131
- rclcpp::get_logger (" lidar_centerpoint" ),
132
- " Applying optimizations and building TRT CUDA engine (" << onnx_path << " ) ..." );
133
131
plan_ = tensorrt_common::TrtUniquePtr<nvinfer1::IHostMemory>(
134
132
builder->buildSerializedNetwork (*network, *config));
135
133
if (!plan_) {
136
- RCLCPP_ERROR ( rclcpp::get_logger ( " lidar_centerpoint " ), " Failed to create serialized network" ) ;
134
+ tensorrt_common::LOG_ERROR (logger_) << " Failed to create serialized network" << std::endl ;
137
135
return false ;
138
136
}
139
137
engine_ = tensorrt_common::TrtUniquePtr<nvinfer1::ICudaEngine>(
140
138
runtime_->deserializeCudaEngine (plan_->data (), plan_->size ()));
141
139
if (!engine_) {
142
- RCLCPP_ERROR ( rclcpp::get_logger ( " lidar_centerpoint " ), " Failed to create engine" ) ;
140
+ tensorrt_common::LOG_ERROR (logger_) << " Failed to create engine" << std::endl ;
143
141
return false ;
144
142
}
145
143
@@ -148,7 +146,7 @@ bool TensorRTWrapper::parseONNX(
148
146
149
147
bool TensorRTWrapper::saveEngine (const std::string & engine_path)
150
148
{
151
- RCLCPP_INFO_STREAM ( rclcpp::get_logger ( " lidar_centerpoint " ), " Writing to " << engine_path) ;
149
+ tensorrt_common::LOG_INFO (logger_) << " Writing to " << engine_path << std::endl ;
152
150
std::ofstream file (engine_path, std::ios::out | std::ios::binary);
153
151
file.write (reinterpret_cast <const char *>(plan_->data ()), plan_->size ());
154
152
return true ;
@@ -162,7 +160,7 @@ bool TensorRTWrapper::loadEngine(const std::string & engine_path)
162
160
std::string engine_str = engine_buffer.str ();
163
161
engine_ = tensorrt_common::TrtUniquePtr<nvinfer1::ICudaEngine>(runtime_->deserializeCudaEngine (
164
162
reinterpret_cast <const void *>(engine_str.data ()), engine_str.size ()));
165
- RCLCPP_INFO_STREAM ( rclcpp::get_logger ( " lidar_centerpoint " ), " Loaded engine from " << engine_path) ;
163
+ tensorrt_common::LOG_INFO (logger_) << " Loaded engine from " << engine_path << std::endl ;
166
164
return true ;
167
165
}
168
166
0 commit comments