Skip to content

Commit 57bcea1

Browse files
leeminju531leeminju531
committed
add : get clients,servers info
Signed-off-by: Minju, Lee <[email protected]>
1 parent 3086b81 commit 57bcea1

File tree

4 files changed

+314
-0
lines changed

4 files changed

+314
-0
lines changed

‎rmw_connextdds/src/rmw_api_impl_ndds.cpp‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -316,6 +316,32 @@ rmw_get_subscriptions_info_by_topic(
316316
node, allocator, topic_name, no_mangle, subscriptions_info);
317317
}
318318

319+
320+
rmw_ret_t
321+
rmw_get_clients_info_by_service(
322+
const rmw_node_t * node,
323+
rcutils_allocator_t * allocator,
324+
const char * service_name,
325+
bool no_mangle,
326+
rmw_topic_endpoint_info_array_t * clients_info)
327+
{
328+
return rmw_api_connextdds_get_clients_info_by_service(
329+
node, allocator, service_name, no_mangle, clients_info);
330+
}
331+
332+
333+
rmw_ret_t
334+
rmw_get_servers_info_by_service(
335+
const rmw_node_t * node,
336+
rcutils_allocator_t * allocator,
337+
const char * service_name,
338+
bool no_mangle,
339+
rmw_topic_endpoint_info_array_t * servers_info)
340+
{
341+
return rmw_api_connextdds_get_servers_info_by_service(
342+
node, allocator, service_name, no_mangle, servers_info);
343+
}
344+
319345
/*****************************************************************************
320346
* Node API
321347
*****************************************************************************/

‎rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -220,6 +220,24 @@ rmw_api_connextdds_get_subscriptions_info_by_topic(
220220
bool no_mangle,
221221
rmw_topic_endpoint_info_array_t * subscriptions_info);
222222

223+
RMW_CONNEXTDDS_PUBLIC
224+
rmw_ret_t
225+
rmw_api_connextdds_get_clients_info_by_service(
226+
const rmw_node_t * node,
227+
rcutils_allocator_t * allocator,
228+
const char * service_name,
229+
bool no_mangle,
230+
rmw_topic_endpoint_info_array_t * clients_info);
231+
232+
RMW_CONNEXTDDS_PUBLIC
233+
rmw_ret_t
234+
rmw_api_connextdds_get_servers_info_by_service(
235+
const rmw_node_t * node,
236+
rcutils_allocator_t * allocator,
237+
const char * service_name,
238+
bool no_mangle,
239+
rmw_topic_endpoint_info_array_t * servers_info);
240+
223241
/*****************************************************************************
224242
* Node API
225243
*****************************************************************************/

‎rmw_connextdds_common/src/common/rmw_info.cpp‎

