Skip to content

Commit 42f28b6

Browse files
leeminju531leeminju531
committed
add : get clients, servers info
Signed-off-by: Minju, Lee <[email protected]>
1 parent 1417a44 commit 42f28b6

File tree

4 files changed

+292
-0
lines changed

4 files changed

+292
-0
lines changed

‎rmw_fastrtps_cpp/src/rmw_get_topic_endpoint_info.cpp‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,4 +43,28 @@ rmw_get_subscriptions_info_by_topic(
4343
return rmw_fastrtps_shared_cpp::__rmw_get_subscriptions_info_by_topic(
4444
eprosima_fastrtps_identifier, node, allocator, topic_name, no_mangle, subscriptions_info);
4545
}
46+
47+
rmw_ret_t
48+
rmw_get_clients_info_by_service(
49+
const rmw_node_t * node,
50+
rcutils_allocator_t * allocator,
51+
const char * service_name,
52+
bool no_mangle,
53+
rmw_topic_endpoint_info_array_t * clients_info)
54+
{
55+
return rmw_fastrtps_shared_cpp::__rmw_get_clients_info_by_service(
56+
eprosima_fastrtps_identifier, node, allocator, service_name, no_mangle, clients_info);
57+
}
58+
59+
rmw_ret_t
60+
rmw_get_servers_info_by_service(
61+
const rmw_node_t * node,
62+
rcutils_allocator_t * allocator,
63+
const char * service_name,
64+
bool no_mangle,
65+
rmw_topic_endpoint_info_array_t * servers_info)
66+
{
67+
return rmw_fastrtps_shared_cpp::__rmw_get_servers_info_by_service(
68+
eprosima_fastrtps_identifier, node, allocator, service_name, no_mangle, servers_info);
69+
}
4670
} // extern "C"

‎rmw_fastrtps_dynamic_cpp/src/rmw_get_topic_endpoint_info.cpp‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,4 +43,28 @@ rmw_get_subscriptions_info_by_topic(
4343
return rmw_fastrtps_shared_cpp::__rmw_get_subscriptions_info_by_topic(
4444
eprosima_fastrtps_identifier, node, allocator, topic_name, no_mangle, subscriptions_info);
4545
}
46+
47+
rmw_ret_t
48+
rmw_get_clients_info_by_service(
49+
const rmw_node_t * node,
50+
rcutils_allocator_t * allocator,
51+
const char * service_name,
52+
bool no_mangle,
53+
rmw_topic_endpoint_info_array_t * clients_info)
54+
{
55+
return rmw_fastrtps_shared_cpp::__rmw_get_clients_info_by_service(
56+
eprosima_fastrtps_identifier, node, allocator, service_name, no_mangle, clients_info);
57+
}
58+
59+
rmw_ret_t
60+
rmw_get_servers_info_by_service(
61+
const rmw_node_t * node,
62+
rcutils_allocator_t * allocator,
63+
const char * service_name,
64+
bool no_mangle,
65+
rmw_topic_endpoint_info_array_t * servers_info)
66+
{
67+
return rmw_fastrtps_shared_cpp::__rmw_get_servers_info_by_service(
68+
eprosima_fastrtps_identifier, node, allocator, service_name, no_mangle, servers_info);
69+
}
4670
} // extern "C"

‎rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -523,6 +523,26 @@ __rmw_get_subscriptions_info_by_topic(
523523
bool no_mangle,
524524
rmw_topic_endpoint_info_array_t * subscriptions_info);
525525

