[jsk_topic_tools/HzMeasureNodelet] Added measure_time param and modified to measure hz in the updateDiagnostics function.

[iory]

What is this?

Since the hz is now calculated in the callback function, so if topic stops for a while, the hz calculation will not be correct.

With this PR, the hz calculation will be correct even if the topic does not come.

Also, the ~measure_time param was added so that hz can be calculated based on the number of topics that came within a certain time period.

Parameter

• ~measure_time (Double, default: 1.0):

  Calculate hz from the number of topics received in time in ~measure_time.

• ~message_num (Int, default: -1):

  Calculate hz from the arrival times of ~message_num topics. Note that if this value is less than 0 or not set, ~measure_time will be used.

• ~warning_hz (Double, default: -1):

  If target hz is smaller than ~warning_hz, this node outputs diagnostics at WARN level (if ~use_warn is True) or ERROR level.

• ~use_warn (Bool, default: False):

  If this parameter is enabled, diagnostic messages on failure is displayed on WARN level instead of ERROR level.

[iory]

@tkmtnt7000 Could you review this?

[tkmtnt7000]

Sorry for late review. I tested this node with following test codes and get the following result. If I make a mistake about how to test this PR, please let me know.

• Test code

  $ rosrun nodelet nodelet manager # terminal 1
  $ roslaunch sample_hz_measure.launch # terminal 2

<!-- sample_hz_measure.launch -->
<launch>
  <node name="hz_measure"
        pkg="nodelet" type="nodelet"
        args="load jsk_topic_tools/HzMeasure /manager" >
    <remap from="~input" to="hw_registered/image_rect_raw" />
    <remap from="~output" to="hw_registered/image_rect_raw/measured_hz" />
    <rosparam subst_value="true" >
      warning_hz: 5
    </rosparam>
  </node>
</launch>

$ rostopic pub /hw_registered/image_rect_raw std_msgs/Int16 -r 30 5

• Result

$ rostopic echo /diagnostics
...
...
---
header: 
  seq: 1
  stamp: 
    secs: 1661417928
    nsecs: 386849346
  frame_id: ''
status: 
  - 
    level: 2
    name: "manager: /hz_measure"
    message: "/hz_measure is waiting input topic."
    hardware_id: "/hz_measure"
    values: []
---
header: 
  seq: 2
  stamp: 
    secs: 1661417930
    nsecs: 386312250
  frame_id: ''
status: 
  - 
    level: 2
    name: "manager: /hz_measure"
    message: "/hz_measure is waiting input topic."
    hardware_id: "/hz_measure"
    values: []
---
header: 
  seq: 3
  stamp: 
    secs: 1661417932
    nsecs: 386144813
  frame_id: ''
status: 
  - 
    level: 2
    name: "manager: /hz_measure"
    message: "/hz_measure is waiting input topic."
    hardware_id: "/hz_measure"
    values: []
---