@@ -29,6 +29,14 @@ namespace tensorrt_yolox
29
29
TrtYoloXNode::TrtYoloXNode (const rclcpp::NodeOptions & node_options)
30
30
: Node(" tensorrt_yolox" , node_options)
31
31
{
32
+ {
33
+ stop_watch_ptr_ =
34
+ std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
35
+ debug_publisher_ =
36
+ std::make_unique<tier4_autoware_utils::DebugPublisher>(this , " tensorrt_yolox" );
37
+ stop_watch_ptr_->tic (" cyclic_time" );
38
+ stop_watch_ptr_->tic (" processing_time" );
39
+ }
32
40
using std::placeholders::_1;
33
41
using std::chrono_literals::operator " " ms;
34
42
@@ -132,6 +140,7 @@ void TrtYoloXNode::onConnect()
132
140
133
141
void TrtYoloXNode::onImage (const sensor_msgs::msg::Image::ConstSharedPtr msg)
134
142
{
143
+ stop_watch_ptr_->toc (" processing_time" , true );
135
144
tier4_perception_msgs::msg::DetectedObjectsWithFeature out_objects;
136
145
137
146
cv_bridge::CvImagePtr in_image_ptr;
@@ -173,6 +182,22 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg)
173
182
174
183
out_objects.header = msg->header ;
175
184
objects_pub_->publish (out_objects);
185
+
186
+ if (debug_publisher_) {
187
+ const double processing_time_ms = stop_watch_ptr_->toc (" processing_time" , true );
188
+ const double cyclic_time_ms = stop_watch_ptr_->toc (" cyclic_time" , true );
189
+ const double pipeline_latency_ms =
190
+ std::chrono::duration<double , std::milli>(
191
+ std::chrono::nanoseconds (
192
+ (this ->get_clock ()->now () - out_objects.header .stamp ).nanoseconds ()))
193
+ .count ();
194
+ debug_publisher_->publish <tier4_debug_msgs::msg::Float64Stamped>(
195
+ " debug/cyclic_time_ms" , cyclic_time_ms);
196
+ debug_publisher_->publish <tier4_debug_msgs::msg::Float64Stamped>(
197
+ " debug/processing_time_ms" , processing_time_ms);
198
+ debug_publisher_->publish <tier4_debug_msgs::msg::Float64Stamped>(
199
+ " debug/pipeline_latency_ms" , pipeline_latency_ms);
200
+ }
176
201
}
177
202
178
203
bool TrtYoloXNode::readLabelFile (const std::string & label_path)
0 commit comments