Lines changed: 245 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -670,3 +670,248 @@ rmw_api_connextdds_get_subscriptions_info_by_topic(
670670
allocator,
671671
subscriptions_info);
672672
}
673+
674+
675+
rmw_ret_t
676+
rmw_api_connextdds_get_clients_info_by_service(
677+
const rmw_node_t * node,
678+
rcutils_allocator_t * allocator,
679+
const char * service_name,
680+
bool no_mangle,
681+
rmw_topic_endpoint_info_array_t * clients_info)
682+
{
683+
RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
684+
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
685+
node,
686+
node->implementation_identifier,
687+
RMW_CONNEXTDDS_ID,
688+
return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
689+
RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT);
690+
RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT);
691+
RMW_CHECK_ARGUMENT_FOR_NULL(clients_info, RMW_RET_INVALID_ARGUMENT);
692+
693+
RCUTILS_CHECK_ALLOCATOR_WITH_MSG(
694+
allocator, "allocator argument is invalid", return RMW_RET_INVALID_ARGUMENT);
695+
696+
if (RMW_RET_OK != rmw_topic_endpoint_info_array_check_zero(clients_info)) {
697+
return RMW_RET_INVALID_ARGUMENT;
698+
}
699+
700+
auto common_context = &node->context->impl->common;
701+
std::string mangled_rq_topic_name, mangled_rp_topic_name;
702+
mangled_rq_topic_name = mangled_rp_topic_name = service_name;
703+
DemangleFunction demangle_type = _identity_demangle;
704+
if (!no_mangle) {
705+
mangled_rq_topic_name =
706+
rmw_connextdds_create_topic_name(
707+
ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false);
708+
mangled_rp_topic_name =
709+
rmw_connextdds_create_topic_name(
710+
ROS_SERVICE_RESPONSE_PREFIX, service_name, "Reply", false);
711+
demangle_type = _demangle_if_ros_type;
712+
}
713+
rmw_topic_endpoint_info_array_t publishers_info = \
714+
rmw_get_zero_initialized_topic_endpoint_info_array();
715+
rmw_ret_t ret = common_context->graph_cache.get_writers_info_by_topic(
716+
mangled_rq_topic_name,
717+
demangle_type,
718+
allocator,
719+
&publishers_info);
720+
std::unique_ptr<
721+
rmw_topic_endpoint_info_array_t,
722+
std::function<void(rmw_topic_endpoint_info_array_t *)>>
723+
publishers_info_delete_on_error(
724+
&publishers_info,
725+
[allocator](rmw_topic_endpoint_info_array_t * p) {
726+
rmw_ret_t ret = rmw_topic_endpoint_info_array_fini(
727+
p,
728+
allocator
729+
);
730+
if (RMW_RET_OK != ret) {
731+
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy publishers_info when function failed.");
732+
}
733+
}
734+
);
735+
if (RMW_RET_OK != ret) {
736+
return ret;
737+
}
738+
rmw_topic_endpoint_info_array_t subscriptions_info = \
739+
rmw_get_zero_initialized_topic_endpoint_info_array();
740+
ret = common_context->graph_cache.get_readers_info_by_topic(
741+
mangled_rp_topic_name,
742+
demangle_type,
743+
allocator,
744+
&subscriptions_info);
745+
std::unique_ptr<
746+
rmw_topic_endpoint_info_array_t,
747+
std::function<void(rmw_topic_endpoint_info_array_t *)>>
748+
subscriptions_info_delete_on_error(
749+
&subscriptions_info,
750+
[allocator](rmw_topic_endpoint_info_array_t * p) {
751+
rmw_ret_t ret = rmw_topic_endpoint_info_array_fini(
752+
p,
753+
allocator
754+
);
755+
if (RMW_RET_OK != ret) {
756+
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy subscriptions_info when function failed.");
757+
}
758+
}
759+
);
760+
if (RMW_RET_OK != ret) {
761+
return ret;
762+
}
763+
764+
size_t total_size = publishers_info.size + subscriptions_info.size;
765+
ret = rmw_topic_endpoint_info_array_init_with_size(clients_info, total_size, allocator);
766+
std::unique_ptr<
767+
rmw_topic_endpoint_info_array_t,
768+
std::function<void(rmw_topic_endpoint_info_array_t *)>>
769+
clients_info_delete_on_error(
770+
clients_info,
771+
[allocator](rmw_topic_endpoint_info_array_t * p) {
772+
rmw_ret_t ret = rmw_topic_endpoint_info_array_fini(
773+
p,
774+
allocator
775+
);
776+
if (RMW_RET_OK != ret) {
777+
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy clients_info when function failed.");
778+
}
779+
}
780+
);
781+
if (RMW_RET_OK != ret) {
782+
return ret;
783+
}
784+
for (size_t i = 0; i < publishers_info.size; ++i) {
785+
clients_info->info_array[i] = publishers_info.info_array[i];
786+
}
787+
for (size_t i = 0; i < subscriptions_info.size; ++i) {
788+
clients_info->info_array[publishers_info.size + i] = subscriptions_info.info_array[i];
789+
}
790+
publishers_info_delete_on_error.release();
791+
subscriptions_info_delete_on_error.release();
792+
clients_info_delete_on_error.release();
793+
return RMW_RET_OK;
794+
}
795+
796+
797+
rmw_ret_t
798+
rmw_api_connextdds_get_servers_info_by_service(
799+
const rmw_node_t * node,
800+
rcutils_allocator_t * allocator,
801+
const char * service_name,
802+
bool no_mangle,
803+
rmw_topic_endpoint_info_array_t * servers_info)
804+
{
805+
RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
806+
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
807+
node,
808+
node->implementation_identifier,
809+
RMW_CONNEXTDDS_ID,
810+
return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
811+
RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT);
812+
RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT);
813+
RMW_CHECK_ARGUMENT_FOR_NULL(servers_info, RMW_RET_INVALID_ARGUMENT);
814+
815+
RCUTILS_CHECK_ALLOCATOR_WITH_MSG(
816+
allocator, "allocator argument is invalid", return RMW_RET_INVALID_ARGUMENT);
817+
818+
if (RMW_RET_OK != rmw_topic_endpoint_info_array_check_zero(servers_info)) {
819+
return RMW_RET_INVALID_ARGUMENT;
820+
}
821+
822+
auto common_context = &node->context->impl->common;
823+
std::string mangled_rq_topic_name, mangled_rp_topic_name;
824+
mangled_rq_topic_name = mangled_rp_topic_name = service_name;
825+
DemangleFunction demangle_type = _identity_demangle;
826+
if (!no_mangle) {
827+
mangled_rq_topic_name =
828+
rmw_connextdds_create_topic_name(
829+
ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", false);
830+
mangled_rp_topic_name =
831+
rmw_connextdds_create_topic_name(
832+
ROS_SERVICE_RESPONSE_PREFIX, service_name, "Reply", false);
833+
demangle_type = _demangle_if_ros_type;
834+
}
835+
836+
rmw_topic_endpoint_info_array_t subscriptions_info = \
837+
rmw_get_zero_initialized_topic_endpoint_info_array();
838+
rmw_ret_t ret = common_context->graph_cache.get_readers_info_by_topic(
839+
mangled_rq_topic_name,
840+
demangle_type,
841+
allocator,
842+
&subscriptions_info);
843+
std::unique_ptr<
844+
rmw_topic_endpoint_info_array_t,
845+
std::function<void(rmw_topic_endpoint_info_array_t *)>>
846+
subscriptions_info_delete_on_error(
847+
&subscriptions_info,
848+
[allocator](rmw_topic_endpoint_info_array_t * p) {
849+
rmw_ret_t ret = rmw_topic_endpoint_info_array_fini(
850+
p,
851+
allocator
852+
);
853+
if (RMW_RET_OK != ret) {
854+
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy subscriptions_info when function failed.");
855+
}
856+
}
857+
);
858+
if (RMW_RET_OK != ret) {
859+
return ret;
860+
}
861+
rmw_topic_endpoint_info_array_t publishers_info = \
862+
rmw_get_zero_initialized_topic_endpoint_info_array();
863+
ret = common_context->graph_cache.get_writers_info_by_topic(
864+
mangled_rp_topic_name,
865+
demangle_type,
866+
allocator,
867+
&publishers_info);
868+
std::unique_ptr<
869+
rmw_topic_endpoint_info_array_t,
870+
std::function<void(rmw_topic_endpoint_info_array_t *)>>
871+
publishers_info_delete_on_error(
872+
&publishers_info,
873+
[allocator](rmw_topic_endpoint_info_array_t * p) {
874+
rmw_ret_t ret = rmw_topic_endpoint_info_array_fini(
875+
p,
876+
allocator
877+
);
878+
if (RMW_RET_OK != ret) {
879+
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy publishers_info when function failed.");
880+
}
881+
}
882+
);
883+
if (RMW_RET_OK != ret) {
884+
return ret;
885+
}
886+
887+
size_t total_size = publishers_info.size + subscriptions_info.size;
888+
ret = rmw_topic_endpoint_info_array_init_with_size(servers_info, total_size, allocator);
889+
std::unique_ptr<
890+
rmw_topic_endpoint_info_array_t,
891+
std::function<void(rmw_topic_endpoint_info_array_t *)>>
892+
servers_info_delete_on_error(
893+
servers_info,
894+
[allocator](rmw_topic_endpoint_info_array_t * p) {
895+
rmw_ret_t ret = rmw_topic_endpoint_info_array_fini(
896+
p,
897+
allocator
898+
);
899+
if (RMW_RET_OK != ret) {
900+
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy servers_info when function failed.");
901+
}
902+
}
903+
);
904+
if (RMW_RET_OK != ret) {
905+
return ret;
906+
}
907+
for (size_t i = 0; i < publishers_info.size; ++i) {
908+
servers_info->info_array[i] = publishers_info.info_array[i];
909+
}
910+
for (size_t i = 0; i < subscriptions_info.size; ++i) {
911+
servers_info->info_array[publishers_info.size + i] = subscriptions_info.info_array[i];
912+
}
913+
publishers_info_delete_on_error.release();
914+
subscriptions_info_delete_on_error.release();
915+
servers_info_delete_on_error.release();
916+
return RMW_RET_OK;
917+
}

‎rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -318,6 +318,31 @@ rmw_get_subscriptions_info_by_topic(
318318
node, allocator, topic_name, no_mangle, subscriptions_info);
319319
}
320320

321+
322+
rmw_ret_t
323+
rmw_get_clients_info_by_service(
324+
const rmw_node_t * node,
325+
rcutils_allocator_t * allocator,
326+
const char * service_name,
327+
bool no_mangle,
328+
rmw_topic_endpoint_info_array_t * clients_info)
329+
{
330+
return rmw_api_connextdds_get_clients_info_by_service(
331+
node, allocator, service_name, no_mangle, clients_info);
332+
}
333+
334+
335+
rmw_ret_t
336+
rmw_get_servers_info_by_service(
337+
const rmw_node_t * node,
338+
rcutils_allocator_t * allocator,
339+
const char * service_name,
340+
bool no_mangle,
341+
rmw_topic_endpoint_info_array_t * servers_info)
342+
{
343+
return rmw_api_connextdds_get_servers_info_by_service(
344+
node, allocator, service_name, no_mangle, servers_info);
345+
}
321346
/*****************************************************************************
322347
* Node API
323348
*****************************************************************************/

0 commit comments

Comments
 (0)