Skip to content

Commit

Permalink
feat(pointcloud_preprocessor): add pipeline_latency_ms debug publishe…
Browse files Browse the repository at this point in the history
…r to missed modules (autowarefoundation#6569)

Signed-off-by: Berkay Karaman <[email protected]>
  • Loading branch information
brkay54 authored and karishma1911 committed Jun 3, 2024
1 parent 5417b73 commit d81add1
Show file tree
Hide file tree
Showing 2 changed files with 23 additions and 0 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -281,6 +281,20 @@ void PointCloudConcatenationComponent::publish()
checkSyncStatus();
combineClouds(concat_cloud_ptr);

for (const auto & e : cloud_stdmap_) {
if (e.second != nullptr) {
if (debug_publisher_) {
const auto pipeline_latency_ms =
std::chrono::duration<double, std::milli>(
std::chrono::nanoseconds(
(this->get_clock()->now() - e.second->header.stamp).nanoseconds()))
.count();
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms);
}
}
}

// publish concatenated pointcloud
if (concat_cloud_ptr) {
auto output = std::make_unique<sensor_msgs::msg::PointCloud2>(*concat_cloud_ptr);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -374,6 +374,15 @@ void PointCloudDataSynchronizerComponent::publish()
// publish transformed raw pointclouds
for (const auto & e : transformed_raw_points) {
if (e.second) {
if (debug_publisher_) {
const auto pipeline_latency_ms =
std::chrono::duration<double, std::milli>(
std::chrono::nanoseconds(
(this->get_clock()->now() - e.second->header.stamp).nanoseconds()))
.count();
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms);
}
auto output = std::make_unique<sensor_msgs::msg::PointCloud2>(*e.second);
transformed_raw_pc_publisher_map_[e.first]->publish(std::move(output));
} else {
Expand Down

0 comments on commit d81add1

Please sign in to comment.