Skip to content

Commit

Permalink
chore: tier4 debug msgs to autoware internal debug msgs
Browse files Browse the repository at this point in the history
Signed-off-by: vividf <[email protected]>
  • Loading branch information
vividf committed Jan 16, 2025
1 parent 137766f commit 14a63fb
Show file tree
Hide file tree
Showing 2 changed files with 5 additions and 5 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -31,14 +31,14 @@
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>

#include <autoware_internal_debug_msgs/msg/int32_stamped.hpp>
#include <autoware_internal_debug_msgs/msg/string_stamped.hpp>
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <tier4_debug_msgs/msg/int32_stamped.hpp>
#include <tier4_debug_msgs/msg/string_stamped.hpp>

#include <message_filters/pass_through.h>
#include <message_filters/subscriber.h>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -330,14 +330,14 @@ void PointCloudConcatenateDataSynchronizerComponent::publish_clouds(
if (debug_publisher_) {
const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
debug_publisher_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
"debug/cyclic_time_ms", cyclic_time_ms);
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
debug_publisher_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
"debug/processing_time_ms", processing_time_ms);

for (const auto & [topic, stamp] : concatenated_cloud_result.topic_to_original_stamp_map) {
const auto pipeline_latency_ms = (this->get_clock()->now().seconds() - stamp) * 1000;
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
debug_publisher_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
"debug" + topic + "/pipeline_latency_ms", pipeline_latency_ms);
}
}
Expand Down

0 comments on commit 14a63fb

Please sign in to comment.