diff --git a/rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp b/rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp index 56fffdad..430929dc 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp @@ -31,6 +31,8 @@ #include "rcutils/types/uint8_array.h" #include "rcpputils/thread_safety_annotations.hpp" +#include "tracetools/tracetools.h" + /****************************************************************************** * General helpers and utilities. ******************************************************************************/ @@ -581,6 +583,9 @@ class RMW_Connext_Client RMW_Connext_Subscriber * reply_sub; std::atomic_uint next_request_id; rmw_context_impl_t * ctx; +#ifndef TRACETOOLS_DISABLED + const rmw_client_t * rmw_client; +#endif // TRACETOOLS_DISABLED RMW_Connext_Client() : request_pub(nullptr), @@ -597,6 +602,9 @@ class RMW_Connext_Client DDS_Publisher * const pub, DDS_Subscriber * const sub, const rosidl_service_type_support_t * const type_supports, +#ifndef TRACETOOLS_DISABLED + const rmw_client_t * const rmw_client, +#endif // TRACETOOLS_DISABLED const char * const svc_name, const rmw_qos_profile_t * const qos_policies); @@ -649,6 +657,9 @@ class RMW_Connext_Service RMW_Connext_Publisher * reply_pub; RMW_Connext_Subscriber * request_sub; rmw_context_impl_t * ctx; +#ifndef TRACETOOLS_DISABLED + const rmw_service_t * rmw_service; +#endif // TRACETOOLS_DISABLED public: static RMW_Connext_Service * @@ -658,6 +669,9 @@ class RMW_Connext_Service DDS_Publisher * const pub, DDS_Subscriber * const sub, const rosidl_service_type_support_t * const type_supports, +#ifndef TRACETOOLS_DISABLED + const rmw_service_t * const rmw_service, +#endif // TRACETOOLS_DISABLED const char * const svc_name, const rmw_qos_profile_t * const qos_policies); diff --git a/rmw_connextdds_common/src/common/rmw_impl.cpp b/rmw_connextdds_common/src/common/rmw_impl.cpp index 3deb1f14..d29a64ec 100644 --- a/rmw_connextdds_common/src/common/rmw_impl.cpp +++ b/rmw_connextdds_common/src/common/rmw_impl.cpp @@ -2411,6 +2411,9 @@ RMW_Connext_Client::create( DDS_Publisher * const pub, DDS_Subscriber * const sub, const rosidl_service_type_support_t * const type_supports, +#ifndef TRACETOOLS_DISABLED + const rmw_client_t * const rmw_client, +#endif // TRACETOOLS_DISABLED const char * const svc_name, const rmw_qos_profile_t * const qos_policies) { @@ -2423,6 +2426,9 @@ RMW_Connext_Client::create( } client_impl->ctx = ctx; +#ifndef TRACETOOLS_DISABLED + client_impl->rmw_client = rmw_client; +#endif // TRACETOOLS_DISABLED auto scope_exit_client_impl_delete = rcpputils::make_scope_exit( [client_impl]() @@ -2692,6 +2698,13 @@ RMW_Connext_Client::take_response( rr_msg.sn) } + TRACETOOLS_TRACEPOINT( + rmw_take_response, + static_cast(this->rmw_client), + static_cast(ros_response), + request_header->request_id.sequence_number, + request_header->source_timestamp, + *taken); return RMW_RET_OK; } @@ -2734,10 +2747,28 @@ RMW_Connext_Client::send_request( return RMW_RET_ERROR; } +#ifndef TRACETOOLS_DISABLED + // In this case, we can get the sequence number before the write() call + if (this->ctx->request_reply_mapping == RMW_Connext_RequestReplyMapping::Basic) { + TRACETOOLS_TRACEPOINT( + rmw_send_request, + static_cast(this->rmw_client), + static_cast(ros_request), + *sequence_id); + } +#endif // TRACETOOLS_DISABLED + rmw_ret_t rc = this->request_pub->write(&rr_msg, false /* serialized */, &write_params); if (this->ctx->request_reply_mapping != RMW_Connext_RequestReplyMapping::Basic) { *sequence_id = write_params.sequence_number; + + // In this other case, we can only get the sequence number after the write() call + TRACETOOLS_TRACEPOINT( + rmw_send_request, + static_cast(this->rmw_client), + static_cast(ros_request), + *sequence_id); } RMW_CONNEXT_LOG_DEBUG_A( @@ -2809,6 +2840,9 @@ RMW_Connext_Service::create( DDS_Publisher * const pub, DDS_Subscriber * const sub, const rosidl_service_type_support_t * const type_supports, +#ifndef TRACETOOLS_DISABLED + const rmw_service_t * const rmw_service, +#endif // TRACETOOLS_DISABLED const char * const svc_name, const rmw_qos_profile_t * const qos_policies) { @@ -2821,6 +2855,9 @@ RMW_Connext_Service::create( } svc_impl->ctx = ctx; +#ifndef TRACETOOLS_DISABLED + svc_impl->rmw_service = rmw_service; +#endif // TRACETOOLS_DISABLED auto scope_exit_svc_impl_delete = rcpputils::make_scope_exit( [svc_impl]() @@ -2998,6 +3035,13 @@ RMW_Connext_Service::take_request( rr_msg.sn) } + TRACETOOLS_TRACEPOINT( + rmw_take_request, + static_cast(this->rmw_service), + static_cast(ros_request), + request_header->request_id.writer_guid, + request_header->request_id.sequence_number, + *taken); return RMW_RET_OK; } @@ -3034,6 +3078,13 @@ RMW_Connext_Service::send_response( reinterpret_cast(rr_msg.gid.data)[2], reinterpret_cast(rr_msg.gid.data)[3], rr_msg.sn) + TRACETOOLS_TRACEPOINT( + rmw_send_response, + static_cast(this->rmw_service), + static_cast(ros_response), + request_id->writer_guid, + request_id->sequence_number, + dds_time_to_u64(&write_params.timestamp)); return this->reply_pub->write(&rr_msg, false /* serialized */, &write_params); } diff --git a/rmw_connextdds_common/src/common/rmw_service.cpp b/rmw_connextdds_common/src/common/rmw_service.cpp index 8dd7fa1e..4413f0ca 100644 --- a/rmw_connextdds_common/src/common/rmw_service.cpp +++ b/rmw_connextdds_common/src/common/rmw_service.cpp @@ -21,6 +21,8 @@ #include "rmw/validate_full_topic_name.h" +#include "tracetools/tracetools.h" + /****************************************************************************** * Clients/Servers functions ******************************************************************************/ @@ -166,6 +168,12 @@ rmw_api_connextdds_create_client( rmw_context_impl_t * ctx = node->context->impl; std::lock_guard guard(ctx->endpoint_mutex); + rmw_client_t * rmw_client = rmw_client_allocate(); + if (nullptr == rmw_client) { + RMW_CONNEXT_LOG_ERROR_SET("failed to create RMW client") + return nullptr; + } + RMW_Connext_Client * const client_impl = RMW_Connext_Client::create( ctx, @@ -173,6 +181,9 @@ rmw_api_connextdds_create_client( ctx->dds_pub, ctx->dds_sub, type_supports, +#ifndef TRACETOOLS_DISABLED + rmw_client, +#endif // TRACETOOLS_DISABLED service_name, &adapted_qos_policies); @@ -191,12 +202,6 @@ rmw_api_connextdds_create_client( delete client_impl; }); - rmw_client_t * rmw_client = rmw_client_allocate(); - if (nullptr == rmw_client) { - RMW_CONNEXT_LOG_ERROR_SET("failed to create RMW client") - return nullptr; - } - rmw_client->implementation_identifier = RMW_CONNEXTDDS_ID; rmw_client->data = client_impl; const size_t svc_name_len = strlen(service_name) + 1; @@ -224,6 +229,10 @@ rmw_api_connextdds_create_client( } scope_exit_client_impl_delete.cancel(); + TRACETOOLS_TRACEPOINT( + rmw_client_init, + static_cast(rmw_client), + client_impl->gid().data); return rmw_client; } @@ -382,6 +391,12 @@ rmw_api_connextdds_create_service( rmw_context_impl_t * ctx = node->context->impl; std::lock_guard guard(ctx->endpoint_mutex); + rmw_service_t * rmw_service = rmw_service_allocate(); + if (nullptr == rmw_service) { + RMW_CONNEXT_LOG_ERROR_SET("failed to create RMW service") + return nullptr; + } + RMW_Connext_Service * const svc_impl = RMW_Connext_Service::create( ctx, @@ -389,6 +404,9 @@ rmw_api_connextdds_create_service( ctx->dds_pub, ctx->dds_sub, type_supports, +#ifndef TRACETOOLS_DISABLED + rmw_service, +#endif // TRACETOOLS_DISABLED service_name, &adapted_qos_policies); @@ -407,12 +425,6 @@ rmw_api_connextdds_create_service( delete svc_impl; }); - rmw_service_t * rmw_service = rmw_service_allocate(); - if (nullptr == rmw_service) { - RMW_CONNEXT_LOG_ERROR_SET("failed to create RMW service") - return nullptr; - } - rmw_service->implementation_identifier = RMW_CONNEXTDDS_ID; rmw_service->data = svc_impl; const size_t svc_name_len = strlen(service_name) + 1;