Skip to content

Commit 7dfeb4a

Browse files
committed
ros2doctor: add and test node/parameter reporting
- Add and update tests: test_node.py, test_parameter.py, test_api.py, common.py Signed-off-by: BhuvanB404<bhuvanb1408@gmail.com> Signed-off-by: BhuvanB404 <bhuvanb1408@gmail.com>
1 parent 5622c13 commit 7dfeb4a

File tree

8 files changed

+689
-49
lines changed

8 files changed

+689
-49
lines changed

ros2doctor/ros2doctor/api/node.py

Lines changed: 9 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
# limitations under the License.
1414

1515

16-
from collections import defaultdict
16+
from collections import Counter
1717

1818
from ros2cli.node.strategy import NodeStrategy
1919
from ros2doctor.api import DoctorCheck
@@ -24,9 +24,10 @@
2424
from ros2node.api import get_node_names
2525

2626

27-
def has_duplicates(values):
28-
"""Find out if there are any exact duplicates in a list of strings."""
29-
return len(set(values)) < len(values)
27+
def find_duplicates(values):
28+
"""Return values that appear more than once."""
29+
counts = Counter(values)
30+
return [v for v, c in counts.items() if c > 1]
3031

3132

3233
class NodeCheck(DoctorCheck):
@@ -40,15 +41,10 @@ def check(self):
4041
with NodeStrategy(None) as node:
4142
node_list = get_node_names(node=node, include_hidden_nodes=True)
4243
node_names = [n.full_name for n in node_list]
43-
if has_duplicates(node_names):
44-
name_counts = defaultdict(int)
45-
for name in node_names:
46-
name_counts[name] += 1
47-
48-
duplicates = [name for name, count in name_counts.items() if count > 1]
49-
for duplicate in duplicates:
50-
doctor_warn(f'Duplicate node name: {duplicate}')
51-
result.add_warning()
44+
duplicates = find_duplicates(node_names)
45+
for duplicate in duplicates:
46+
doctor_warn(f'Duplicate node name: {duplicate}')
47+
result.add_warning()
5248
return result
5349

5450

ros2doctor/ros2doctor/api/parameter.py

Lines changed: 63 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -13,8 +13,10 @@
1313
# limitations under the License.
1414

1515

16-
from rcl_interfaces.srv import ListParameters
1716
import rclpy
17+
from rclpy.parameter import parameter_value_to_python
18+
from rclpy.parameter_client import AsyncParameterClient
19+
1820
from ros2cli.node.direct import DirectNode
1921
from ros2cli.node.strategy import NodeStrategy
2022
from ros2doctor.api import DoctorCheck
@@ -24,33 +26,40 @@
2426
from ros2doctor.api.format import doctor_warn
2527

2628

27-
def call_list_parameters(node, node_name, namespace='/'):
29+
def call_list_parameters(node, node_name):
2830
"""Call the list_parameters service for a specific node."""
2931
try:
30-
# Create service name and client for the target node's list_parameters service
31-
service_name = f"{namespace.rstrip('/')}/{node_name}/list_parameters"
32-
if service_name.startswith('//'):
33-
service_name = service_name[1:]
34-
35-
client = node.create_client(ListParameters, service_name)
36-
37-
if not client.wait_for_service(timeout_sec=1.0):
32+
client = AsyncParameterClient(node, node_name)
33+
ready = client.wait_for_services(timeout_sec=2.0)
34+
if not ready:
3835
return None
36+
future = client.list_parameters()
37+
rclpy.spin_until_future_complete(node, future, timeout_sec=2.0)
38+
if future.done() and future.result() is not None:
39+
# Return the parameter names list
40+
return future.result().result.names
41+
return None
42+
except Exception:
43+
return None
3944

40-
request = ListParameters.Request()
41-
future = client.call_async(request)
4245

43-
# Spin until the service call completes or times out
44-
rclpy.spin_until_future_complete(node, future, timeout_sec=2.0)
46+
def call_get_parameters(node, node_name, parameter_names):
47+
"""
48+
Call the get_parameters service for a specific node.
4549
46-
if future.done():
47-
response = future.result()
48-
node.destroy_client(client)
49-
return response
50-
else:
51-
node.destroy_client(client)
50+
Return the parameter values.
51+
"""
52+
try:
53+
client = AsyncParameterClient(node, node_name)
54+
ready = client.wait_for_services(timeout_sec=2.0)
55+
if not ready:
5256
return None
53-
57+
future = client.get_parameters(parameter_names)
58+
rclpy.spin_until_future_complete(node, future, timeout_sec=2.0)
59+
if future.done() and future.result() is not None:
60+
# Return the parameter values list
61+
return future.result().values
62+
return None
5463
except Exception:
5564
return None
5665

