diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 8e1aac2aa7..d6e0f536ed 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -325,8 +325,20 @@ class Subscription : public SubscriptionBase const std::shared_ptr & serialized_message, const rclcpp::MessageInfo & message_info) override { - // TODO(wjwwood): enable topic statistics for serialized messages + std::chrono::time_point now; + if (subscription_topic_statistics_) { + // get current time before executing callback to + // exclude callback duration from topic statistics result. + now = std::chrono::system_clock::now(); + } + any_callback_.dispatch(serialized_message, message_info); + + if (subscription_topic_statistics_) { + const auto nanos = std::chrono::time_point_cast(now); + const auto time = rclcpp::Time(nanos.time_since_epoch().count()); + subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time); + } } void