Skip to content

Commit 6ee9627

Browse files
authored
feat(pointcloud_preprocessor): add pipeline_latency_ms debug publisher to missed modules (#6569)
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent ca46a8f commit 6ee9627

File tree

2 files changed

+23
-0
lines changed

2 files changed

+23
-0
lines changed

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp

+14
Original file line numberDiff line numberDiff line change
@@ -281,6 +281,20 @@ void PointCloudConcatenationComponent::publish()
281281
checkSyncStatus();
282282
combineClouds(concat_cloud_ptr);
283283

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+
284298
// publish concatenated pointcloud
285299
if (concat_cloud_ptr) {
286300
auto output = std::make_unique<sensor_msgs::msg::PointCloud2>(*concat_cloud_ptr);

sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp

+9
Original file line numberDiff line numberDiff line change
@@ -374,6 +374,15 @@ void PointCloudDataSynchronizerComponent::publish()
374374
// publish transformed raw pointclouds
375375
for (const auto & e : transformed_raw_points) {
376376
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+
}
377386
auto output = std::make_unique<sensor_msgs::msg::PointCloud2>(*e.second);
378387
transformed_raw_pc_publisher_map_[e.first]->publish(std::move(output));
379388
} else {

0 commit comments

Comments
 (0)