From 66cdb1f516c4d53902d56f952222635d2d229bd0 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Wed, 9 Oct 2024 15:53:39 -0700 Subject: [PATCH] Add tracing instrumentation using tracetools Signed-off-by: Christophe Bedard --- rmw_zenoh_cpp/CMakeLists.txt | 2 + rmw_zenoh_cpp/package.xml | 1 + rmw_zenoh_cpp/src/detail/rmw_node_data.cpp | 1 + .../src/detail/rmw_publisher_data.cpp | 18 ++++++-- .../src/detail/rmw_publisher_data.hpp | 4 ++ rmw_zenoh_cpp/src/detail/zenoh_utils.cpp | 12 +++-- rmw_zenoh_cpp/src/detail/zenoh_utils.hpp | 3 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 46 +++++++++++++++++-- 8 files changed, 77 insertions(+), 10 deletions(-) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 89b5a598..2163afae 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(rcutils REQUIRED) find_package(rosidl_typesupport_fastrtps_c REQUIRED) find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) find_package(rmw REQUIRED) +find_package(tracetools REQUIRED) find_package(zenoh_c_vendor REQUIRED) add_library(rmw_zenoh_cpp SHARED @@ -68,6 +69,7 @@ target_link_libraries(rmw_zenoh_cpp rosidl_typesupport_fastrtps_c::rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp::rosidl_typesupport_fastrtps_cpp rmw::rmw + tracetools::tracetools zenohc::lib ) diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index e1d2a24a..97695c92 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -25,6 +25,7 @@ rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp rmw + tracetools ament_lint_auto ament_lint_common diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp index cb4817a5..a7fe2c4a 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp @@ -144,6 +144,7 @@ bool NodeData::create_pub_data( auto pub_data = PublisherData::make( std::move(session), + publisher, node_, entity_->node_info(), id_, diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index d1449b63..c58d89a7 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -35,6 +35,8 @@ #include "rmw/get_topic_endpoint_info.h" #include "rmw/impl/cpp/macros.hpp" +#include "tracetools/tracetools.h" + namespace rmw_zenoh_cpp { // TODO(yuyuan): SHM, make this configurable @@ -43,6 +45,7 @@ namespace rmw_zenoh_cpp ///============================================================================= std::shared_ptr PublisherData::make( const z_loaned_session_t * session, + const rmw_publisher_t * const rmw_publisher, const rmw_node_t * const node, liveliness::NodeInfo node_info, std::size_t node_id, @@ -192,6 +195,7 @@ std::shared_ptr PublisherData::make( return std::shared_ptr( new PublisherData{ + rmw_publisher, node, std::move(entity), std::move(pub), @@ -204,6 +208,7 @@ std::shared_ptr PublisherData::make( ///============================================================================= PublisherData::PublisherData( + const rmw_publisher_t * const rmw_publisher, const rmw_node_t * rmw_node, std::shared_ptr entity, z_owned_publisher_t pub, @@ -211,7 +216,8 @@ PublisherData::PublisherData( z_owned_liveliness_token_t token, const void * type_support_impl, std::unique_ptr type_support) -: rmw_node_(rmw_node), +: rmw_publisher_(rmw_publisher), + rmw_node_(rmw_node), entity_(std::move(entity)), pub_(std::move(pub)), pub_cache_(std::move(pub_cache)), @@ -308,9 +314,12 @@ rmw_ret_t PublisherData::publish( z_owned_bytes_t attachment; uint8_t local_gid[RMW_GID_STORAGE_SIZE]; entity_->copy_gid(local_gid); - create_map_and_set_sequence_num(&attachment, sequence_number_++, local_gid); + int64_t source_timestamp = 0; + create_map_and_set_sequence_num(&attachment, sequence_number_++, local_gid, &source_timestamp); options.attachment = z_move(attachment); + TRACETOOLS_TRACEPOINT( + rmw_publish, static_cast(rmw_publisher_), ros_message, source_timestamp); z_owned_bytes_t payload; if (shmbuf.has_value()) { z_bytes_from_shm_mut(&payload, z_move(shmbuf.value())); @@ -357,11 +366,14 @@ rmw_ret_t PublisherData::publish_serialized_message( uint8_t local_gid[RMW_GID_STORAGE_SIZE]; entity_->copy_gid(local_gid); z_owned_bytes_t attachment; - create_map_and_set_sequence_num(&attachment, sequence_number_++, local_gid); + int64_t source_timestamp = 0; + create_map_and_set_sequence_num(&attachment, sequence_number_++, local_gid, &source_timestamp); options.attachment = z_move(attachment); z_owned_bytes_t payload; z_bytes_copy_from_buf(&payload, serialized_message->buffer, data_length); + TRACETOOLS_TRACEPOINT( + rmw_publish, static_cast(rmw_publisher_), serialized_message, source_timestamp); z_result_t res = z_publisher_put(z_loan(pub_), z_move(payload), &options); if (res != Z_OK) { if (res == Z_ESESSION_CLOSED) { diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp index bde0c13b..a3a0a45c 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp @@ -43,6 +43,7 @@ class PublisherData final // Make a shared_ptr of PublisherData. static std::shared_ptr make( const z_loaned_session_t * session, + const rmw_publisher_t * const rmw_publisher, const rmw_node_t * const node, liveliness::NodeInfo node_info, std::size_t node_id, @@ -88,6 +89,7 @@ class PublisherData final private: // Constructor. PublisherData( + const rmw_publisher_t * const rmw_publisher, const rmw_node_t * rmw_node, std::shared_ptr entity, z_owned_publisher_t pub, @@ -98,6 +100,8 @@ class PublisherData final // Internal mutex. mutable std::mutex mutex_; + // The rmw publisher + const rmw_publisher_t * rmw_publisher_; // The parent node. const rmw_node_t * rmw_node_; // The Entity generated for the publisher. diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp index 77bb4dc2..e1148034 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp @@ -26,13 +26,19 @@ namespace rmw_zenoh_cpp { ///============================================================================= void create_map_and_set_sequence_num( - z_owned_bytes_t * out_bytes, int64_t sequence_number, uint8_t gid[RMW_GID_STORAGE_SIZE]) + z_owned_bytes_t * out_bytes, + int64_t sequence_number, + uint8_t gid[RMW_GID_STORAGE_SIZE], + int64_t * source_timestamp) { auto now = std::chrono::system_clock::now().time_since_epoch(); auto now_ns = std::chrono::duration_cast(now); - int64_t source_timestamp = now_ns.count(); + int64_t timestamp = now_ns.count(); + if (nullptr != source_timestamp) { + *source_timestamp = timestamp; + } - AttachmentData data(sequence_number, source_timestamp, gid); + AttachmentData data(sequence_number, timestamp, gid); data.serialize_to_zbytes(out_bytes); } diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp index 5f569082..7226d48c 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp @@ -29,7 +29,8 @@ namespace rmw_zenoh_cpp void create_map_and_set_sequence_num( z_owned_bytes_t * out_bytes, int64_t sequence_number, - uint8_t gid[RMW_GID_STORAGE_SIZE]); + uint8_t gid[RMW_GID_STORAGE_SIZE], + int64_t * source_timestamp = nullptr); ///============================================================================= // A class to store the replies to service requests. diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 1e13a710..2d88fd9c 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -56,6 +56,8 @@ #include "rmw/validate_namespace.h" #include "rmw/validate_node_name.h" +#include "tracetools/tracetools.h" + namespace { //============================================================================== @@ -454,6 +456,14 @@ rmw_create_publisher( free_topic_name.cancel(); free_rmw_publisher.cancel(); + if (TRACETOOLS_TRACEPOINT_ENABLED(rmw_publisher_init)) { + rmw_gid_t gid{}; + // Trigger tracepoint even if we cannot get the GID + rmw_ret_t gid_ret = rmw_get_gid_for_publisher(rmw_publisher, &gid); + static_cast(gid_ret); + TRACETOOLS_DO_TRACEPOINT( + rmw_publisher_init, static_cast(rmw_publisher), gid.data); + } return rmw_publisher; } @@ -990,6 +1000,11 @@ rmw_create_subscription( free_topic_name.cancel(); free_rmw_subscription.cancel(); + // rmw does not require GIDs for subscriptions, and GIDs in rmw_zenoh are not based on any ID of + // the underlying zenoh objects, so there is no need to collect a GID here + rmw_gid_t gid{}; + TRACETOOLS_TRACEPOINT( + rmw_subscription_init, static_cast(rmw_subscription), gid.data); return rmw_subscription; } @@ -1134,7 +1149,18 @@ rmw_take( static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); - return sub_data->take_one_message(ros_message, nullptr, taken); + if (!TRACETOOLS_TRACEPOINT_ENABLED(rmw_take)) { + return sub_data->take_one_message(ros_message, nullptr, taken); + } + rmw_message_info_t message_info{}; + rmw_ret_t ret = sub_data->take_one_message(ros_message, &message_info, taken); + TRACETOOLS_DO_TRACEPOINT( + rmw_take, + static_cast(subscription), + static_cast(ros_message), + message_info.source_timestamp, + *taken); + return ret; } //============================================================================== @@ -1163,7 +1189,14 @@ rmw_take_with_info( static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); - return sub_data->take_one_message(ros_message, message_info, taken); + rmw_ret_t ret = sub_data->take_one_message(ros_message, message_info, taken); + TRACETOOLS_TRACEPOINT( + rmw_take, + static_cast(subscription), + static_cast(ros_message), + message_info->source_timestamp, + *taken); + return ret; } //============================================================================== @@ -1269,11 +1302,18 @@ __rmw_take_serialized( static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); - return sub_data->take_serialized_message( + rmw_ret_t ret = sub_data->take_serialized_message( serialized_message, taken, message_info ); + TRACETOOLS_TRACEPOINT( + rmw_take, + static_cast(subscription), + static_cast(serialized_message), + message_info->source_timestamp, + *taken); + return ret; } } // namespace