From 850358092468386b559e9eb4a9933e24d78f96aa Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Tue, 8 Mar 2022 16:43:58 -0500 Subject: [PATCH] Add pub/sub init, publish and take instrumentation using tracetools Signed-off-by: Christophe Bedard --- rmw_fastrtps_cpp/CMakeLists.txt | 3 +++ rmw_fastrtps_cpp/package.xml | 3 +++ rmw_fastrtps_cpp/src/publisher.cpp | 6 ++++++ rmw_fastrtps_cpp/src/subscription.cpp | 7 +++++++ rmw_fastrtps_shared_cpp/CMakeLists.txt | 3 +++ rmw_fastrtps_shared_cpp/package.xml | 2 ++ rmw_fastrtps_shared_cpp/src/rmw_publish.cpp | 3 +++ rmw_fastrtps_shared_cpp/src/rmw_take.cpp | 8 ++++++++ 8 files changed, 35 insertions(+) diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index ea82908a8..1a9dc4c5e 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -35,6 +35,7 @@ find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(rmw_dds_common REQUIRED) find_package(rmw_fastrtps_shared_cpp REQUIRED) +find_package(tracetools REQUIRED) find_package(fastrtps_cmake_module REQUIRED) find_package(fastcdr REQUIRED CONFIG) @@ -105,6 +106,7 @@ ament_target_dependencies(rmw_fastrtps_cpp "rmw_fastrtps_shared_cpp" "rmw" "rosidl_runtime_c" + "tracetools" ) target_link_libraries(rmw_fastrtps_cpp @@ -130,6 +132,7 @@ ament_export_dependencies(rmw_fastrtps_shared_cpp) ament_export_dependencies(rosidl_runtime_c) ament_export_dependencies(rosidl_typesupport_fastrtps_c) ament_export_dependencies(rosidl_typesupport_fastrtps_cpp) +ament_export_dependencies(tracetools) register_rmw_implementation( "c:rosidl_typesupport_c:rosidl_typesupport_fastrtps_c:rosidl_typesupport_introspection_c" diff --git a/rmw_fastrtps_cpp/package.xml b/rmw_fastrtps_cpp/package.xml index e434ee51e..092130942 100644 --- a/rmw_fastrtps_cpp/package.xml +++ b/rmw_fastrtps_cpp/package.xml @@ -29,6 +29,7 @@ rosidl_runtime_cpp rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp + tracetools fastcdr fastrtps @@ -41,11 +42,13 @@ rosidl_runtime_cpp rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp + tracetools rcpputils rcutils rmw rmw_fastrtps_shared_cpp + tracetools ament_cmake_gtest ament_lint_auto diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 6aaeed6cd..efd27d4d4 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -47,6 +47,8 @@ #include "rmw_fastrtps_cpp/identifier.hpp" #include "rmw_fastrtps_cpp/publisher.hpp" +#include "tracetools/tracetools.h" + #include "type_support_common.hpp" using DataSharingKind = eprosima::fastdds::dds::DataSharingKind; @@ -321,5 +323,9 @@ rmw_fastrtps_cpp::create_publisher( cleanup_datawriter.cancel(); cleanup_info.cancel(); + TRACEPOINT( + rmw_publisher_init, + static_cast(rmw_publisher), + info->publisher_gid.data); return rmw_publisher; } diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index b1f723ca1..e416c1095 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -49,6 +49,8 @@ #include "rmw_fastrtps_cpp/identifier.hpp" #include "rmw_fastrtps_cpp/subscription.hpp" +#include "tracetools/tracetools.h" + #include "type_support_common.hpp" using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; @@ -336,6 +338,11 @@ create_subscription( cleanup_rmw_subscription.cancel(); cleanup_datareader.cancel(); cleanup_info.cancel(); + + TRACEPOINT( + rmw_subscription_init, + static_cast(rmw_subscription), + info->subscription_gid_.data); return rmw_subscription; } } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index a7e19655f..988704e85 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -40,6 +40,7 @@ find_package(rcutils REQUIRED) find_package(rmw_dds_common REQUIRED) find_package(rosidl_typesupport_introspection_c REQUIRED) find_package(rosidl_typesupport_introspection_cpp REQUIRED) +find_package(tracetools REQUIRED) find_package(fastrtps_cmake_module REQUIRED) find_package(fastcdr REQUIRED CONFIG) @@ -109,6 +110,7 @@ ament_target_dependencies(rmw_fastrtps_shared_cpp "rmw_dds_common" "rosidl_typesupport_introspection_c" "rosidl_typesupport_introspection_cpp" + "tracetools" ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -130,6 +132,7 @@ ament_export_dependencies(rmw) ament_export_dependencies(rmw_dds_common) ament_export_dependencies(rosidl_typesupport_introspection_c) ament_export_dependencies(rosidl_typesupport_introspection_cpp) +ament_export_dependencies(tracetools) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rmw_fastrtps_shared_cpp/package.xml b/rmw_fastrtps_shared_cpp/package.xml index cff628d1b..7e134dd53 100644 --- a/rmw_fastrtps_shared_cpp/package.xml +++ b/rmw_fastrtps_shared_cpp/package.xml @@ -24,6 +24,7 @@ rmw_dds_common rosidl_typesupport_introspection_c rosidl_typesupport_introspection_cpp + tracetools fastcdr fastrtps @@ -34,6 +35,7 @@ rmw_dds_common rosidl_typesupport_introspection_c rosidl_typesupport_introspection_cpp + tracetools ament_lint_auto ament_lint_common diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp index a6e07a7c7..c6033a260 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp @@ -24,6 +24,8 @@ #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +#include "tracetools/tracetools.h" + namespace rmw_fastrtps_shared_cpp { rmw_ret_t @@ -55,6 +57,7 @@ __rmw_publish( data.is_cdr_buffer = false; data.data = const_cast(ros_message); data.impl = info->type_support_impl_; + TRACEPOINT(rmw_publish, ros_message); if (!info->data_writer_->write(&data)) { RMW_SET_ERROR_MSG("cannot publish data"); return RMW_RET_ERROR; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 8c7515d81..e4e3dfb6c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -34,6 +34,8 @@ #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/utils.hpp" +#include "tracetools/tracetools.h" + namespace rmw_fastrtps_shared_cpp { @@ -114,6 +116,12 @@ _take( } } + TRACEPOINT( + rmw_take, + static_cast(subscription), + static_cast(ros_message), + (message_info ? message_info->source_timestamp : 0LL), + *taken); return RMW_RET_OK; }