Aggregator: publish diagnostics_toplevel_state immediately when error is received

[outrider-jhulas]

To improve the diagnostic_aggregator, additional functionality is added to publish the diagnostics_top_level_state as soon as a diagnostic message with an ERROR state is received.

This can be particularly helpful, when using the diagnostics_top_level_state as a system error indicator. For example, immediately stop the robot, as soon as any error in the system is detected.

With the current implementation, the error state will take up the the 1/pub_rate seconds to be published, which is a significant delay in many robotic systems.
This point was already brought up in !197 and #48.

To guarantee backwards compatibility, the functionality is disabled by default and can be enabled with the critical parameter set to true .

To avoid spamming the topic, the critical error state message will be published only once, and only if the system is error free.

[ct2034]

The changes look good. Thanks for your contribution.

Can you please additionally provide tests. See https://github.com/ros/diagnostics/pull/197/files#diff-d52d1bb554fc7e0c46c8810896296d59f4bf8902bd0eca3c8629e231989c76af for an inspiration.

[ct2034]

Thanks for your contribution. This looks very nice.

[ct2034]

@outrider-jhulas Do you have an idea what the problem is here: https://build.ros2.org/job/Hdev__diagnostics__ubuntu_jammy_amd64/31/testReport/junit/(root)/projectroot/test_test_critical_pub_py/

[ct2034]

I opened this: #323. If I can't fix it i will have to revert this

[outrider-jhulas]

Seems like sometimes the aggregator publishes stale msg before the test starts. This should fix it. Let me know if you want me to make a new MR

        # Ensure the aggregator doesn't publish stale msg at startup
        status = DiagnosticStatus.OK
        while True:
            self.publish_message(status)
            if status == self.received_state[0]:
                return
            else:
                self.received_state.clear()
    
    def test_critical_publisher(self):
        self.init_aggregator()

[Timple]

Sorry to be late to the party. But why was this done only for ERROR cases?

I would expect this to be active on all regresions. So dropping from OK to WARN, from WARN to ERROR and also for OK to ERROR directly of course.