Skip to content

Commit 0caeff7

Browse files
committed
Add tests for nested YAML and don't specify the topics parameter every time
Signed-off-by: Blake McHale <bmchale@nvidia.com>
1 parent 4a3f4d3 commit 0caeff7

File tree

8 files changed

+94
-33
lines changed

8 files changed

+94
-33
lines changed

greenwave_monitor/examples/example.launch.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -53,9 +53,9 @@ def generate_launch_description():
5353
parameters=[
5454
{
5555
'topics': {
56-
'/imu_topic': {'expected_frequency': 100.0},
57-
'/image_topic': {'expected_frequency': 30.0},
58-
'/string_topic': {'expected_frequency': 1000.0}
56+
'/imu_topic': {'expected_frequency': 100.0, 'tolerance': 5.0},
57+
'/image_topic': {'expected_frequency': 30.0, 'tolerance': 5.0},
58+
'/string_topic': {'expected_frequency': 1000.0, 'tolerance': 5.0}
5959
},
6060
}
6161
]

greenwave_monitor/greenwave_monitor/test_utils.py

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -73,14 +73,15 @@ def create_monitor_node(namespace: str = MONITOR_NODE_NAMESPACE,
7373
topics: List[str] = None,
7474
topic_configs: dict = None):
7575
"""Create a greenwave_monitor node for testing."""
76-
if topics is None:
77-
topics = ['/test_topic']
78-
79-
# Ensure topics list has at least one element (even if empty string)
80-
if not topics:
81-
topics = ['']
82-
83-
params = {'topics': topics}
76+
params = {}
77+
78+
# Only add topics param if explicitly provided or no topic_configs
79+
if topics is not None:
80+
if not topics:
81+
topics = ['']
82+
params['topics'] = topics
83+
elif not topic_configs:
84+
params['topics'] = ['/test_topic']
8485

8586
if topic_configs:
8687
for topic, config in topic_configs.items():

greenwave_monitor/test/parameters/test_param_dynamic.py

Lines changed: 38 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@
4343

4444

4545
TEST_TOPIC = '/dynamic_param_topic'
46+
NEW_TOPIC = '/new_param_topic'
4647
TEST_FREQUENCY = 30.0
4748

4849

@@ -89,20 +90,23 @@ def make_tol_param(topic: str) -> str:
8990
def generate_test_description():
9091
"""Test dynamic parameter changes via ros2 param set."""
9192
ros2_monitor_node = create_monitor_node(
92-
namespace=MONITOR_NODE_NAMESPACE,
93-
node_name=MONITOR_NODE_NAME,
94-
topics=[TEST_TOPIC], # Topic exists but no expected frequency
95-
topic_configs={}
93+
topics=[TEST_TOPIC] # Topic exists but no expected frequency
9694
)
9795

9896
publisher = create_minimal_publisher(
9997
TEST_TOPIC, TEST_FREQUENCY, 'imu', '_dynamic'
10098
)
10199

100+
# Publisher for topic not initially monitored
101+
new_topic_publisher = create_minimal_publisher(
102+
NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic'
103+
)
104+
102105
return (
103106
launch.LaunchDescription([
104107
ros2_monitor_node,
105108
publisher,
109+
new_topic_publisher,
106110
launch_testing.actions.ReadyToTest()
107111
]), {}
108112
)
@@ -194,6 +198,36 @@ def test_verify_params_with_get(self):
194198
msg=f'Tolerance mismatch: expected {expected_tol}, got {actual_tol}'
195199
)
196200

201+
def test_add_new_topic_via_param(self):
202+
"""Test that setting frequency param for unmonitored topic starts monitoring."""
203+
time.sleep(1.0)
204+
205+
# Verify topic is not initially monitored
206+
initial_diagnostics = collect_diagnostics_for_topic(
207+
self.test_node, NEW_TOPIC, expected_count=1, timeout_sec=2.0
208+
)
209+
self.assertEqual(
210+
len(initial_diagnostics), 0,
211+
f'{NEW_TOPIC} should not be monitored initially'
212+
)
213+
214+
# Set expected frequency for the new topic
215+
freq_param = make_freq_param(NEW_TOPIC)
216+
success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY)
217+
self.assertTrue(success, f'Failed to set {freq_param}')
218+
219+
time.sleep(2.0)
220+
221+
# Verify topic is now monitored
222+
received_diagnostics = collect_diagnostics_for_topic(
223+
self.test_node, NEW_TOPIC, expected_count=3, timeout_sec=10.0
224+
)
225+
226+
self.assertGreaterEqual(
227+
len(received_diagnostics), 3,
228+
f'{NEW_TOPIC} should be monitored after setting frequency param'
229+
)
230+
197231

198232
if __name__ == '__main__':
199233
unittest.main()

greenwave_monitor/test/parameters/test_param_freq_only.py

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,6 @@
2626
collect_diagnostics_for_topic,
2727
create_minimal_publisher,
2828
create_monitor_node,
29-
MONITOR_NODE_NAME,
3029
MONITOR_NODE_NAMESPACE
3130
)
3231
import launch
@@ -51,9 +50,6 @@ def generate_test_description():
5150
}
5251

