From 9087c872d7668bff336b6417d4031e7cd2813089 Mon Sep 17 00:00:00 2001 From: "Minju, Lee" Date: Sun, 22 Jun 2025 09:04:54 +0900 Subject: [PATCH 1/2] Add rmw_get_service_endpoint_info and update graph cache entity to support service type hash Signed-off-by: Minju, Lee --- rmw_connextdds/src/rmw_api_impl_ndds.cpp | 26 +++ .../include/rmw_connextdds/dds_api.hpp | 12 +- .../include/rmw_connextdds/graph_cache.hpp | 6 +- .../include/rmw_connextdds/rmw_api_impl.hpp | 18 ++ .../include/rmw_connextdds/rmw_impl.hpp | 9 +- .../src/common/rmw_graph.cpp | 51 +++-- rmw_connextdds_common/src/common/rmw_impl.cpp | 39 +++- rmw_connextdds_common/src/common/rmw_info.cpp | 203 ++++++++++++++++++ .../src/common/rmw_service.cpp | 6 +- .../src/ndds/dds_api_ndds.cpp | 22 +- .../src/rtime/dds_api_rtime.cpp | 22 +- .../src/rmw_api_impl_rtime.cpp | 27 +++ 12 files changed, 388 insertions(+), 53 deletions(-) diff --git a/rmw_connextdds/src/rmw_api_impl_ndds.cpp b/rmw_connextdds/src/rmw_api_impl_ndds.cpp index 13ddce63..5067f3fa 100644 --- a/rmw_connextdds/src/rmw_api_impl_ndds.cpp +++ b/rmw_connextdds/src/rmw_api_impl_ndds.cpp @@ -16,6 +16,7 @@ #include "rmw/event.h" #include "rmw/get_node_info_and_types.h" +#include "rmw/get_service_endpoint_info.h" #include "rmw/get_service_names_and_types.h" #include "rmw/get_topic_endpoint_info.h" #include "rmw/get_topic_names_and_types.h" @@ -323,6 +324,31 @@ rmw_get_subscriptions_info_by_topic( node, allocator, topic_name, no_mangle, subscriptions_info); } +rmw_ret_t +rmw_get_clients_info_by_service( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * service_name, + bool no_mangle, + rmw_service_endpoint_info_array_t * clients_info) +{ + return rmw_api_connextdds_get_clients_info_by_service( + node, allocator, service_name, no_mangle, clients_info); +} + + +rmw_ret_t +rmw_get_servers_info_by_service( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * service_name, + bool no_mangle, + rmw_service_endpoint_info_array_t * servers_info) +{ + return rmw_api_connextdds_get_servers_info_by_service( + node, allocator, service_name, no_mangle, servers_info); +} + /***************************************************************************** * Node API *****************************************************************************/ diff --git a/rmw_connextdds_common/include/rmw_connextdds/dds_api.hpp b/rmw_connextdds_common/include/rmw_connextdds/dds_api.hpp index 35e37090..a39c81d8 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/dds_api.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/dds_api.hpp @@ -108,7 +108,8 @@ rmw_connextdds_get_datawriter_qos( DDS_Topic * const topic, DDS_DataWriterQos * const qos, const rmw_qos_profile_t * const qos_policies, - const rmw_publisher_options_t * const pub_options); + const rmw_publisher_options_t * const pub_options, + const rosidl_type_hash_t * ser_type_hash = nullptr); rmw_ret_t rmw_connextdds_get_datareader_qos( @@ -117,7 +118,8 @@ rmw_connextdds_get_datareader_qos( DDS_TopicDescription * const topic_desc, DDS_DataReaderQos * const qos, const rmw_qos_profile_t * const qos_policies, - const rmw_subscription_options_t * const sub_options); + const rmw_subscription_options_t * const sub_options, + const rosidl_type_hash_t * ser_type_hash = nullptr); DDS_DataWriter * rmw_connextdds_create_datawriter( @@ -129,7 +131,8 @@ rmw_connextdds_create_datawriter( const bool internal, RMW_Connext_MessageTypeSupport * const type_support, DDS_Topic * const topic, - DDS_DataWriterQos * const dw_qos); + DDS_DataWriterQos * const dw_qos, + const rosidl_type_hash_t * ser_type_hash = nullptr); DDS_DataReader * rmw_connextdds_create_datareader( @@ -141,7 +144,8 @@ rmw_connextdds_create_datareader( const bool internal, RMW_Connext_MessageTypeSupport * const type_support, DDS_TopicDescription * const topic_desc, - DDS_DataReaderQos * const dr_qos); + DDS_DataReaderQos * const dr_qos, + const rosidl_type_hash_t * ser_type_hash = nullptr); rmw_ret_t rmw_connextdds_write_message( diff --git a/rmw_connextdds_common/include/rmw_connextdds/graph_cache.hpp b/rmw_connextdds_common/include/rmw_connextdds/graph_cache.hpp index 4a2b01be..8d66e63c 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/graph_cache.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/graph_cache.hpp @@ -70,7 +70,8 @@ rmw_ret_t rmw_connextdds_graph_on_service_created( rmw_context_impl_t * const ctx, const rmw_node_t * const node, - RMW_Connext_Service * const svc); + RMW_Connext_Service * const svc, + const rosidl_type_hash_t * ser_type_hash); rmw_ret_t rmw_connextdds_graph_on_service_deleted( @@ -82,7 +83,8 @@ rmw_ret_t rmw_connextdds_graph_on_client_created( rmw_context_impl_t * const ctx, const rmw_node_t * const node, - RMW_Connext_Client * const client); + RMW_Connext_Client * const client, + const rosidl_type_hash_t * ser_type_hash); rmw_ret_t rmw_connextdds_graph_on_client_deleted( diff --git a/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp b/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp index fce5981c..7cc723bd 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp @@ -224,6 +224,24 @@ rmw_api_connextdds_get_subscriptions_info_by_topic( bool no_mangle, rmw_topic_endpoint_info_array_t * subscriptions_info); +RMW_CONNEXTDDS_PUBLIC +rmw_ret_t +rmw_api_connextdds_get_clients_info_by_service( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * service_name, + bool no_mangle, + rmw_service_endpoint_info_array_t * clients_info); + +RMW_CONNEXTDDS_PUBLIC +rmw_ret_t +rmw_api_connextdds_get_servers_info_by_service( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * service_name, + bool no_mangle, + rmw_service_endpoint_info_array_t * servers_info); + /***************************************************************************** * Node API *****************************************************************************/ diff --git a/rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp b/rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp index 2deeb24b..81974c2d 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp @@ -152,7 +152,8 @@ class RMW_Connext_Publisher const RMW_Connext_MessageType msg_type = RMW_CONNEXT_MESSAGE_USERDATA, const void * const intro_members = nullptr, const bool intro_members_cpp = false, - std::string * const type_name = nullptr); + std::string * const type_name = nullptr, + const rosidl_type_hash_t * ser_type_hash = nullptr); rmw_ret_t finalize(); @@ -417,7 +418,8 @@ class RMW_Connext_Subscriber std::string * const type_name = nullptr, const char * const cft_name = nullptr, const char * const cft_filter = nullptr, - RMW_Connext_Publisher * const related_pub = nullptr); + RMW_Connext_Publisher * const related_pub = nullptr, + const rosidl_type_hash_t * ser_type_hash = nullptr); rmw_ret_t finalize(); @@ -978,7 +980,8 @@ rmw_connextdds_get_readerwriter_qos( DDS_UserDataQosPolicy * const user_data, const rmw_qos_profile_t * const qos_policies, const rmw_publisher_options_t * const pub_options, - const rmw_subscription_options_t * const sub_options); + const rmw_subscription_options_t * const sub_options, + const rosidl_type_hash_t * ser_type_hash = nullptr); rmw_ret_t rmw_connextdds_readerwriter_qos_to_ros( diff --git a/rmw_connextdds_common/src/common/rmw_graph.cpp b/rmw_connextdds_common/src/common/rmw_graph.cpp index 8793e7af..c52604ea 100644 --- a/rmw_connextdds_common/src/common/rmw_graph.cpp +++ b/rmw_connextdds_common/src/common/rmw_graph.cpp @@ -41,7 +41,8 @@ rmw_connextdds_graph_add_entityEA( const DDS_LivelinessQosPolicy * const liveliness, const DDS_LifespanQosPolicy * const lifespan, const bool is_reader, - const bool local); + const bool local, + const rosidl_type_hash_t * ser_type_hash = nullptr); static rmw_ret_t rmw_connextdds_graph_remove_entityEA( @@ -53,13 +54,15 @@ static rmw_ret_t rmw_connextdds_graph_add_local_publisherEA( rmw_context_impl_t * const ctx, const rmw_node_t * const node, - RMW_Connext_Publisher * const pub); + RMW_Connext_Publisher * const pub, + const rosidl_type_hash_t * ser_type_hash = nullptr); static rmw_ret_t rmw_connextdds_graph_add_local_subscriberEA( rmw_context_impl_t * const ctx, const rmw_node_t * const node, - RMW_Connext_Subscriber * const sub); + RMW_Connext_Subscriber * const sub, + const rosidl_type_hash_t * ser_type_hash = nullptr); rmw_ret_t rmw_connextdds_graph_initialize(rmw_context_impl_t * const ctx) @@ -431,7 +434,8 @@ rmw_ret_t rmw_connextdds_graph_on_service_created( rmw_context_impl_t * const ctx, const rmw_node_t * const node, - RMW_Connext_Service * const svc) + RMW_Connext_Service * const svc, + const rosidl_type_hash_t * ser_type_hash) { std::lock_guard guard(ctx->common_mutex); const rmw_gid_t pub_gid = *svc->publisher()->gid(), @@ -454,14 +458,14 @@ rmw_connextdds_graph_on_service_created( }); rmw_ret_t rc = rmw_connextdds_graph_add_local_subscriberEA( - ctx, node, svc->subscriber()); + ctx, node, svc->subscriber(), ser_type_hash); if (RMW_RET_OK != rc) { return rc; } // set it so that it can be removed in the `scope_exit_entities_reset` added_sub = true; - rc = rmw_connextdds_graph_add_local_publisherEA(ctx, node, svc->publisher()); + rc = rmw_connextdds_graph_add_local_publisherEA(ctx, node, svc->publisher(), ser_type_hash); if (RMW_RET_OK != rc) { return rc; } @@ -509,7 +513,8 @@ rmw_ret_t rmw_connextdds_graph_on_client_created( rmw_context_impl_t * const ctx, const rmw_node_t * const node, - RMW_Connext_Client * const client) + RMW_Connext_Client * const client, + const rosidl_type_hash_t * ser_type_hash) { std::lock_guard guard(ctx->common_mutex); const rmw_gid_t pub_gid = *client->publisher()->gid(), @@ -532,13 +537,13 @@ rmw_connextdds_graph_on_client_created( }); rmw_ret_t rc = rmw_connextdds_graph_add_local_subscriberEA( - ctx, node, client->subscriber()); + ctx, node, client->subscriber(), ser_type_hash); if (RMW_RET_OK != rc) { return rc; } added_sub = true; - rc = rmw_connextdds_graph_add_local_publisherEA(ctx, node, client->publisher()); + rc = rmw_connextdds_graph_add_local_publisherEA(ctx, node, client->publisher(), ser_type_hash); if (RMW_RET_OK != rc) { return rc; } @@ -666,7 +671,8 @@ rmw_connextdds_graph_add_entityEA( const DDS_LivelinessQosPolicy * const liveliness, const DDS_LifespanQosPolicy * const lifespan, const bool is_reader, - const bool local) + const bool local, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(local); rmw_gid_t gid; @@ -723,7 +729,8 @@ rmw_connextdds_graph_add_entityEA( type_hash, dp_gid, qos_profile, - is_reader)) + is_reader, + ser_type_hash)) { // This is downgraded to a debug message because we might // enter this path when asserting entities from discovery. @@ -755,7 +762,8 @@ rmw_ret_t rmw_connextdds_graph_add_local_publisherEA( rmw_context_impl_t * const ctx, const rmw_node_t * const node, - RMW_Connext_Publisher * const pub) + RMW_Connext_Publisher * const pub, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(node); RMW_CONNEXT_LOG_DEBUG_A( @@ -820,14 +828,16 @@ rmw_connextdds_graph_add_local_publisherEA( nullptr /* Micro doesn't support LifespanQosPolicy */, #endif /* RMW_CONNEXT_DDS_API */ false /* is_reader */, - true /* local */); + true /* local */, + ser_type_hash); } rmw_ret_t rmw_connextdds_graph_add_local_subscriberEA( rmw_context_impl_t * const ctx, const rmw_node_t * const node, - RMW_Connext_Subscriber * const sub) + RMW_Connext_Subscriber * const sub, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(node); RMW_CONNEXT_LOG_DEBUG_A( @@ -888,7 +898,8 @@ rmw_connextdds_graph_add_local_subscriberEA( &dr_qos.liveliness, nullptr /* Lifespan is a writer-only qos policy */, true /* is_reader */, - true /* local */); + true /* local */, + ser_type_hash); } rmw_ret_t @@ -932,6 +943,13 @@ rmw_connextdds_graph_add_remote_entity( // We handled the error, so clear it out rmw_reset_error(); } + rosidl_type_hash_t ser_type_hash; + rosidl_type_hash_t * ser_type_hash_ptr = nullptr; + if (RMW_RET_OK == rmw_dds_common::parse_sertype_hash_from_user_data( + reinterpret_cast(user_data_data), user_data_size, ser_type_hash)) + { + ser_type_hash_ptr = &ser_type_hash; + } rmw_ret_t rc = rmw_connextdds_graph_add_entityEA( ctx, @@ -947,7 +965,8 @@ rmw_connextdds_graph_add_remote_entity( liveliness, lifespan, is_reader, - false /* local */); + false /* local */, + ser_type_hash_ptr); if (RMW_RET_OK != rc) { return rc; } diff --git a/rmw_connextdds_common/src/common/rmw_impl.cpp b/rmw_connextdds_common/src/common/rmw_impl.cpp index d0e5a456..94b99d0e 100644 --- a/rmw_connextdds_common/src/common/rmw_impl.cpp +++ b/rmw_connextdds_common/src/common/rmw_impl.cpp @@ -316,7 +316,8 @@ rmw_connextdds_get_readerwriter_qos( DDS_UserDataQosPolicy * const user_data, const rmw_qos_profile_t * const qos_policies, const rmw_publisher_options_t * const pub_options, - const rmw_subscription_options_t * const sub_options) + const rmw_subscription_options_t * const sub_options, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(writer_qos); UNUSED_ARG(publish_mode); @@ -474,6 +475,14 @@ rmw_connextdds_get_readerwriter_qos( // We handled the error, so clear it out rmw_reset_error(); } + if (ser_type_hash) { + std::string typehash_str; + if (RMW_RET_OK == rmw_dds_common::encode_sertype_hash_for_user_data_qos( + *ser_type_hash, typehash_str)) + { + user_data_str += typehash_str; + } + } DDS_OctetSeq_from_array( &user_data->value, reinterpret_cast(user_data_str.c_str()), @@ -693,7 +702,8 @@ RMW_Connext_Publisher::create( const RMW_Connext_MessageType msg_type, const void * const intro_members, const bool intro_members_cpp, - std::string * const type_name) + std::string * const type_name, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(internal); @@ -812,7 +822,8 @@ RMW_Connext_Publisher::create( internal, type_support, topic, - &dw_qos); + &dw_qos, + ser_type_hash); if (nullptr == dds_writer) { RMW_CONNEXT_LOG_ERROR("failed to create DDS writer") @@ -1314,7 +1325,8 @@ RMW_Connext_Subscriber::create( std::string * const type_name, const char * const cft_name, const char * const cft_filter, - RMW_Connext_Publisher * const related_pub) + RMW_Connext_Publisher * const related_pub, + const rosidl_type_hash_t * ser_type_hash) { RMW_Connext_MessageTypeSupport * const type_support = RMW_Connext_MessageTypeSupport::register_type_support( @@ -1474,7 +1486,8 @@ RMW_Connext_Subscriber::create( internal, type_support, sub_topic, - &dr_qos); + &dr_qos, + ser_type_hash); if (nullptr == dds_reader) { RMW_CONNEXT_LOG_ERROR_SET("failed to create DDS reader") @@ -2592,7 +2605,7 @@ RMW_Connext_Client::create( rmw_publisher_options_t pub_options = rmw_get_default_publisher_options(); rmw_subscription_options_t sub_options = rmw_get_default_subscription_options(); - + const rosidl_type_hash_t * ser_type_hash = type_supports->get_type_hash_func(type_supports); RMW_CONNEXT_LOG_DEBUG_A( "creating request publisher: " @@ -2614,7 +2627,8 @@ RMW_Connext_Client::create( RMW_CONNEXT_MESSAGE_REQUEST, svc_members_req, svc_members_req_cpp, - &request_type); + &request_type, + ser_type_hash); if (nullptr == client_impl->request_pub) { RMW_CONNEXT_LOG_ERROR("failed to create client requester") @@ -2666,7 +2680,9 @@ RMW_Connext_Client::create( svc_members_res_cpp, &reply_type, cft_name, - cft_filter); + cft_filter, + nullptr, + ser_type_hash); if (nullptr == client_impl->reply_sub) { RMW_CONNEXT_LOG_ERROR("failed to create client replier") @@ -3025,6 +3041,7 @@ RMW_Connext_Service::create( rmw_publisher_options_t pub_options = rmw_get_default_publisher_options(); rmw_subscription_options_t sub_options = rmw_get_default_subscription_options(); + const rosidl_type_hash_t * ser_type_hash = type_supports->get_type_hash_func(type_supports); RMW_CONNEXT_LOG_DEBUG_A( "creating reply publisher: " @@ -3046,7 +3063,8 @@ RMW_Connext_Service::create( RMW_CONNEXT_MESSAGE_REPLY, svc_members_res, svc_members_res_cpp, - &reply_type); + &reply_type, + ser_type_hash); if (nullptr == svc_impl->reply_pub) { RMW_CONNEXT_LOG_ERROR("failed to create service replier") @@ -3079,7 +3097,8 @@ RMW_Connext_Service::create( /* If we are using the extended RPC mapping, then we cache the reply writer so that we can notify it of "subscription_match" events. */ (ctx->request_reply_mapping == RMW_Connext_RequestReplyMapping::Extended ? - svc_impl->reply_pub : nullptr)); + svc_impl->reply_pub : nullptr), + ser_type_hash); if (nullptr == svc_impl->request_sub) { RMW_CONNEXT_LOG_ERROR("failed to create service requester") diff --git a/rmw_connextdds_common/src/common/rmw_info.cpp b/rmw_connextdds_common/src/common/rmw_info.cpp index 0418acf9..9f76918d 100644 --- a/rmw_connextdds_common/src/common/rmw_info.cpp +++ b/rmw_connextdds_common/src/common/rmw_info.cpp @@ -670,3 +670,206 @@ rmw_api_connextdds_get_subscriptions_info_by_topic( allocator, subscriptions_info); } + + +rmw_ret_t +rmw_api_connextdds_get_clients_info_by_service( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * service_name, + bool no_mangle, + rmw_service_endpoint_info_array_t * clients_info) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + RMW_CONNEXTDDS_ID, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(clients_info, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "allocator argument is invalid", return RMW_RET_INVALID_ARGUMENT); + + if (RMW_RET_OK != rmw_service_endpoint_info_array_check_zero(clients_info)) { + return RMW_RET_INVALID_ARGUMENT; + } + if (no_mangle) { + // Services in DDS require mangled topic names + // because they internally use separate readers and writers. + // Therefore, this function cannot support the 'no_mangle' option. + // If user need to query raw topic information without mangling, + // use`rmw_get_publishers_info_by_topic` or `rmw_get_subscriptions_info_by_topic` instead. + RMW_SET_ERROR_MSG( + "'no_mangle' is not supported for services" + " because they rely on internally mangled topic names.\n" + "Use 'rmw_get_publishers_info_by_topic' or 'rmw_get_subscriptions_info_by_topic'" + " instead to access unmangled topic information."); + return RMW_RET_INVALID_ARGUMENT; + } + + auto common_context = &node->context->impl->common; + std::string mangled_rq_topic_name = \ + rmw_connextdds_create_topic_name(ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false); + std::string mangled_rp_topic_name = \ + rmw_connextdds_create_topic_name(ROS_SERVICE_RESPONSE_PREFIX, service_name, "Reply", false); + DemangleFunction demangle_type = _demangle_service_type_only; + + rmw_topic_endpoint_info_array_t subscriptions_info = + rmw_get_zero_initialized_topic_endpoint_info_array(); + std::unique_ptr< + rmw_topic_endpoint_info_array_t, + std::function> + subscriptions_info_delete( + &subscriptions_info, + [allocator](rmw_topic_endpoint_info_array_t * p) { + rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( + p, + allocator + ); + if (RMW_RET_OK != ret) { + RMW_SET_ERROR_MSG("Failed to destroy subscriptions_info when function ended."); + } + } + ); + rmw_ret_t ret = common_context->graph_cache.get_readers_info_by_topic( + mangled_rp_topic_name, + demangle_type, + allocator, + &subscriptions_info); + if(RMW_RET_OK != ret) { + return ret; + } + + rmw_topic_endpoint_info_array_t publishers_info = + rmw_get_zero_initialized_topic_endpoint_info_array(); + std::unique_ptr< + rmw_topic_endpoint_info_array_t, + std::function> + publishers_info_delete( + &publishers_info, + [allocator](rmw_topic_endpoint_info_array_t * p) { + rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( + p, + allocator + ); + if (RMW_RET_OK != ret) { + RMW_SET_ERROR_MSG("Failed to destroy publishers_info when function ended."); + } + } + ); + ret = common_context->graph_cache.get_writers_info_by_topic( + mangled_rq_topic_name, + demangle_type, + allocator, + &publishers_info); + if(RMW_RET_OK != ret) { + return ret; + } + return common_context->graph_cache.get_clients_info_by_service( + &subscriptions_info, + &publishers_info, + allocator, + clients_info); +} + + +rmw_ret_t +rmw_api_connextdds_get_servers_info_by_service( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * service_name, + bool no_mangle, + rmw_service_endpoint_info_array_t * servers_info) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + RMW_CONNEXTDDS_ID, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(servers_info, RMW_RET_INVALID_ARGUMENT); + + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "allocator argument is invalid", return RMW_RET_INVALID_ARGUMENT); + + if (no_mangle) { + // Services in DDS require mangled topic names + // because they internally use separate readers and writers. + // Therefore, this function cannot support the 'no_mangle' option. + // If user need to query raw topic information without mangling, + // use`rmw_get_publishers_info_by_topic` or `rmw_get_subscriptions_info_by_topic` instead. + RMW_SET_ERROR_MSG( + "'no_mangle' is not supported for services" + " because they rely on internally mangled topic names.\n" + "Use 'rmw_get_publishers_info_by_topic' or 'rmw_get_subscriptions_info_by_topic'" + " instead to access unmangled topic information."); + return RMW_RET_INVALID_ARGUMENT; + } + auto common_context = &node->context->impl->common; + std::string mangled_rq_topic_name = \ + rmw_connextdds_create_topic_name(ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false); + std::string mangled_rp_topic_name = \ + rmw_connextdds_create_topic_name(ROS_SERVICE_RESPONSE_PREFIX, service_name, "Reply", false); + DemangleFunction demangle_type = _demangle_service_type_only; + + rmw_topic_endpoint_info_array_t subscriptions_info = + rmw_get_zero_initialized_topic_endpoint_info_array(); + std::unique_ptr< + rmw_topic_endpoint_info_array_t, + std::function> + subscriptions_info_delete( + &subscriptions_info, + [allocator](rmw_topic_endpoint_info_array_t * p) { + rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( + p, + allocator + ); + if (RMW_RET_OK != ret) { + RMW_SET_ERROR_MSG("Failed to destroy subscriptions_info when function failed."); + } + } + ); + rmw_ret_t ret = common_context->graph_cache.get_readers_info_by_topic( + mangled_rq_topic_name, + demangle_type, + allocator, + &subscriptions_info); + if(RMW_RET_OK != ret) { + return ret; + } + + rmw_topic_endpoint_info_array_t publishers_info = + rmw_get_zero_initialized_topic_endpoint_info_array(); + std::unique_ptr< + rmw_topic_endpoint_info_array_t, + std::function> + publishers_info_delete( + &publishers_info, + [allocator](rmw_topic_endpoint_info_array_t * p) { + rmw_ret_t ret = rmw_topic_endpoint_info_array_fini( + p, + allocator + ); + if (RMW_RET_OK != ret) { + RMW_SET_ERROR_MSG("Failed to destroy publishers_info when function failed."); + } + } + ); + ret = common_context->graph_cache.get_writers_info_by_topic( + mangled_rp_topic_name, + demangle_type, + allocator, + &publishers_info); + if(RMW_RET_OK != ret) { + return ret; + } + return common_context->graph_cache.get_servers_info_by_service( + &subscriptions_info, + &publishers_info, + allocator, + servers_info); +} diff --git a/rmw_connextdds_common/src/common/rmw_service.cpp b/rmw_connextdds_common/src/common/rmw_service.cpp index 60dd134e..ccbb1b22 100644 --- a/rmw_connextdds_common/src/common/rmw_service.cpp +++ b/rmw_connextdds_common/src/common/rmw_service.cpp @@ -220,7 +220,8 @@ rmw_api_connextdds_create_client( } if (RMW_RET_OK != - rmw_connextdds_graph_on_client_created(ctx, node, client_impl)) + rmw_connextdds_graph_on_client_created( + ctx, node, client_impl, type_supports->get_type_hash_func(type_supports))) { RMW_CONNEXT_LOG_ERROR("failed to update graph for client") return nullptr; @@ -441,7 +442,8 @@ rmw_api_connextdds_create_service( } if (RMW_RET_OK != - rmw_connextdds_graph_on_service_created(ctx, node, svc_impl)) + rmw_connextdds_graph_on_service_created( + ctx, node, svc_impl, type_supports->get_type_hash_func(type_supports))) { RMW_CONNEXT_LOG_ERROR("failed to update graph for service") return nullptr; diff --git a/rmw_connextdds_common/src/ndds/dds_api_ndds.cpp b/rmw_connextdds_common/src/ndds/dds_api_ndds.cpp index e1d91c20..62c3568d 100644 --- a/rmw_connextdds_common/src/ndds/dds_api_ndds.cpp +++ b/rmw_connextdds_common/src/ndds/dds_api_ndds.cpp @@ -485,7 +485,8 @@ rmw_connextdds_get_datawriter_qos( DDS_Topic * const topic, DDS_DataWriterQos * const qos, const rmw_qos_profile_t * const qos_policies, - const rmw_publisher_options_t * const pub_options) + const rmw_publisher_options_t * const pub_options, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(topic); @@ -515,7 +516,8 @@ rmw_connextdds_get_datawriter_qos( &qos->user_data, qos_policies, pub_options, - nullptr /* sub_options */)) + nullptr /* sub_options */, + ser_type_hash)) { return RMW_RET_ERROR; } @@ -601,7 +603,8 @@ rmw_connextdds_get_datareader_qos( DDS_TopicDescription * const topic_desc, DDS_DataReaderQos * const qos, const rmw_qos_profile_t * const qos_policies, - const rmw_subscription_options_t * const sub_options) + const rmw_subscription_options_t * const sub_options, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(ctx); UNUSED_ARG(topic_desc); @@ -631,7 +634,8 @@ rmw_connextdds_get_datareader_qos( &qos->user_data, qos_policies, nullptr /* pub_options */, - sub_options)) + sub_options, + ser_type_hash)) { return RMW_RET_ERROR; } @@ -684,7 +688,8 @@ rmw_connextdds_create_datawriter( const bool internal, RMW_Connext_MessageTypeSupport * const type_support, DDS_Topic * const topic, - DDS_DataWriterQos * const dw_qos) + DDS_DataWriterQos * const dw_qos, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(ctx); UNUSED_ARG(participant); @@ -692,7 +697,7 @@ rmw_connextdds_create_datawriter( if (RMW_RET_OK != rmw_connextdds_get_datawriter_qos( - ctx, type_support, topic, dw_qos, qos_policies, publisher_options)) + ctx, type_support, topic, dw_qos, qos_policies, publisher_options, ser_type_hash)) { RMW_CONNEXT_LOG_ERROR("failed to convert writer QoS") return nullptr; @@ -716,7 +721,8 @@ rmw_connextdds_create_datareader( const bool internal, RMW_Connext_MessageTypeSupport * const type_support, DDS_TopicDescription * const topic_desc, - DDS_DataReaderQos * const dr_qos) + DDS_DataReaderQos * const dr_qos, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(ctx); UNUSED_ARG(participant); @@ -724,7 +730,7 @@ rmw_connextdds_create_datareader( if (RMW_RET_OK != rmw_connextdds_get_datareader_qos( - ctx, type_support, topic_desc, dr_qos, qos_policies, subscriber_options)) + ctx, type_support, topic_desc, dr_qos, qos_policies, subscriber_options, ser_type_hash)) { RMW_CONNEXT_LOG_ERROR("failed to convert reader QoS") return nullptr; diff --git a/rmw_connextdds_common/src/rtime/dds_api_rtime.cpp b/rmw_connextdds_common/src/rtime/dds_api_rtime.cpp index 332a8fc5..1c1445e5 100644 --- a/rmw_connextdds_common/src/rtime/dds_api_rtime.cpp +++ b/rmw_connextdds_common/src/rtime/dds_api_rtime.cpp @@ -1008,7 +1008,8 @@ rmw_connextdds_get_datawriter_qos( DDS_Topic * const topic, DDS_DataWriterQos * const qos, const rmw_qos_profile_t * const qos_policies, - const rmw_publisher_options_t * const pub_options) + const rmw_publisher_options_t * const pub_options, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(ctx); UNUSED_ARG(topic); @@ -1028,7 +1029,8 @@ rmw_connextdds_get_datawriter_qos( &qos->user_data, qos_policies, pub_options, - nullptr /* sub_options */)) + nullptr /* sub_options */, + ser_type_hash)) { return RMW_RET_ERROR; } @@ -1058,7 +1060,8 @@ rmw_connextdds_get_datareader_qos( DDS_TopicDescription * const topic_desc, DDS_DataReaderQos * const qos, const rmw_qos_profile_t * const qos_policies, - const rmw_subscription_options_t * const sub_options) + const rmw_subscription_options_t * const sub_options, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(ctx); UNUSED_ARG(topic_desc); @@ -1078,7 +1081,8 @@ rmw_connextdds_get_datareader_qos( &qos->user_data, qos_policies, nullptr /* pub_options */, - sub_options)) + sub_options, + ser_type_hash)) { return RMW_RET_ERROR; } @@ -1110,7 +1114,8 @@ rmw_connextdds_create_datawriter( const bool internal, RMW_Connext_MessageTypeSupport * const type_support, DDS_Topic * const topic, - DDS_DataWriterQos * const dw_qos) + DDS_DataWriterQos * const dw_qos, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(ctx); UNUSED_ARG(participant); @@ -1118,7 +1123,7 @@ rmw_connextdds_create_datawriter( if (RMW_RET_OK != rmw_connextdds_get_datawriter_qos( - ctx, type_support, topic, dw_qos, qos_policies, publisher_options)) + ctx, type_support, topic, dw_qos, qos_policies, publisher_options, ser_type_hash)) { RMW_CONNEXT_LOG_ERROR("failed to convert writer QoS") return nullptr; @@ -1141,7 +1146,8 @@ rmw_connextdds_create_datareader( const bool internal, RMW_Connext_MessageTypeSupport * const type_support, DDS_TopicDescription * const topic_desc, - DDS_DataReaderQos * const dr_qos) + DDS_DataReaderQos * const dr_qos, + const rosidl_type_hash_t * ser_type_hash) { UNUSED_ARG(ctx); UNUSED_ARG(participant); @@ -1149,7 +1155,7 @@ rmw_connextdds_create_datareader( if (RMW_RET_OK != rmw_connextdds_get_datareader_qos( - ctx, type_support, topic_desc, dr_qos, qos_policies, subscriber_options)) + ctx, type_support, topic_desc, dr_qos, qos_policies, subscriber_options, ser_type_hash)) { RMW_CONNEXT_LOG_ERROR("failed to convert reader QoS") return nullptr; diff --git a/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp b/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp index 5197ef76..452de736 100644 --- a/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp +++ b/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp @@ -16,6 +16,7 @@ #include "rmw/event.h" #include "rmw/get_node_info_and_types.h" +#include "rmw/get_service_endpoint_info.h" #include "rmw/get_service_names_and_types.h" #include "rmw/get_topic_endpoint_info.h" #include "rmw/get_topic_names_and_types.h" @@ -324,6 +325,32 @@ rmw_get_subscriptions_info_by_topic( node, allocator, topic_name, no_mangle, subscriptions_info); } + +rmw_ret_t +rmw_get_clients_info_by_service( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * service_name, + bool no_mangle, + rmw_service_endpoint_info_array_t * clients_info) +{ + return rmw_api_connextdds_get_clients_info_by_service( + node, allocator, service_name, no_mangle, clients_info); +} + + +rmw_ret_t +rmw_get_servers_info_by_service( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * service_name, + bool no_mangle, + rmw_service_endpoint_info_array_t * servers_info) +{ + return rmw_api_connextdds_get_servers_info_by_service( + node, allocator, service_name, no_mangle, servers_info); +} + /***************************************************************************** * Node API *****************************************************************************/ From d242434e1ef1473cdeaf86aff403230cf3221f0a Mon Sep 17 00:00:00 2001 From: "Minju, Lee" Date: Sun, 22 Jun 2025 09:05:44 +0900 Subject: [PATCH 2/2] Fix rmw_api_connextdds_count_services Signed-off-by: Minju, Lee --- rmw_connextdds_common/src/common/rmw_info.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rmw_connextdds_common/src/common/rmw_info.cpp b/rmw_connextdds_common/src/common/rmw_info.cpp index 9f76918d..d94c9cb3 100644 --- a/rmw_connextdds_common/src/common/rmw_info.cpp +++ b/rmw_connextdds_common/src/common/rmw_info.cpp @@ -354,11 +354,11 @@ rmw_api_connextdds_count_services( } auto common_context = &node->context->impl->common; - const std::string mangled_rp_service_name = + const std::string mangled_rq_service_name = rmw_connextdds_create_topic_name( - ROS_SERVICE_RESPONSE_PREFIX, service_name, "Reply", false); - return common_context->graph_cache.get_writer_count( - mangled_rp_service_name, count); + ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false); + return common_context->graph_cache.get_reader_count( + mangled_rq_service_name, count); }