@@ -65,15 +74,18 @@ def check(self):
6574
result = Result()
6675
with NodeStrategy(None) as node:
6776
try:
68-
node_names_and_namespaces = node.get_node_names_and_namespaces()
77+
node_names_and_namespaces = \
78+
node.get_node_names_and_namespaces()
6979
except Exception:
7080
node_names_and_namespaces = []
7181
with DirectNode(None) as param_node:
7282
for node_name, namespace in node_names_and_namespaces:
73-
response = call_list_parameters(param_node.node, node_name, namespace)
74-
if response is None:
75-
full_name = f"{namespace.rstrip('/')}/{node_name}"
76-
doctor_warn(f'Node {full_name} has no parameter services.')
83+
full_name = f"{namespace.rstrip('/')}/{node_name}"
84+
param_names = call_list_parameters(
85+
param_node.node, full_name)
86+
if param_names is None:
87+
doctor_warn(
88+
f'Node {full_name} has no parameter services.')
7789
result.add_warning()
7890
return result
7991

@@ -88,7 +100,8 @@ def report(self):
88100
report = Report('PARAMETER LIST')
89101
with NodeStrategy(None) as node:
90102
try:
91-
node_names_and_namespaces = node.get_node_names_and_namespaces()
103+
node_names_and_namespaces = \
104+
node.get_node_names_and_namespaces()
92105
except Exception:
93106
node_names_and_namespaces = []
94107
if not node_names_and_namespaces:
@@ -103,15 +116,29 @@ def report(self):
103116
with DirectNode(None) as param_node:
104117
for node_name, namespace in sorted(node_names_and_namespaces):
105118
nodes_checked += 1
106-
response = call_list_parameters(param_node.node, node_name, namespace)
107-
if response and hasattr(response, 'result') and response.result:
108-
result = response.result
109-
param_names = result.names if hasattr(result, 'names') else []
110-
if param_names:
111-
total_param_count += len(param_names)
112-
full_name = f"{namespace.rstrip('/')}/{node_name}"
113-
114-
report.add_to_report('node', full_name)
119+
full_name = f"{namespace.rstrip('/')}/{node_name}"
120+
param_names = call_list_parameters(
121+
param_node.node, full_name)
122+
123+
if param_names:
124+
total_param_count += len(param_names)
125+
report.add_to_report('node', full_name)
126+
param_values = call_get_parameters(
127+
param_node.node, full_name, param_names
128+
)
129+
if (
130+
param_values and
131+
len(param_values) == len(param_names)
132+
):
133+
params_with_values = sorted(
134+
zip(param_names, param_values),
135+
key=lambda x: x[0]
136+
)
137+
for name, value_msg in params_with_values:
138+
value = parameter_value_to_python(value_msg)
139+
report.add_to_report(
140+
'parameter', f'{name}: {value}')
141+
else:
115142
for param_name in sorted(param_names):
116143
report.add_to_report('parameter', param_name)
117144

