We have seen how diagnostics play an important role in robotics in one of our latest blog posts Using ROS Diagnostics in Robotics Operations. We have yet to see how to create a diagnostics publisher that will produce these important messages for understanding how the robot is working.

A diagnostic publisher can be used to report the status of a system. There are many ways to publish a status. It can be a self-diagnosed status; for example, a motor driver publishes its own internal status. Another option is subscribing to a topic that contains a value and reporting if said value is out of limits.

In this tutorial, you are going to learn how to create a node that works in both ways. It will report the status of a topic and an internal status form the same node. You can find the source code for this tutorial in our GitHub.

Diagnostic publisher

Create a new ROS 2 package called diagnostics with dependencies rclcpp, std_msgs and diagnostic_msgs. In the src folder, create a new file called main.cpp.

Start the file by including the necessary headers and useful namespaces.

Now create a new class called DiagnosticsPublisher that inherits from rclcpp::Node. Create a constructor and initialize the node with name “diagnostics_publisher”, a counter to 0 and an indicator for the last time a message was received to now(). Inside the constructor, declare the DiagnosticsArray publisher, the Int16 subscriber and two timers; one for checking the staleness of the topic and the other to publish the diagnostic message. This is the public section of the class.

On to the private part. In this section, declare the following callbacks and methods:

1. topic_callback: in this callback we store the value of the topic in a private variable and save the current instant.
2. staleness_check_callback: this will be triggered by a timer that will check the elapsed time since the last message was received. When this time is bigger than 1 second, the status of the topic will be STALE.
3. timer_callback: this is the self timer of the node, where the counter will increase up to 10. This value will be used later to modify a status. Additionally, use this callback to publish the DiagnosticArray after including the two status for the topic and the self timer.
4. update_topic_status: this method is not a callback; it will be called from the topic_callback to update the topic status. This could have been included in the callback, but a good coding practice includes partitioning the code into smaller functions for better readability.
5. update_self_status: same idea as the former; this method is used to update the self status and is better organized in a separate function instead of being all inside the timer_callback.

Still in the private section, don’t forget to declare your publisher, subscriber, the timers, the DiagnosticStatus messages and additional attributes used. With this done, close the class.

To finish this file, create the main function where you’ll declare the node and spin it until shutdown.

Go to the CMakeLists.txt file and add the following after the find_package(...) section

Compile with colcon build and remember to source your workspace with source install/setup.bash. Run the node with ros2 run diagnostics diagnostic_publisher and in an additional terminal run the foxglove_bridge.

Visualization in Foxglove

Open a new Foxglove window and load the following layout.

Diagnostics.json

You’ll see the following panels: "Diagnostics - Detail", "Diagnostics - Summary" and "Publish". In the "Diagnostics - Summary", you’ll find all the information contained in the DiagnosticsArray, where you should see the timer increasing and changing level depending on the value and the int16_topic stale. This is because there has been no message received. Use the Publish panel to publish a number in topic and see what happens in the diagnostics.

Dive deeper

There are some wonderful open-source tools that can help you improve your ROS diagnostics messages. To name a few, the diagnostics_updater can simplify the collection of constant data and check whether it’s between certain bounds. On the other hand, the diagnostics_aggregator helps to organize complex systems where many diagnostics are constantly published into structured diagnostic messages.

Now that you know the basics of diagnostics publishing, we invite you to play around the node you’ve created and discover how you can integrate this into your robotics career and, in-hand with Foxglove, improve your development as a roboticist!

Keep in touch with our Discord community and get started with Foxglove today.