Commit 6ee9627 1 parent ca46a8f commit 6ee9627 Copy full SHA for 6ee9627
File tree 2 files changed +23
-0
lines changed
sensing/pointcloud_preprocessor/src
2 files changed +23
-0
lines changed Original file line number Diff line number Diff line change @@ -281,6 +281,20 @@ void PointCloudConcatenationComponent::publish()
281
281
checkSyncStatus ();
282
282
combineClouds (concat_cloud_ptr);
283
283
284
+ for (const auto & e : cloud_stdmap_) {
285
+ if (e.second != nullptr ) {
286
+ if (debug_publisher_) {
287
+ const auto pipeline_latency_ms =
288
+ std::chrono::duration<double , std::milli>(
289
+ std::chrono::nanoseconds (
290
+ (this ->get_clock ()->now () - e.second ->header .stamp ).nanoseconds ()))
291
+ .count ();
292
+ debug_publisher_->publish <tier4_debug_msgs::msg::Float64Stamped>(
293
+ " debug" + e.first + " /pipeline_latency_ms" , pipeline_latency_ms);
294
+ }
295
+ }
296
+ }
297
+
284
298
// publish concatenated pointcloud
285
299
if (concat_cloud_ptr) {
286
300
auto output = std::make_unique<sensor_msgs::msg::PointCloud2>(*concat_cloud_ptr);
Original file line number Diff line number Diff line change @@ -374,6 +374,15 @@ void PointCloudDataSynchronizerComponent::publish()
374
374
// publish transformed raw pointclouds
375
375
for (const auto & e : transformed_raw_points) {
376
376
if (e.second ) {
377
+ if (debug_publisher_) {
378
+ const auto pipeline_latency_ms =
379
+ std::chrono::duration<double , std::milli>(
380
+ std::chrono::nanoseconds (
381
+ (this ->get_clock ()->now () - e.second ->header .stamp ).nanoseconds ()))
382
+ .count ();
383
+ debug_publisher_->publish <tier4_debug_msgs::msg::Float64Stamped>(
384
+ " debug" + e.first + " /pipeline_latency_ms" , pipeline_latency_ms);
385
+ }
377
386
auto output = std::make_unique<sensor_msgs::msg::PointCloud2>(*e.second );
378
387
transformed_raw_pc_publisher_map_[e.first ]->publish (std::move (output));
379
388
} else {
You can’t perform that action at this time.
0 commit comments