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

Using CallbackQueue

asked 2014-07-21 14:46:41 -0500

Ashesh Goswami gravatar image

Hi,

I have a code where I have already subscribed to the following topics

pose_sub = n2.subscribe("RosAria/pose", 1, poseCallback);
scan_sub = n.subscribe("scan", 1, scanCallback);

I am using ros::spinOnce() to callback to these topics. However, for a part of my code,I want to just monitor the pose messages and enter into poseCallback from inside a particular loop. However during this execution, I do not want to enter into the scanCallback function. I know this is possible but I am not very sure about its implementation. I came across this article link text which mentions about using CallbackQueue but I did not understand its implementation or rather its application completely. Could anyone suggest whether it is this feature that I should be using or is there any other way I could get hold of the pose messages during the execution of my inner loop Thanks.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
4

answered 2014-07-21 15:34:50 -0500

McMurdo gravatar image

Hi,

I also had the same doubts. After you asked the question, I decided to find out.

Here is some working code that should make things very clear.

If someone can assist me in making this a tutorial, it will be great.

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

#include <std_msgs/Empty.h>

void callback_1(const std_msgs::EmptyConstPtr& _msg)
{
    ROS_INFO("Called callback_1\n");
}

void callback_2(const std_msgs::EmptyConstPtr& _msg)
{
    ROS_INFO("Called callback_2\n");
}

int main(int argn, char* args[])
{
    ros::init(argn, args, "callback_q_subscriber");
    ros::NodeHandle nh_1;
    ros::NodeHandle nh_2;

    ros::CallbackQueue queue_1, queue_2;

    nh_1.setCallbackQueue(&queue_1);
    nh_2.setCallbackQueue(&queue_2);

    ros::Subscriber s_1 = nh_1.subscribe("/topic_1", 1, callback_1);
    ros::Subscriber s_2 = nh_2.subscribe("/topic_2", 1, callback_2);

    pthread_t id_1, id_2;

    int i = 20;
    while(i--)
    {
        queue_1.callOne(ros::WallDuration(1.0)); // can also be callAvailable()
    }

    i = 20;
    while(i--)
    {
        queue_2.callOne(ros::WallDuration(1.0)); // can also be callAvailable()
    }

    ROS_INFO("Hurray!");

    return 0;   
}

STEP 1:

Create a catkin package called callback_queue_test or what have you.

catkin_create_pkg callback_queue_test roscpp std_msgs

STEP 2:

a. Open favorite editor and paste the code inside it. Save it as simple.cpp.
b. Edit CMakeLists.txt to add an executable called simple_node. Add the target_link_libraries() for it.

STEP 3:

Open 4 terminals. Run the following in each in the same order.

a. roscore
b. rostopic pub -r 10 /topic_1 std_msgs/Empty
c. rostopic pub -r 10 /topic_2 std_msgs/Empty
d. rosrun callback_queue_test simple_node

That's it.

Have a nice day!

edit flag offensive delete link more

Comments

1

Hey, thanks a ton. I can get my code to perform the same function. I had almost implemented the same thing except I had not given ros::WallDuration(1.0) as a parameter while calling callOne(). But now my problem seems to be solved. Once again, thanks a lot.

Ashesh Goswami gravatar image Ashesh Goswami  ( 2014-07-21 16:17:03 -0500 )edit

The ros::WallDuration is a timeout. I also assumed that there ought to be a default value. But, I think it is very low. For me without the WallDuration, the callback never happened.

McMurdo gravatar image McMurdo  ( 2014-07-22 00:28:25 -0500 )edit

It might be the reason. Anyway, thanks a lot. In case if anyone finds out why the code doesnt work without mentioning WallDuration, please let me know.

Ashesh Goswami gravatar image Ashesh Goswami  ( 2014-07-22 13:45:43 -0500 )edit

where is pthread used?

ZiyangLI gravatar image ZiyangLI  ( 2015-01-24 07:02:08 -0500 )edit

What's the use of the line "pthread_t id_1, id_2;" ?

alienmon gravatar image alienmon  ( 2016-08-18 21:40:50 -0500 )edit

Where do I call the callAvailable() if I have the nodehandle in a class?

ashwath1993 gravatar image ashwath1993  ( 2017-12-08 05:07:12 -0500 )edit

You should probably make a public member function to do that.

McMurdo gravatar image McMurdo  ( 2017-12-08 05:16:19 -0500 )edit

@ZiyangLI never used. Probably some old code leftover.

McMurdo gravatar image McMurdo  ( 2017-12-08 05:17:22 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2014-07-21 14:46:41 -0500

Seen: 3,638 times

Last updated: Jul 21 '14