Robotics StackExchange | Archived questions

How to use diagnostic_updater for talker node?

I want to know the status of talker node. For example frequency at which talker node is publishing the data, size of the published data.

Using diagnostic tools like diagnostic updater, diagnostic aggregator.

Please help me to solve this problem.

Asked by SumitP on 2017-11-03 09:24:55 UTC

Comments

Answers

You can get a lot of information about topics using rostopic. For example:

Frequency:

rostopic hz /topic

Bandwidth:

rostopic bw /topic

Please check out the wiki entry to see all of the details of what's possible.

Asked by jayess on 2017-11-06 10:58:35 UTC

Comments

Okay, But is it possible with diagnostic tools? I want to use with diagnostics like diagnostic aggregator or diagnostic updater.

Asked by SumitP on 2017-11-06 11:09:06 UTC

Ah sorry. I didn't notice that part of the title of your question. You probably want to tag your question with that (not just diagnostics) and specifically mention it in your question too. It's pretty easy to miss that detail when it's only in the title.

Asked by jayess on 2017-11-06 11:13:01 UTC

Okay, Sorry for that. So, is there a solution for question?

Asked by SumitP on 2017-11-06 11:33:40 UTC

Not sure, I've never used those packages.

Asked by jayess on 2017-11-06 11:38:15 UTC

Okay, Thank you.

Asked by SumitP on 2017-11-07 03:23:47 UTC

This is roughly what it looks like to adapt the roscpp talker to use a diagnosed updater and diagnosed publisher.

I haven't tested that this compiles, but it should be pretty close.

#include <ros/ros.h>
#include <std_msgs/String.h>

#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>

#include <sstream>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");

  ros::NodeHandle n;

  // The Updater class advertises to /diagnostics, and has a
  // ~diagnostic_period parameter that says how often the diagnostics
  // should be published.
  diagnostic_updater::Updater updater;

  double desired_freq = 10.0; // desired publish frequency
  double min_freq = 9.0; // minimum frequency before error
  double max_freq = 11.0; // maximum frequency before error
  double freq_tolerance = 0.1; // Tolerance before stating error on publish frequency, fractional percent of desired frequencies.
  int window_size = 100; // Number of samples to consider in frequency
  double min_acceptable = 0.05; // The minimum publishing delay (in seconds) before publish times.
  double max_acceptable = 0.15; // Maximum delay between message publish times
  diagnostic_updater::DiagnosedPublisher<std_msgs::String> chatter_pub(n.advertise<std_msgs::String>("chatter", 1000),
               updater,
               diagnostic_updater::FrequencyStatusParam(&min_freq_, &max_freq_, freq_tolerance, window_size),
               diagnostic_updater::TimeStampStatusParam(min_acceptable, max_acceptable)));

  ros::Rate loop_rate(desired_freq);

  int count = 0;
  while (ros::ok())
  {
    std_msgs::String msg;

    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();

    ROS_INFO("%s", msg.data.c_str());

    chatter_pub.publish(msg);

    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }
  return 0;
}

The CMakeLists.txt to build this node in the beginner_tutorials package should be:

cmake_minimum_required(VERSION 2.8.3)
project(beginner_tutorials)

## Find catkin and any catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp diagnostic_updater std_msgs)

## Declare a catkin package
catkin_package()

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})

Asked by ahendrix on 2017-11-06 17:58:54 UTC

Comments

That's great. Thank you.

Asked by SumitP on 2017-11-07 02:42:34 UTC

After compiling the talker node, there are errors in diagnostic_updater/include/diagnostic_updater/publisher.h

error: ‘const struct std_msgs::String_std::allocator<void >’ has no member named ‘header’ virtual void publish(const T& message) { tick(message.header.stamp);

Asked by SumitP on 2017-11-07 03:21:48 UTC

error: ‘struct std_msgs::String_std::allocator<void >’ has no member named ‘header’ tick(message->header.stamp); publisher_.publish(message); }

Please can you check with this error.

Asked by SumitP on 2017-11-07 03:22:25 UTC

Ah; the DiagnosedPublisher requires a message type with a header.

Asked by ahendrix on 2017-11-07 23:38:58 UTC

I have changed it but I got one more error including previous errors.

error: no matching function for call to ‘diagnostic_updater::DiagnosedPublisher > >::publish(std_msgs::String&)’ chatter_pub.publish(message);

Asked by SumitP on 2017-11-08 05:19:09 UTC

I found the solution. Thank for your support.

Asked by SumitP on 2017-11-10 10:21:02 UTC

Is there any chance you could edit the answer or publish the working code in your question? Looks like it would be pretty handy

Asked by TheMilkman on 2018-09-18 02:19:43 UTC