ros2doctor/test/common.py

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,3 +33,32 @@ def generate_expected_service_report(services: Iterable[str], serv_counts: Itera
3333
expected_report.add_to_report('service count', serv_count)
3434
expected_report.add_to_report('client count', cli_count)
3535
return expected_report
36+
37+
38+
def generate_expected_node_report(node_count: int, nodes: Iterable[str]) -> Report:
39+
expected_report = Report('NODE LIST')
40+
if node_count == 0:
41+
expected_report.add_to_report('node count', 0)
42+
expected_report.add_to_report('node', 'none')
43+
else:
44+
expected_report.add_to_report('node count', node_count)
45+
for node in nodes:
46+
expected_report.add_to_report('node', node)
47+
return expected_report
48+
49+
50+
def generate_expected_parameter_report(nodes_checked: int, param_count: int,
51+
node_params: Iterable[tuple]) -> Report:
52+
expected_report = Report('PARAMETER LIST')
53+
if nodes_checked == 0:
54+
expected_report.add_to_report('total nodes checked', 0)
55+
expected_report.add_to_report('total parameter count', 0)
56+
expected_report.add_to_report('parameter', 'none')
57+
else:
58+
for node_name, params in node_params:
59+
expected_report.add_to_report('node', node_name)
60+
for param in params:
61+
expected_report.add_to_report('parameter', param)
62+
expected_report.add_to_report('total nodes checked', nodes_checked)
63+
expected_report.add_to_report('total parameter count', param_count)
64+
return expected_report
Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
# Copyright 2019 Open Source Robotics Foundation, Inc.
2+
# Copyright 2026 Open Source Robotics Foundation, Inc.
3+
#
4+
# Licensed under the Apache License, Version 2.0 (the "License");
5+
# you may not use this file except in compliance with the License.
6+
# You may obtain a copy of the License at
7+
#
8+
# http://www.apache.org/licenses/LICENSE-2.0
9+
#
10+
# Unless required by applicable law or agreed to in writing, software
11+
# distributed under the License is distributed on an "AS IS" BASIS,
12+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
# See the License for the specific language governing permissions and
14+
# limitations under the License.
15+
16+
import rclpy
17+
from rclpy.action import ActionServer
18+
from rclpy.executors import ExternalShutdownException
19+
from rclpy.node import Node
20+
from rclpy.qos import qos_profile_system_default
21+
22+
from test_msgs.action import Fibonacci
23+
from test_msgs.msg import Arrays
24+
from test_msgs.msg import Strings
25+
from test_msgs.srv import BasicTypes
26+
27+
28+
class ComplexNode(Node):
29+
30+
def __init__(self):
31+
super().__init__('complex_node')
32+
self.publisher = self.create_publisher(
33+
Arrays, 'arrays', qos_profile_system_default
34+
)
35+
self.subscription = self.create_subscription(
36+
Strings, 'strings', lambda msg: None, qos_profile_system_default
37+
)
38+
self.server = self.create_service(BasicTypes, 'basic', lambda req, res: res)
39+
self.action_server = ActionServer(
40+
self, Fibonacci, 'fibonacci', self.action_callback
41+
)
42+
self.timer = self.create_timer(1.0, self.pub_callback)
43+
44+
def destroy_node(self):
45+
self.timer.destroy()
46+
self.publisher.destroy()
47+
self.subscription.destroy()
48+
self.server.destroy()
49+
self.action_server.destroy()
50+
super().destroy_node()
51+
52+
def pub_callback(self):
53+
self.publisher.publish(Arrays())
54+
55+
def action_callback(self, goal_handle):
56+
goal_handle.succeed()
57+
return Fibonacci.Result()
58+
59+
60+
def main(args=None):
61+
try:
62+
with rclpy.init(args=args):
63+
node = ComplexNode()
64+
rclpy.spin(node)
65+
except (KeyboardInterrupt, ExternalShutdownException):
66+
print('node stopped cleanly')
67+
68+
69+
if __name__ == '__main__':
70+
main()
Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
# Copyright 2025 Open Source Robotics Foundation, Inc.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
"""Test fixture node with parameters for ros2doctor testing."""
16+
17+
import rclpy
18+
from rclpy.executors import ExternalShutdownException
19+
from rclpy.parameter import PARAMETER_SEPARATOR_STRING
20+
21+
22+
def main():
23+
try:
24+
with rclpy.init():
25+
node = rclpy.create_node('parameter_node')
26+
# Declare various parameter types for testing
27+
node.declare_parameter('bool_param', True)
28+
node.declare_parameter('int_param', 42)
29+
node.declare_parameter('double_param', 3.14)
30+
node.declare_parameter('str_param', 'hello')
31+
node.declare_parameter('bool_array_param', [True, False])
32+
node.declare_parameter('int_array_param', [1, 2, 3])
33+
node.declare_parameter('double_array_param', [1.0, 2.0])
34+
node.declare_parameter('str_array_param', ['foo', 'bar'])
35+
node.declare_parameter(
36+
'nested' + PARAMETER_SEPARATOR_STRING + 'param', 'nested_value'
37+
)
38+
rclpy.spin(node)
39+
except (KeyboardInterrupt, ExternalShutdownException):
40+
print('parameter_node stopped cleanly')
41+
42+
43+
if __name__ == '__main__':
44+
main()

ros2doctor/test/test_api.py

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414

1515
import unittest
1616

17+
from common import generate_expected_node_report
1718
from common import generate_expected_service_report
1819
from common import generate_expected_topic_report
1920

@@ -25,6 +26,10 @@
2526

2627
import pytest
2728

29+
from ros2doctor.api.node import NodeCheck
30+
from ros2doctor.api.node import NodeReport
31+
from ros2doctor.api.parameter import ParameterCheck
32+
from ros2doctor.api.parameter import ParameterReport
2833
from ros2doctor.api.service import ServiceReport
2934
from ros2doctor.api.topic import TopicCheck
3035
from ros2doctor.api.topic import TopicReport
@@ -74,3 +79,29 @@ def test_no_service_report(self):
7479
self.assertEqual(report.name, expected_report.name)
7580
self.assertEqual(report.items, expected_report.items)
7681
self.assertEqual(report, expected_report)
82+
83+
def test_node_check(self):
84+
"""Assume no duplicate nodes exist in a clean environment."""
85+
node_check = NodeCheck()
86+
check_result = node_check.check()
87+
self.assertEqual(check_result.error, 0)
88+
self.assertEqual(check_result.warning, 0)
89+
90+
def test_no_node_report(self):
91+
"""Assume no nodes are running in a clean environment."""
92+
report = NodeReport().report()
93+
expected_report = generate_expected_node_report(0, [])
94+
self.assertEqual(report.name, expected_report.name)
95+
self.assertEqual(report.name, 'NODE LIST')
96+
97+
def test_parameter_check(self):
98+
"""Test parameter check runs without errors in clean environment."""
99+
param_check = ParameterCheck()
100+
check_result = param_check.check()
101+
self.assertEqual(check_result.error, 0)
102+
103+
def test_no_parameter_report(self):
104+
"""Assume no parameters in a clean environment."""
105+
report = ParameterReport().report()
106+
self.assertEqual(report.name, 'PARAMETER LIST')
107+
self.assertIsNotNone(report)

0 commit comments

Comments
 (0)