From de83a74814f8686d9a740d903b4f11da7c336178 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 3 Jul 2024 02:55:37 +0800 Subject: [PATCH] Implement rmw_publisher_assert_liveliness Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 4d9b9e9b..3e4660f1 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -1166,8 +1166,21 @@ rmw_get_serialized_message_size( rmw_ret_t rmw_publisher_assert_liveliness(const rmw_publisher_t * publisher) { - static_cast(publisher); - return RMW_RET_UNSUPPORTED; + RMW_CHECK_FOR_NULL_WITH_MSG( + publisher, "publisher handle is null", + return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + publisher->data, "publisher data is null", + return RMW_RET_INVALID_ARGUMENT); + rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = + static_cast(publisher->data); + RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); + + if (!zc_liveliness_token_check(&pub_data->token)) { + return RMW_RET_ERROR; + } + + return RMW_RET_OK; } //============================================================================== @@ -3798,6 +3811,7 @@ rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = static_cast(publisher->data); + RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; memcpy(gid->data, pub_data->pub_gid, RMW_GID_STORAGE_SIZE);