Hello, I have a gazebo model of a robot in a simulation. The robot uses some gazebo plugins to simulate cameras (navigation, depth, etc) and each of them are publishing on different ros topics ("/navcamera/imageraw", "/depthcam/imageraw") at 30 Hz. I want to use the diagnostics package to check the status of these simulated cameras and display the status in Robot Monitor (like a ERROR if the camera is off and doesn't publish anything). I noted that RViz does something similar( if frames are received then the status is 'OK', else it displays a 'Warning').

rviz camera

My idea is to use TopicDiagnostic on the camera topic (e.g '/navcamera/imageraw') and TimeStampStatusParam to create a task that checks if the topic is regularly updated but I'm facing several problems in understanding the documentation and the usage of these tools (mainly because of my little knowledge and experience with c++).

So basically what I think I have to do is to create a 'TimeStampStatus' with 'TimeStampStatusParam':

    double min_freq = 5;
    double max_freq = 10;

    const double min_ts = 1;
    const double max_ts = 5;

    diagnostic_updater::TopicDiagnostic pub1_freq(
        "/nav_camera/image_raw", 
        navcamUpdater,
        diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10),
        diagnostic_updater::TimeStampStatusParam(min_ts, max_ts));

Then I need to call .tick(stamp)

 while (nh.ok())
  {
    ros::Duration(0.1).sleep();
    std_msgs::Bool msg;
    msg.data = false;
    pub1_freq.tick(stamp);

    navcamUpdater.update();
  }
  return 0; 

Full code:

#include <diagnostic_updater/navcam_updater.h>

ros::Time stamp(0.1);
int main(int argc, char **argv)
{
    ros::init(argc, argv, "navcam_updater"); // Ros initialization function.
    ros::NodeHandle nh; // Construct a NodeHandle Class. This class is used for writing nodes. 
    diagnostic_updater::Updater navcamUpdater; // Construct an updater class.

    double min_freq = 5;
    double max_freq = 10;

    const double min_ts = 1;
    const double max_ts = 5;

    diagnostic_updater::TopicDiagnostic pub1_freq(
        "/nav_camera/image_raw", 
        navcamUpdater,
        diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10),
        diagnostic_updater::TimeStampStatusParam(min_ts, max_ts));



    navcamUpdater.force_update();

  while (nh.ok())
  {
    ros::Duration(0.1).sleep();
    std_msgs::Bool msg;
    msg.data = false;
    pub1_freq.tick(stamp);

    navcamUpdater.update();
  }
  return 0; 
}

Since I don't understand what stamp does in .tick(stamp) I setted it as 0.1.

The rate and delay of the topic are this:

topic update rate

topic delay

The result if this code is:

nav camera updater monitor

I don't understand why the rates and delays in the robot monitor don't match at all with the true values given by 'rostopic hz (delay) /navcamer/imageraw'.

Am I wrong on what 'TopicDiagnostic', 'FrequencyStatusParam' and 'TimeStampStatusParam' effectively do or I'm using them in the wrong way (e.g. parameters, stamp, sleep)

There's definitely something that I'm missing out and it would be nice to understand what. Is there a different solution to my intent? Hope someone can help me to figure out something.

I'm on ROS melodic, Ubuntu 18.04.6 LTS