From 4b7f0f1c1141a105dfe071b8bd7854ae6745d97d Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 12 May 2022 11:12:02 +0800 Subject: [PATCH 1/3] Add statistics for handle_loaned_message Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/subscription.hpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 69b6031405..11bf9c6e43 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -363,11 +363,31 @@ class Subscription : public SubscriptionBase void * loaned_message, const rclcpp::MessageInfo & message_info) override { + if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) { + // In this case, the message will be delivered via intra process and + // we should ignore this copy of the message. + return; + } + auto typed_message = static_cast(loaned_message); // message is loaned, so we have to make sure that the deleter does not deallocate the message auto sptr = std::shared_ptr( typed_message, [](ROSMessageType * msg) {(void) msg;}); + + 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(sptr, 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(*typed_message, time); + } } /// Return the borrowed message. From bc45ab2214d5b780ae85905e602ace0fa8efa04b Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 13 May 2022 16:16:20 +0800 Subject: [PATCH 2/3] Remove unused code Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/subscription.hpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 11bf9c6e43..f11a3d92d6 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -363,12 +363,6 @@ class Subscription : public SubscriptionBase void * loaned_message, const rclcpp::MessageInfo & message_info) override { - if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) { - // In this case, the message will be delivered via intra process and - // we should ignore this copy of the message. - return; - } - auto typed_message = static_cast(loaned_message); // message is loaned, so we have to make sure that the deleter does not deallocate the message auto sptr = std::shared_ptr( From 8a705023f5102ae5bcb008497342da8c43267185 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Mon, 16 May 2022 15:27:59 +0800 Subject: [PATCH 3/3] Revert "Remove unused code" This reverts commit 565d683ca4aa2ddac9f450a38b248939f673a6f7. Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/subscription.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index f11a3d92d6..11bf9c6e43 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -363,6 +363,12 @@ class Subscription : public SubscriptionBase void * loaned_message, const rclcpp::MessageInfo & message_info) override { + if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) { + // In this case, the message will be delivered via intra process and + // we should ignore this copy of the message. + return; + } + auto typed_message = static_cast(loaned_message); // message is loaned, so we have to make sure that the deleter does not deallocate the message auto sptr = std::shared_ptr(