5352
ros2_monitor_node = create_monitor_node(
54-
namespace=MONITOR_NODE_NAMESPACE,
55-
node_name=MONITOR_NODE_NAME,
56-
topics=[],
5753
topic_configs=topic_configs
5854
)
5955

greenwave_monitor/test/parameters/test_param_new_topic.py

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -64,12 +64,7 @@ def make_freq_param(topic: str) -> str:
6464
@pytest.mark.launch_test
6565
def generate_test_description():
6666
"""Test adding a new topic via ros2 param set."""
67-
ros2_monitor_node = create_monitor_node(
68-
namespace=MONITOR_NODE_NAMESPACE,
69-
node_name=MONITOR_NODE_NAME,
70-
topics=[''], # Empty - no initial topics
71-
topic_configs={}
72-
)
67+
ros2_monitor_node = create_monitor_node()
7368

7469
publisher = create_minimal_publisher(
7570
NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic'

greenwave_monitor/test/parameters/test_param_tol_only.py

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,6 @@
4848
def generate_test_description():
4949
"""Test with only tolerance specified (should not monitor)."""
5050
params = {
51-
'topics': [''],
5251
f'{TOPIC_PARAM_PREFIX}{TEST_TOPIC}{TOL_SUFFIX}': 15.0
5352
}
5453

greenwave_monitor/test/parameters/test_param_yaml.py

Lines changed: 43 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -39,22 +39,28 @@
3939

4040

4141
YAML_TOPIC = '/yaml_config_topic'
42+
NESTED_TOPIC = '/nested_yaml_topic'
4243
TEST_FREQUENCY = 50.0
44+
NESTED_FREQUENCY = 25.0
4345
TEST_TOLERANCE = 10.0
4446

4547

4648
@pytest.mark.launch_test
4749
def generate_test_description():
4850
"""Test loading parameters from a YAML file."""
49-
# Write YAML manually with proper quoting (yaml.dump doesn't quote keys with dots)
50-
# Use /** to apply to all nodes, or use full namespace path
51+
# Write YAML manually - demonstrates both flat dotted keys and nested dict formats
52+
# Use full namespace path for node parameters
5153
yaml_content = (
5254
f'/{MONITOR_NODE_NAMESPACE}/{MONITOR_NODE_NAME}:\n'
5355
f' ros__parameters:\n'
54-
f' topics:\n'
55-
f" - ''\n"
56+
f' # Flat dotted key format (requires quotes)\n'
5657
f' "topics.{YAML_TOPIC}.expected_frequency": {TEST_FREQUENCY}\n'
5758
f' "topics.{YAML_TOPIC}.tolerance": {TEST_TOLERANCE}\n'
59+
f' # Nested dictionary format\n'
60+
f' topics:\n'
61+
f' {NESTED_TOPIC}:\n'
62+
f' expected_frequency: {NESTED_FREQUENCY}\n'
63+
f' tolerance: {TEST_TOLERANCE}\n'
5864
)
5965

6066
yaml_dir = tempfile.mkdtemp()
@@ -75,10 +81,15 @@ def generate_test_description():
7581
YAML_TOPIC, TEST_FREQUENCY, 'imu', '_yaml'
7682
)
7783

84+
nested_publisher = create_minimal_publisher(
85+
NESTED_TOPIC, NESTED_FREQUENCY, 'imu', '_nested_yaml'
86+
)
87+
7888
return (
7989
launch.LaunchDescription([
8090
ros2_monitor_node,
8191
publisher,
92+
nested_publisher,
8293
launch_testing.actions.ReadyToTest()
8394
]), {}
8495
)
@@ -127,6 +138,34 @@ def test_topic_configured_via_yaml(self):
127138

128139
self.assertTrue(has_valid_rate, 'Should have valid frame rate from YAML config')
129140

141+
def test_nested_dict_topic_configured_via_yaml(self):
142+
"""Test that topic configured via nested YAML dict is monitored."""
143+
time.sleep(2.0)
144+
145+
received_diagnostics = collect_diagnostics_for_topic(
146+
self.test_node, NESTED_TOPIC, expected_count=3, timeout_sec=10.0
147+
)
148+
149+
self.assertGreaterEqual(
150+
len(received_diagnostics), 3,
151+
'Expected diagnostics from nested YAML-configured topic'
152+
)
153+
154+
has_valid_rate = False
155+
for status in received_diagnostics:
156+
for kv in status.values:
157+
if kv.key == 'frame_rate_node':
158+
try:
159+
if float(kv.value) > 0:
160+
has_valid_rate = True
161+
break
162+
except ValueError:
163+
continue
164+
if has_valid_rate:
165+
break
166+
167+
self.assertTrue(has_valid_rate, 'Should have valid frame rate from nested YAML config')
168+
130169

131170
if __name__ == '__main__':
132171
unittest.main()

greenwave_monitor/test/parameters/test_topic_parameters.py

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -54,9 +54,6 @@ def generate_test_description():
5454
}
5555

5656
ros2_monitor_node = create_monitor_node(
57-
namespace=MONITOR_NODE_NAMESPACE,
58-
node_name=MONITOR_NODE_NAME,
59-
topics=[],
6057
topic_configs=topic_configs
6158
)
6259

0 commit comments

Comments
 (0)