Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions ros2cli/ros2cli/daemon/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,8 @@ def serve(server: LocalXMLRPCServer, *, timeout: int = 2 * 60 * 60):
node.get_subscriber_names_and_types_by_node,
node.get_subscriptions_info_by_topic,
node.get_service_names_and_types_by_node,
node.get_servers_info_by_service,
node.get_clients_info_by_service,
node.get_client_names_and_types_by_node,
bind(rclpy.action.get_action_server_names_and_types_by_node, node),
bind(rclpy.action.get_action_client_names_and_types_by_node, node),
Expand Down
20 changes: 13 additions & 7 deletions ros2cli/ros2cli/xmlrpc/marshal/rclpy.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@
from xmlrpc.client import Unmarshaller

import rclpy.duration
import rclpy.endpoint_info
import rclpy.qos
import rclpy.topic_endpoint_info
import rclpy.type_hash

from .generic import dump_any_enum
Expand Down Expand Up @@ -66,16 +66,22 @@ def dump_duration(marshaller, value, write):
Marshaller.dispatch[enum_type] = dump_any_enum


Unmarshaller.dispatch[fullname(rclpy.topic_endpoint_info.TopicEndpointInfo)] = \
functools.partial(end_any_with_slots, type_=rclpy.topic_endpoint_info.TopicEndpointInfo)
Unmarshaller.dispatch[fullname(rclpy.endpoint_info.TopicEndpointInfo)] = \
functools.partial(end_any_with_slots, type_=rclpy.endpoint_info.TopicEndpointInfo)

Marshaller.dispatch[rclpy.topic_endpoint_info.TopicEndpointInfo] = \
Marshaller.dispatch[rclpy.endpoint_info.TopicEndpointInfo] = \
functools.partial(dump_any_with_slots, transform=lambda slot: slot.lstrip('_'))

Unmarshaller.dispatch[fullname(rclpy.topic_endpoint_info.TopicEndpointTypeEnum)] = \
functools.partial(end_any_enum, enum_=rclpy.topic_endpoint_info.TopicEndpointTypeEnum)
Unmarshaller.dispatch[fullname(rclpy.endpoint_info.ServiceEndpointInfo)] = \
functools.partial(end_any_with_slots, type_=rclpy.endpoint_info.ServiceEndpointInfo)

Marshaller.dispatch[rclpy.topic_endpoint_info.TopicEndpointTypeEnum] = dump_any_enum
Marshaller.dispatch[rclpy.endpoint_info.ServiceEndpointInfo] = \
functools.partial(dump_any_with_slots, transform=lambda slot: slot.lstrip('_'))

Unmarshaller.dispatch[fullname(rclpy.endpoint_info.EndpointTypeEnum)] = \
functools.partial(end_any_enum, enum_=rclpy.endpoint_info.EndpointTypeEnum)

Marshaller.dispatch[rclpy.endpoint_info.EndpointTypeEnum] = dump_any_enum


def end_type_hash(unmarshaller, data):
Expand Down
45 changes: 43 additions & 2 deletions ros2cli/test/test_ros2cli_daemon.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@

import rclpy
import rclpy.action
from rclpy.endpoint_info import EndpointTypeEnum

from ros2cli.node.daemon import DaemonNode
from ros2cli.node.daemon import is_daemon_running
Expand Down Expand Up @@ -45,6 +46,8 @@
history=rclpy.qos.HistoryPolicy.KEEP_LAST,
depth=8
)
TEST_SERVICE_CLIENT_QOS = TEST_TOPIC_PUBLISHER_QOS
TEST_SERVICE_SERVER_QOS = TEST_TOPIC_SUBSCRIPTION_QOS
TEST_SERVICE_NAME = '/test/service'
TEST_SERVICE_TYPE = 'test_msgs/srv/Empty'
TEST_ACTION_NAME = '/test/action'
Expand Down Expand Up @@ -73,12 +76,14 @@ def local_node():
service = node.create_service(
srv_type=test_msgs.srv.Empty,
srv_name=TEST_SERVICE_NAME,
callback=(lambda req, res: res)
callback=(lambda req, res: res),
qos_profile=TEST_SERVICE_SERVER_QOS
)
service # to avoid "assigned by never used" warning
client = node.create_client(
srv_type=test_msgs.srv.Empty,
srv_name=TEST_SERVICE_NAME
srv_name=TEST_SERVICE_NAME,
qos_profile=TEST_SERVICE_CLIENT_QOS
)
client # to avoid "assigned by never used" warning

