Skip to content

Commit

Permalink
Merge branch 'ros2' into asymingt/aggregate-discard-stale-gracefully
Browse files Browse the repository at this point in the history
  • Loading branch information
ct2034 committed Dec 12, 2023
2 parents ad76de7 + b169c12 commit 2b88b89
Show file tree
Hide file tree
Showing 3 changed files with 24 additions and 12 deletions.
16 changes: 10 additions & 6 deletions diagnostic_aggregator/test/test_critical_pub.py
Original file line number Diff line number Diff line change
Expand Up @@ -61,10 +61,10 @@ def publish_message(self, level):
msg.status.append(DiagnosticStatus())
msg.status[0].level = level
msg.status[0].name = 'test status'
while len(self.received_state) == 0:
while msg.status[0].level not in self.received_state:
self.received_state.clear()
self.publisher.publish(msg)
rclpy.spin_once(self.node)

return self.node.get_clock().now()

def test_critical_publisher(self):
Expand All @@ -73,15 +73,17 @@ def test_critical_publisher(self):
time_0 = self.publish_message(state)

assert (self.received_state[0] == state), \
'Received state is not the same as the sent state'
('Received state is not the same as the sent state:'
+ f"'{self.received_state[0]}' != '{state}'")
self.received_state.clear()

# Publish the ok message and expect the toplevel state after 1 second period
time_1 = self.publish_message(state)
assert (time_1 - time_0 > rclpy.duration.Duration(seconds=0.99)), \
'OK message received too early'
assert (self.received_state[0] == state), \
'Received state is not the same as the sent state'
('Received state is not the same as the sent state:'
+ f"'{self.received_state[0]}' != '{state}'")
self.received_state.clear()

# Publish the message and expect the critical error message immediately
Expand All @@ -91,7 +93,8 @@ def test_critical_publisher(self):
assert (time_2 - time_1 < rclpy.duration.Duration(seconds=0.1)), \
'Critical error message not received within 0.1 second'
assert (self.received_state[0] == state), \
'Received state is not the same as the sent state'
('Received state is not the same as the sent state:'
+ f"'{self.received_state[0]}' != '{state}'")
self.received_state.clear()

# Next error message should be sent at standard 1 second rate
Expand All @@ -100,4 +103,5 @@ def test_critical_publisher(self):
assert (time_3 - time_1 > rclpy.duration.Duration(seconds=0.99)), \
'Periodic error message received too early'
assert (self.received_state[0] == state), \
'Received state is not the same as the sent state'
('Received state is not the same as the sent state:'
+ f"'{self.received_state[0]}' != '{state}'")
19 changes: 14 additions & 5 deletions diagnostic_common_diagnostics/test/systemtest/test_ntp_monitor.py
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ def __init__(self, methodName: str = 'runTest') -> None:

def setUp(self):
self.n_msgs_received = 0
n = self._count_msgs(TIMEOUT_MAX_S)
n = self._count_msgs(1.)
self.assertEqual(n, 0)
self.subprocess = subprocess.Popen(
[
Expand All @@ -73,20 +73,25 @@ def tearDown(self):
self.subprocess.kill()

def _diagnostics_callback(self, msg):
rclpy.logging.get_logger('test_ntp_monitor').info(
f'Received diagnostics message: {msg}'
)
search_strings = [
'NTP offset from',
'NTP self-offset for'
]
for search_string in search_strings:
if search_string not in ''.join([
if search_string in ''.join([
s.name for s in msg.status
]):
return
self.n_msgs_received += 1
self.n_msgs_received += 1

def _count_msgs(self, timeout_s):
self.n_msgs_received = 0
node = rclpy.create_node('test_ntp_monitor')
rclpy.logging.get_logger('test_ntp_monitor').info(
'_count_msgs'
)
node.create_subscription(
DiagnosticArray,
'diagnostics',
Expand All @@ -95,9 +100,13 @@ def _count_msgs(self, timeout_s):
)
TIME_D_S = .05
waited_s = 0.
start = node.get_clock().now()
while waited_s < timeout_s and self.n_msgs_received == 0:
rclpy.spin_once(node, timeout_sec=TIME_D_S)
waited_s += TIME_D_S
waited_s = (node.get_clock().now() - start).nanoseconds / 1e9
rclpy.logging.get_logger('test_ntp_monitor').info(
f'received {self.n_msgs_received} messages after {waited_s}s'
)
node.destroy_node()
return self.n_msgs_received

Expand Down
1 change: 0 additions & 1 deletion diagnostic_updater/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,6 @@
<test_depend>ament_lint_common</test_depend>
<test_depend>launch</test_depend>
<test_depend>launch_testing</test_depend>
<!-- <test_depend>launch_testing_ros</test_depend> -->
<test_depend>python3-pytest</test_depend>
<test_depend>rclcpp_lifecycle</test_depend>

Expand Down

0 comments on commit 2b88b89

Please sign in to comment.