From 19752e3757d6cf13497f1b9c57cfe9132def7e3e Mon Sep 17 00:00:00 2001 From: h-suzuki Date: Wed, 10 Jan 2024 14:40:58 +0900 Subject: [PATCH 1/6] Add tracepoint for generic publisher/subscriber Signed-off-by: h-suzuki --- .../include/rclcpp/generic_subscription.hpp | 28 ++++++++++++------- rclcpp/src/rclcpp/generic_publisher.cpp | 4 +++ rclcpp/src/rclcpp/generic_subscription.cpp | 2 +- 3 files changed, 23 insertions(+), 11 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index e9bf79deea..76fb73bea8 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -84,13 +84,23 @@ class GenericSubscription : public rclcpp::SubscriptionBase options.event_callbacks, options.use_default_callbacks, DeliveredMessageKind::SERIALIZED_MESSAGE), - callback_([callback]( - std::shared_ptr serialized_message, - const rclcpp::MessageInfo & message_info) mutable { - callback.dispatch(serialized_message, message_info); - }), + any_callback_(callback), ts_lib_(ts_lib) - {} + { + auto callback_ptr = static_cast(&any_callback_); + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_init, + static_cast(get_subscription_handle().get()), + static_cast(this)); + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_callback_added, + static_cast(this), + callback_ptr); + +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif + } RCLCPP_PUBLIC virtual ~GenericSubscription() = default; @@ -153,10 +163,8 @@ class GenericSubscription : public rclcpp::SubscriptionBase private: RCLCPP_DISABLE_COPY(GenericSubscription) - - std::function, - const rclcpp::MessageInfo)> callback_; + + AnySubscriptionCallback> any_callback_; // The type support library should stay loaded, so it is stored in the GenericSubscription std::shared_ptr ts_lib_; }; diff --git a/rclcpp/src/rclcpp/generic_publisher.cpp b/rclcpp/src/rclcpp/generic_publisher.cpp index 87ace1eb19..7ab0785b8a 100644 --- a/rclcpp/src/rclcpp/generic_publisher.cpp +++ b/rclcpp/src/rclcpp/generic_publisher.cpp @@ -23,6 +23,9 @@ namespace rclcpp void GenericPublisher::publish(const rclcpp::SerializedMessage & message) { + TRACETOOLS_TRACEPOINT(rclcpp_publish, + static_cast(publisher_handle_.get()), + static_cast(&message.get_rcl_serialized_message())); auto return_code = rcl_publish_serialized_message( get_publisher_handle().get(), &message.get_rcl_serialized_message(), NULL); @@ -68,6 +71,7 @@ void GenericPublisher::deserialize_message( void GenericPublisher::publish_loaned_message(void * loaned_message) { + TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast(loaned_message)); auto return_code = rcl_publish_loaned_message( get_publisher_handle().get(), loaned_message, NULL); diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index 2d2be61277..4878489826 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -51,7 +51,7 @@ GenericSubscription::handle_serialized_message( const std::shared_ptr & message, const rclcpp::MessageInfo & message_info) { - callback_(message, message_info); + any_callback_.dispatch(message,message_info); } void From 1b5c1066c6b61270d5f693fc57797a5dec5062cb Mon Sep 17 00:00:00 2001 From: h-suzuki Date: Tue, 5 Mar 2024 07:52:01 +0000 Subject: [PATCH 2/6] Fix: tracepoint args Signed-off-by: h-suzuki --- rclcpp/src/rclcpp/generic_publisher.cpp | 2 +- rclcpp/src/rclcpp/generic_subscription.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/generic_publisher.cpp b/rclcpp/src/rclcpp/generic_publisher.cpp index 7ab0785b8a..456df9133a 100644 --- a/rclcpp/src/rclcpp/generic_publisher.cpp +++ b/rclcpp/src/rclcpp/generic_publisher.cpp @@ -24,7 +24,7 @@ namespace rclcpp void GenericPublisher::publish(const rclcpp::SerializedMessage & message) { TRACETOOLS_TRACEPOINT(rclcpp_publish, - static_cast(publisher_handle_.get()), + nullptr, static_cast(&message.get_rcl_serialized_message())); auto return_code = rcl_publish_serialized_message( get_publisher_handle().get(), &message.get_rcl_serialized_message(), NULL); diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index 4878489826..ae28354b98 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -51,7 +51,7 @@ GenericSubscription::handle_serialized_message( const std::shared_ptr & message, const rclcpp::MessageInfo & message_info) { - any_callback_.dispatch(message,message_info); + any_callback_.dispatch(message, message_info); } void From a8eb620c527bc16b27c21ab4d572203dd366ef38 Mon Sep 17 00:00:00 2001 From: h-suzuki Date: Fri, 8 Mar 2024 02:47:57 +0000 Subject: [PATCH 3/6] Fix: code style Signed-off-by: h-suzuki --- rclcpp/include/rclcpp/generic_subscription.hpp | 1 - rclcpp/src/rclcpp/generic_publisher.cpp | 3 ++- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 76fb73bea8..a1e41dcbab 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -163,7 +163,6 @@ class GenericSubscription : public rclcpp::SubscriptionBase private: RCLCPP_DISABLE_COPY(GenericSubscription) - AnySubscriptionCallback> any_callback_; // The type support library should stay loaded, so it is stored in the GenericSubscription std::shared_ptr ts_lib_; diff --git a/rclcpp/src/rclcpp/generic_publisher.cpp b/rclcpp/src/rclcpp/generic_publisher.cpp index 456df9133a..d526a6fac1 100644 --- a/rclcpp/src/rclcpp/generic_publisher.cpp +++ b/rclcpp/src/rclcpp/generic_publisher.cpp @@ -23,7 +23,8 @@ namespace rclcpp void GenericPublisher::publish(const rclcpp::SerializedMessage & message) { - TRACETOOLS_TRACEPOINT(rclcpp_publish, + TRACETOOLS_TRACEPOINT( + rclcpp_publish, nullptr, static_cast(&message.get_rcl_serialized_message())); auto return_code = rcl_publish_serialized_message( From a028a5421e7c0369b59d9f11f524a91863ede7bd Mon Sep 17 00:00:00 2001 From: h-suzuki-isp <146712054+h-suzuki-isp@users.noreply.github.com> Date: Thu, 14 Mar 2024 14:48:09 +0900 Subject: [PATCH 4/6] Update rclcpp/include/rclcpp/generic_subscription.hpp Co-authored-by: Tomoya Fujita Signed-off-by: h-suzuki-isp <146712054+h-suzuki-isp@users.noreply.github.com> --- rclcpp/include/rclcpp/generic_subscription.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index a1e41dcbab..dd0e8be94d 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -87,7 +87,6 @@ class GenericSubscription : public rclcpp::SubscriptionBase any_callback_(callback), ts_lib_(ts_lib) { - auto callback_ptr = static_cast(&any_callback_); TRACETOOLS_TRACEPOINT( rclcpp_subscription_init, static_cast(get_subscription_handle().get()), @@ -95,7 +94,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase TRACETOOLS_TRACEPOINT( rclcpp_subscription_callback_added, static_cast(this), - callback_ptr); + static_cast(&any_callback_)); #ifndef TRACETOOLS_DISABLED any_callback_.register_callback_for_tracing(); From eae8938ff30c7031b181dc58a9243fd4e3e28337 Mon Sep 17 00:00:00 2001 From: h-suzuki Date: Fri, 15 Mar 2024 02:03:08 +0000 Subject: [PATCH 5/6] Add: tracepoint for take_serialized Signed-off-by: h-suzuki --- rclcpp/src/rclcpp/subscription_base.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index e8510b6444..892663075c 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -244,6 +244,7 @@ SubscriptionBase::take_serialized( &message_out.get_rcl_serialized_message(), &message_info_out.get_rmw_message_info(), nullptr); + TRACETOOLS_TRACEPOINT(rclcpp_take, static_cast(&message_out.get_rcl_serialized_message())); if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { return false; } else if (RCL_RET_OK != ret) { From 7050982430255fdca7c8530fe5bbb115ff2248f4 Mon Sep 17 00:00:00 2001 From: h-suzuki-isp Date: Wed, 27 Mar 2024 10:41:26 +0900 Subject: [PATCH 6/6] Fix: 'whitespace/line_length' errors Signed-off-by: h-suzuki-isp --- rclcpp/src/rclcpp/subscription_base.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 892663075c..f7c238a910 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -244,7 +244,9 @@ SubscriptionBase::take_serialized( &message_out.get_rcl_serialized_message(), &message_info_out.get_rmw_message_info(), nullptr); - TRACETOOLS_TRACEPOINT(rclcpp_take, static_cast(&message_out.get_rcl_serialized_message())); + TRACETOOLS_TRACEPOINT( + rclcpp_take, + static_cast(&message_out.get_rcl_serialized_message())); if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { return false; } else if (RCL_RET_OK != ret) {