526+
RMW_FASTRTPS_SHARED_CPP_PUBLIC
527+
rmw_ret_t
528+
__rmw_get_clients_info_by_service(
529+
const char * identifier,
530+
const rmw_node_t * node,
531+
rcutils_allocator_t * allocator,
532+
const char * service_name,
533+
bool no_mangle,
534+
rmw_topic_endpoint_info_array_t * clients_info);
535+
536+
RMW_FASTRTPS_SHARED_CPP_PUBLIC
537+
rmw_ret_t
538+
__rmw_get_servers_info_by_service(
539+
const char * identifier,
540+
const rmw_node_t * node,
541+
rcutils_allocator_t * allocator,
542+
const char * service_name,
543+
bool no_mangle,
544+
rmw_topic_endpoint_info_array_t * servers_info);
545+
526546
RMW_FASTRTPS_SHARED_CPP_PUBLIC
527547
rmw_ret_t
528548
__rmw_qos_profile_check_compatible(

‎rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp‎

Lines changed: 224 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -123,4 +123,228 @@ __rmw_get_subscriptions_info_by_topic(
123123
allocator,
124124
subscriptions_info);
125125
}
126+
127+
rmw_ret_t
128+
__rmw_get_clients_info_by_service(
129+
const char * identifier,
130+
const rmw_node_t * node,
131+
rcutils_allocator_t * allocator,
132+
const char * service_name,
133+
bool no_mangle,
134+
rmw_topic_endpoint_info_array_t * clients_info)
135+
{
136+
rmw_ret_t ret = __validate_arguments(
137+
identifier,
138+
node,
139+
allocator,
140+
service_name,
141+
clients_info);
142+
if (ret != RMW_RET_OK) {
143+
return ret;
144+
}
145+
auto common_context = static_cast<rmw_dds_common::Context *>(node->context->impl->common);
146+
std::string mangled_rq_topic_name, mangled_rp_topic_name;
147+
mangled_rq_topic_name = mangled_rp_topic_name = service_name;
148+
DemangleFunction demangle_type = _identity_demangle;
149+
if (!no_mangle) {
150+
mangled_rq_topic_name = \
151+
_mangle_topic_name(ros_service_requester_prefix, service_name, "Request").to_string();
152+
mangled_rp_topic_name = \
153+
_mangle_topic_name(ros_service_response_prefix, service_name, "Reply").to_string();
154+
demangle_type = _demangle_if_ros_type;
155+
}
156+
rmw_topic_endpoint_info_array_t publishers_info = \
157+
rmw_get_zero_initialized_topic_endpoint_info_array();
158+
ret = common_context->graph_cache.get_writers_info_by_topic(
159+
mangled_rq_topic_name,
160+
demangle_type,
161+
allocator,
162+
&publishers_info);
163+
std::unique_ptr<
164+
rmw_topic_endpoint_info_array_t,
165+
std::function<void(rmw_topic_endpoint_info_array_t *)>>
166+
publishers_info_delete_on_error(
167+
&publishers_info,
168+
[allocator](rmw_topic_endpoint_info_array_t * p) {
169+
rmw_ret_t ret = rmw_topic_endpoint_info_array_fini(
170+
p,
171+
allocator
172+
);
173+
if (RMW_RET_OK != ret) {
174+
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy publishers_info when function failed.");
175+
}
176+
}
177+
);
178+
if (RMW_RET_OK != ret) {
179+
return ret;
180+
}
181+
rmw_topic_endpoint_info_array_t subscriptions_info = \
182+
rmw_get_zero_initialized_topic_endpoint_info_array();
183+
ret = common_context->graph_cache.get_readers_info_by_topic(
184+
mangled_rp_topic_name,
185+
demangle_type,
186+
allocator,
187+
&subscriptions_info);
188+
std::unique_ptr<
189+
rmw_topic_endpoint_info_array_t,
190+
std::function<void(rmw_topic_endpoint_info_array_t *)>>
191+
subscriptions_info_delete_on_error(
192+
&subscriptions_info,
193+
[allocator](rmw_topic_endpoint_info_array_t * p) {
194+
rmw_ret_t ret = rmw_topic_endpoint_info_array_fini(
195+
p,
196+
allocator
197+
);
198+
if (RMW_RET_OK != ret) {
199+
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy subscriptions_info when function failed.");
200+
}
201+
}
202+
);
203+
if (RMW_RET_OK != ret) {
204+
return ret;
205+
}
206+
207+
size_t total_size = publishers_info.size + subscriptions_info.size;
208+
ret = rmw_topic_endpoint_info_array_init_with_size(clients_info, total_size, allocator);
209+
std::unique_ptr<
210+
rmw_topic_endpoint_info_array_t,
211+
std::function<void(rmw_topic_endpoint_info_array_t *)>>
212+
clients_info_delete_on_error(
213+
clients_info,
214+
[allocator](rmw_topic_endpoint_info_array_t * p) {
215+
rmw_ret_t ret = rmw_topic_endpoint_info_array_fini(
216+
p,
217+
allocator
218+
);
219+
if (RMW_RET_OK != ret) {
220+
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy clients_info when function failed.");
221+
}
222+
}
223+
);
224+
if (RMW_RET_OK != ret) {
225+
return ret;
226+
}
227+
for (size_t i = 0; i < publishers_info.size; ++i) {
228+
clients_info->info_array[i] = publishers_info.info_array[i];
229+
}
230+
for (size_t i = 0; i < subscriptions_info.size; ++i) {
231+
clients_info->info_array[publishers_info.size + i] = subscriptions_info.info_array[i];
232+
}
233+
publishers_info_delete_on_error.release();
234+
subscriptions_info_delete_on_error.release();
235+
clients_info_delete_on_error.release();
236+
return RMW_RET_OK;
237+
}
238+
239+
rmw_ret_t
240+
__rmw_get_servers_info_by_service(
241+
const char * identifier,
242+
const rmw_node_t * node,
243+
rcutils_allocator_t * allocator,
244+
const char * service_name,
245+
bool no_mangle,
246+
rmw_topic_endpoint_info_array_t * servers_info)
247+
{
248+
rmw_ret_t ret = __validate_arguments(
249+
identifier,
250+
node,
251+
allocator,
252+
service_name,
253+
servers_info);
254+
if (ret != RMW_RET_OK) {
255+
return ret;
256+
}
257+
auto common_context = static_cast<rmw_dds_common::Context *>(node->context->impl->common);
258+
std::string mangled_rq_topic_name, mangled_rp_topic_name;
259+
mangled_rq_topic_name = mangled_rp_topic_name = service_name;
260+
DemangleFunction demangle_type = _identity_demangle;
261+
if (!no_mangle) {
262+
mangled_rq_topic_name = \
263+
_mangle_topic_name(ros_service_requester_prefix, service_name, "Request").to_string();
264+
mangled_rp_topic_name = \
265+
_mangle_topic_name(ros_service_response_prefix, service_name, "Reply").to_string();
266+
demangle_type = _demangle_if_ros_type;
267+
}
268+
rmw_topic_endpoint_info_array_t subscriptions_info = \
269+
rmw_get_zero_initialized_topic_endpoint_info_array();
270+
ret = common_context->graph_cache.get_readers_info_by_topic(
271+
mangled_rq_topic_name,
272+
demangle_type,
273+
allocator,
274+
&subscriptions_info);
275+
std::unique_ptr<
276+
rmw_topic_endpoint_info_array_t,
277+
std::function<void(rmw_topic_endpoint_info_array_t *)>>
278+
subscriptions_info_delete_on_error(
279+
&subscriptions_info,
280+
[allocator](rmw_topic_endpoint_info_array_t * p) {
281+
rmw_ret_t ret = rmw_topic_endpoint_info_array_fini(
282+
p,
283+
allocator
284+
);
285+
if (RMW_RET_OK != ret) {
286+
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy subscriptions_info when function failed.");
287+
}
288+
}
289+
);
290+
if (RMW_RET_OK != ret) {
291+
return ret;
292+
}
293+
rmw_topic_endpoint_info_array_t publishers_info = \
294+
rmw_get_zero_initialized_topic_endpoint_info_array();
295+
ret = common_context->graph_cache.get_writers_info_by_topic(
296+
mangled_rp_topic_name,
297+
demangle_type,
298+
allocator,
299+
&publishers_info);
300+
std::unique_ptr<
301+
rmw_topic_endpoint_info_array_t,
302+
std::function<void(rmw_topic_endpoint_info_array_t *)>>
303+
publishers_info_delete_on_error(
304+
&publishers_info,
305+
[allocator](rmw_topic_endpoint_info_array_t * p) {
306+
rmw_ret_t ret = rmw_topic_endpoint_info_array_fini(
307+
p,
308+
allocator
309+
);
310+
if (RMW_RET_OK != ret) {
311+
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy publishers_info when function failed.");
312+
}
313+
}
314+
);
315+
if (RMW_RET_OK != ret) {
316+
return ret;
317+
}
318+
319+
size_t total_size = publishers_info.size + subscriptions_info.size;
320+
ret = rmw_topic_endpoint_info_array_init_with_size(servers_info, total_size, allocator);
321+
std::unique_ptr<
322+
rmw_topic_endpoint_info_array_t,
323+
std::function<void(rmw_topic_endpoint_info_array_t *)>>
324+
servers_info_delete_on_error(
325+
servers_info,
326+
[allocator](rmw_topic_endpoint_info_array_t * p) {
327+
rmw_ret_t ret = rmw_topic_endpoint_info_array_fini(
328+
p,
329+
allocator
330+
);
331+
if (RMW_RET_OK != ret) {
332+
RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to destroy servers_info when function failed.");
333+
}
334+
}
335+
);
336+
if (RMW_RET_OK != ret) {
337+
return ret;
338+
}
339+
for (size_t i = 0; i < publishers_info.size; ++i) {
340+
servers_info->info_array[i] = publishers_info.info_array[i];
341+
}
342+
for (size_t i = 0; i < subscriptions_info.size; ++i) {
343+
servers_info->info_array[publishers_info.size + i] = subscriptions_info.info_array[i];
344+
}
345+
publishers_info_delete_on_error.release();
346+
subscriptions_info_delete_on_error.release();
347+
servers_info_delete_on_error.release();
348+
return RMW_RET_OK;
349+
}
126350
} // namespace rmw_fastrtps_shared_cpp

0 commit comments

Comments
 (0)