ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

multiple callback queues

asked 2015-07-27 02:59:20 -0500

TheBoris gravatar image

Hello, I'm using a ROS node to read from several topics calling ros::spinOnce in a loop.

Every time I call spinOnce it checks every topic to which the node is subscribed, what I want instead is to check specific topics at different points in my code. I've seen here that I can achieve it using different callback queues, but when it comes to explain how to assign different callbacks to different queues the link refers to subscribers API and I got lost. Can someone post an example of how to assign, let's say, Callback_1 to queue_1 and Callback_2 to queue_2?

Thanks in advance!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
9

answered 2015-07-27 09:00:01 -0500

updated 2015-07-27 09:14:31 -0500

Here is a very simple example with the comments inline:

#include <ros/spinner.h>
#include <ros/callback_queue.h>

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

// callbacks to be added to internal ROS queue
void headerCallback(const std_msgs::Header::ConstPtr& msg)
{
  ROS_INFO("stamp.sec: %d", msg->stamp.sec);
}

// callbacks to be added to user queue
void stringCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("string: %s", msg->data.c_str());
}

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

  ros::NodeHandle n;
  // nothing special for internal ROS queue
  ros::Subscriber sub1 = n.subscribe("/header_topic", 10, headerCallback);

  // define user callback queue
  ros::CallbackQueue string_queue;
  // create options for subscriber and pass pointer to our custom queue
  ros::SubscribeOptions ops =
    ros::SubscribeOptions::create<std_msgs::String>(
      "/string_topic", // topic name
      10, // queue length
      stringCallback, // callback
      ros::VoidPtr(), // tracked object, we don't need one thus NULL
      &string_queue // pointer to callback queue object
    );
  // subscribe
  ros::Subscriber sub2 = n.subscribe(ops);

  // spawn async spinner with 1 thread, running on our custom queue
  ros::AsyncSpinner async_spinner(1, &string_queue);
  // start the spinner
  async_spinner.start();

  while (ros::ok())
  {
    // process one message from ROS internal queue
    ros::spinOnce();
    ROS_INFO("Press Enter to process available messages from ROS internal queue");
    // wait for user input
    getchar();
  }

  return 0;
}

EDIT: The custom queue is handled in parallel thread without interruption and handles messages that arrive on /string_topic, while internal queue is processed once user presses Enter in terminal. To check it, just publish to these topics from terminal with rostopic pub. You will see, that /string_topic outputs continuously, but /header_topic waits for user input.

edit flag offensive delete link more

Comments

thank you, extremely useful!

TheBoris gravatar image TheBoris  ( 2015-07-27 09:48:43 -0500 )edit
2

What if I do have a tracked object i.e., the callback functions are members of a class along with the subscribers?

2ROS0 gravatar image 2ROS0  ( 2017-02-08 10:55:52 -0500 )edit

This is excellent. Thank you!

cpagravel gravatar image cpagravel  ( 2017-03-31 19:39:47 -0500 )edit

Did someone solve this for tracked objects usage?

Marcel Usai gravatar image Marcel Usai  ( 2020-03-25 13:39:20 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2015-07-27 02:59:20 -0500

Seen: 7,295 times

Last updated: Jul 27 '15