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