Expand Down Expand Up @@ -235,6 +240,42 @@ def test_get_subscriptions_info_by_topic(daemon_node):
TEST_TOPIC_SUBSCRIPTION_QOS.depth


def test_get_clients_info_by_service(daemon_node):
clients_info = daemon_node.get_clients_info_by_service(TEST_SERVICE_NAME)
assert len(clients_info) == 1
test_client_info = clients_info[0]
assert test_client_info.node_name == TEST_NODE_NAME
assert test_client_info.node_namespace == TEST_NODE_NAMESPACE
assert test_client_info.service_type == TEST_SERVICE_TYPE
assert test_client_info.endpoint_type == EndpointTypeEnum.CLIENT
assert (test_client_info.endpoint_count == 1 or test_client_info.endpoint_count == 2)
assert len(test_client_info.qos_profiles) == test_client_info.endpoint_count
assert len(test_client_info.endpoint_gids) == test_client_info.endpoint_count
for i in range(test_client_info.endpoint_count):
assert test_client_info.qos_profiles[i].durability == \
TEST_SERVICE_CLIENT_QOS.durability
assert test_client_info.qos_profiles[i].reliability == \
TEST_SERVICE_CLIENT_QOS.reliability


def test_get_servers_info_by_service(daemon_node):
servers_info = daemon_node.get_servers_info_by_service(TEST_SERVICE_NAME)
assert len(servers_info) == 1
test_server_info = servers_info[0]
assert test_server_info.node_name == TEST_NODE_NAME
assert test_server_info.node_namespace == TEST_NODE_NAMESPACE
assert test_server_info.service_type == TEST_SERVICE_TYPE
assert test_server_info.endpoint_type == EndpointTypeEnum.SERVER
assert (test_server_info.endpoint_count == 1 or test_server_info.endpoint_count == 2)
assert len(test_server_info.qos_profiles) == test_server_info.endpoint_count
assert len(test_server_info.endpoint_gids) == test_server_info.endpoint_count
for i in range(test_server_info.endpoint_count):
assert test_server_info.qos_profiles[i].durability == \
TEST_SERVICE_SERVER_QOS.durability
assert test_server_info.qos_profiles[i].reliability == \
TEST_SERVICE_SERVER_QOS.reliability


def test_count_publishers(daemon_node):
assert 1 == daemon_node.count_publishers(TEST_TOPIC_NAME)

Expand Down
27 changes: 27 additions & 0 deletions ros2service/ros2service/verb/info.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
# Copyright 2022 CLOBOT Co., Ltd.
# Copyright 2025 Minju Lee (이민주). All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -27,6 +28,13 @@ def add_arguments(self, parser, cli_name):
arg = parser.add_argument(
'service_name',
help="Name of the ROS service to get info (e.g. '/add_two_ints')")
parser.add_argument(
'--verbose',
'-v',
action='store_true',
help='Prints detailed information like the node name, node namespace, service type, '
'service type hash, GUIDs, and QoS Profiless of the clients and servers to '
'this service')
arg.completer = ServiceNameCompleter(
include_hidden_services_key='include_hidden_services')

Expand All @@ -44,11 +52,30 @@ def main(self, *, args):
else:
return "Unknown service '%s'" % service_name

line_end = '\n'
if args.verbose:
line_end = '\n\n'

type_str = service_types[0] if len(service_types) == 1 else service_types
print('Type: %s' % type_str, end='\n')

print('Clients count: %d' %
node.count_clients(service_name), end='\n')

if args.verbose:
try:
for info in node.get_clients_info_by_service(service_name):
print(info, end=line_end)

except NotImplementedError as e:
return str(e)

print('Services count: %d' %
node.count_services(service_name), end='\n')
if args.verbose:
try:
for info in node.get_servers_info_by_service(service_name):
print(info, end=line_end)

except NotImplementedError as e:
return str(e)