00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "ros/ros.h"
00029 #include "ros/callback_queue.h"
00030 #include "std_msgs/String.h"
00031
00032 #include <boost/thread.hpp>
00033
00042 void chatterCallbackMainQueue(const std_msgs::String::ConstPtr& msg)
00043 {
00044 ROS_INFO_STREAM("I heard: [ " << msg->data << "] in thread [" << boost::this_thread::get_id() << "] (Main thread)");
00045 }
00046
00050 void chatterCallbackCustomQueue(const std_msgs::String::ConstPtr& msg)
00051 {
00052 ROS_INFO_STREAM("I heard: [ " << msg->data << "] in thread [" << boost::this_thread::get_id() << "]");
00053 }
00054
00058 ros::CallbackQueue g_queue;
00059 void callbackThread()
00060 {
00061 ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());
00062
00063 ros::NodeHandle n;
00064 while (n.ok())
00065 {
00066 g_queue.callAvailable(ros::WallDuration(0.01));
00067 }
00068 }
00069
00070 int main(int argc, char **argv)
00071 {
00072 ros::init(argc, argv, "listener_with_custom_callback_processing");
00073 ros::NodeHandle n;
00074
00081 ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::String>("chatter", 1000,
00082 chatterCallbackCustomQueue,
00083 ros::VoidPtr(), &g_queue);
00084 ros::Subscriber sub = n.subscribe(ops);
00085
00089 ros::Subscriber sub2 = n.subscribe("chatter", 1000, chatterCallbackMainQueue);
00090
00094 boost::thread chatter_thread(callbackThread);
00095
00096 ROS_INFO_STREAM("Main thread id=" << boost::this_thread::get_id());
00100 ros::Rate r(1);
00101 while (n.ok())
00102 {
00103 ros::spinOnce();
00104 r.sleep();
00105 }
00106
00107 chatter_thread.join();
00108
00109 return 0;
00110 }