(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Adding Analyzers at Runtime

Description: Use launch files or bonds and a service to add a new set of analyzers to the aggregator.

Tutorial Level: INTERMEDIATE

New in ROS indigo

Overview

It is possible to modify what the diagnostic aggregator analyzes while it is running. This allows you to temporarily add diagnostic analyzers only when they are needed, to reduce clutter, or to add analyzers corresponding to a specific node or collection of nodes in a launch file so that they are added when the launch file is run. This has the added benefit of allowing you to separate analyzers based on which parts of the system they are analyzing.

This tutorial will show you how to add analyzers either from a launch file or from inside a node. We will create a small test package to demonstrate the functionality.

Creating the Test Package

catkin_create_pkg test_add_analyzers diagnostic_msgs rospy roscpp bondpy

Create directories for launch files, scripts for diagnostic publishing, and parameters for analyzer setup.

roscd test_add_analyzers
mkdir launch scripts param

Add a launch file for the diagnostic aggregator.

launch/base.launch

<launch>
  <node pkg="test_add_analyzers" type="base_node.py" name="base_node"/>
  <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator">
    <rosparam command="load" file="$(find test_add_analyzers)/param/base_analyzers.yaml"/>
  </node>
</launch>

Add a node to publish some diagnostic messages. Don't forget to make it executable.

scripts/base_node.py

   1 #!/usr/bin/env python
   2 
   3 import rospy
   4 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
   5 
   6 if __name__ == '__main__':
   7     rospy.init_node('base_node')
   8     pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
   9 
  10     arr = DiagnosticArray()
  11     arr.status = [
  12         DiagnosticStatus(name='battery battery_message', message='the battery message'),
  13         DiagnosticStatus(name='joystick joystick_message', message='the joystick message')
  14     ]
  15 
  16     rate = rospy.Rate(1)
  17     while not rospy.is_shutdown():
  18         arr.header.stamp = rospy.Time.now()
  19         pub.publish(arr)
  20         rate.sleep()

Add a .yaml containing some simple analyzers.

param/base_analyzers.yaml

analyzers:
  base:
    type: diagnostic_aggregator/AnalyzerGroup
    path: Base
    analyzers:
      battery:
        type: diagnostic_aggregator/GenericAnalyzer
        path: Battery
        find_and_remove_prefix: 'battery'
      joy:
        type: diagnostic_aggregator/GenericAnalyzer
        path: Joystick
        find_and_remove_prefix: 'joystick'

Loading From Launch Files

Loading from a launch file is done in a similar way to how the diagnostic aggregator is launched. Parameters defining the analyzers are loaded directly into the node namespace, which the aggregator then reads in order to initialise the new analyzers. The add_analyzers node is linked to the aggregator via a bond object. When the node is shut down, the bond breaks, and the analyzers associated with that node will be removed from the aggregator. The parameters which were loaded by the node are not deleted when the node shuts down.

Add a launch file to add the analyzers.

launch/simple.launch

<launch>
  <node pkg="test_add_analyzers" type="simple_node.py" name="simple_node"/>
  <node pkg="diagnostic_aggregator" type="add_analyzers" name="add_simple_analyzers">
    <rosparam command="load" file="$(find test_add_analyzers)/param/simple_analyzers.yaml" />
  </node>
</launch>

Add a node to publish some dummy data to the diagnostics topic. Remember to make it executable.

scripts/simple_node.py

   1 #!/usr/bin/env python
   2 
   3 import rospy
   4 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
   5 
   6 if __name__ == '__main__':
   7     rospy.init_node('simple_node')
   8     pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
   9 
  10     arr = DiagnosticArray()
  11     arr.status = [
  12         DiagnosticStatus(name='rgb_stream rgb_message', message='the rgb data'),
  13         DiagnosticStatus(name='rgbd_stream rgbd_message', message='the rgbd data')
  14     ]
  15 
  16     rate = rospy.Rate(1)
  17     while not rospy.is_shutdown():
  18         arr.header.stamp = rospy.Time.now()
  19         pub.publish(arr)
  20         rate.sleep()

Add a .yaml for some different analyzer specifications.

param/simple_analyzers.yaml

analyzers:
    vision:
      type: diagnostic_aggregator/AnalyzerGroup
      path: Vision
      analyzers:
        rgb:
          type: diagnostic_aggregator/GenericAnalyzer
          path: CameraRGB
          find_and_remove_prefix: 'rgb_stream'
        rgbd:
          type: diagnostic_aggregator/GenericAnalyzer
          path: CameraRGBD
          find_and_remove_prefix: 'rgbd_stream'

First, launch the aggregator with the base analyzers using

roslaunch test_add_analyzers base.launch

Then, run the rqt_robot_monitor to view the aggregator output.

rosrun rqt_robot_monitor rqt_robot_monitor

Finally, add the simple analyzers to the aggregator output.

roslaunch test_add_analyzers simple.launch

You should see the diagnostic output appear, first in the Other group, and then in the correct groups when the aggregator finishes updating the analyzers.

Loading From a Node

It is also possible to add analyzers to the aggregator from inside a node.

Create a launcher for the node.

launch/manual_node.launch

<launch>
  <node pkg="test_add_analyzers" type="manual_diag.py" name="manual_diag"/>
  <node pkg="test_add_analyzers" type="manual_node.py" name="manual_node">
    <param name="analyzer_yaml" value="$(find test_add_analyzers)/param/manual_analyzers.yaml" />
  </node>
</launch>

Add some analyzer parameters.

param/manual_analyzers.yaml

analyzers:
  buttons:
    type: diagnostic_aggregator/AnalyzerGroup
    path: Buttons
    analyzers:
      go_button:
        type: diagnostic_aggregator/GenericAnalyzer
        path: Go
        find_and_remove_prefix: 'button_go'
      stop_button:
        type: diagnostic_aggregator/GenericAnalyzer
        path: Stop
        find_and_remove_prefix: 'button_stop'

Add a node to publish diagnostics

scripts/manual_diag.py

   1 #!/usr/bin/env python
   2 
   3 import rospy
   4 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
   5 
   6 if __name__ == '__main__':
   7     rospy.init_node('manual_diag')
   8     pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
   9 
  10     arr = DiagnosticArray()
  11     arr.status = [
  12         DiagnosticStatus(name='button_stop go_message', message='the go button message'),
  13         DiagnosticStatus(name='button_go stop_message', message='the stop button message')
  14     ]
  15 
  16     rate = rospy.Rate(1)
  17     while not rospy.is_shutdown():
  18         arr.header.stamp = rospy.Time.now()
  19         pub.publish(arr)
  20         rate.sleep()

Add a node to do the additions.

scripts/manual_node.py

   1 #!/usr/bin/env python
   2 
   3 import rospy
   4 import rosparam
   5 from bondpy import bondpy
   6 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
   7 from diagnostic_msgs.srv import AddDiagnostics
   8 
   9 class ManualNode(object):
  10 
  11     def __init__(self):
  12         rospy.init_node('manual')
  13         rospy.on_shutdown(self.shutdown)
  14         # Load yaml parameters from a file, and load them into the parameter server
  15         # under this node's namespace.
  16         paramlist = rosparam.load_file(rospy.get_param('~analyzer_yaml'))
  17         for params, ns in paramlist:
  18             rosparam.upload_params(rospy.get_name() + '/' + ns, params)
  19 
  20         # Can use any namespace into which the parameters that we want to use to
  21         # specify analyzers have been loaded
  22         self.namespace = rospy.get_name()
  23 
  24 
  25     def start(self):
  26 
  27         # Create a bond to try and connect to the diagnostics aggregator. The second
  28         # parameter to the bond is the name of the bond. This name should be the
  29         # same as the load_namespace below
  30         self.bond = bondpy.Bond("/diagnostics_agg/bond", rospy.resolve_name(self.namespace))
  31 
  32         # Start the bond, initialising the connection and creating the analyzers
  33         # that were defined by the parameters loaded earlier
  34         self.bond.start()
  35 
  36         # To start the other side of the bond in the diagnostic aggregator, we must
  37         # send request via the add_diagnostics service. Once a service message is
  38         # received, the bond will form, and the analyzers will be added to the
  39         # aggregator.
  40         rospy.wait_for_service('/diagnostics_agg/add_diagnostics', timeout=10)
  41         add_diagnostics = rospy.ServiceProxy('/diagnostics_agg/add_diagnostics', AddDiagnostics)
  42 
  43         # The response indicates whether setup was successful or not. If the
  44         # namespace given is the same as a namespace that was sent previously, the
  45         # analyzers will not be added.
  46         resp = add_diagnostics(load_namespace=self.namespace)
  47 
  48         rate = rospy.Rate(1)
  49         while not rospy.is_shutdown():
  50             rate.sleep()
  51 
  52     def shutdown(self):
  53         # Make sure the bond is shut down once you're done. If it is not shut down
  54         # manually, the other side of the bond will automatically shut down after a
  55         # short time (~5 seconds), removing the analyzers from the aggregator.
  56         self.bond.shutdown()
  57         
  58 
  59 if __name__ == '__main__':
  60     m = ManualNode()
  61     m.start()

Having launched the base aggregator, launch the node

roslaunch test_add_analyzers manual_node.launch

Wiki: diagnostics/Tutorials/Adding Analyzers at Runtime (last edited 2016-01-08 07:05:29 by MichalStaniaszek)