猪事大吉 - ROS 学习 - BigRobot - 第二日 - ROS多线程部分

ros::spin()和ros::spinOnce()

点击(此处)折叠或打开

1. 什么时候用ros::spin()和ros::spinOnce()
   
2. 当只有话题订阅时, 则程序只需要在话题消息来临时响应回调函数即可, 即为使用 ros::spin(), 即为死循环.
   
3. 
   
4. 当还有其他事情要做,是仅仅把时间片给ROS系统用,可以用来响应一些订阅回调(也可能什么也没做)或者给ROS系统做一些其他事情, 完毕后ROS系统再把时间片回到程序中来.
5. 一般的用法:
6.     ros::Rate loop_rate(10);
   
7.     while (ros::ok()) {
8.         do_something();
   
9.         ros::spinOnce();
10.         loop_rate.sleep();
    
11.     }
    

自定义消息队列 + 处理线程 

点击(此处)折叠或打开

1. #include "ros/ros.h"
   
2. #include "ros/callback_queue.h"
   
3. #include "std_msgs/String.h"
   
4.  
   
5. #include <boost/thread.hpp> 
6. /**
   
7.  * tutorial demonstrates the use of custom separate callback queues that can be processed
   
8.  * independently, whether in different threads or just at different times. 
9.  * 演示了自定义独立回调队列的使用，
   
10.  * 这些回调队列可以在不同的线程中独立处理，也可以在不同的时间进行处理。
    
11.  */
    
12.  
    
13. void chatterCallbackMainQueue(const std_msgs::String::ConstPtr& msg)
    
14. {
    
15.   ROS_INFO_STREAM("I heard: [ " << msg->data << "] in thread [" << boost::this_thread::get_id() << "] (Main thread)");
    
16. }
    
17.  
    
18. //主线程中的调用
19. void chatterCallbackCustomQueue(const std_msgs::String::ConstPtr& msg)
    
20. {
    
21.   ROS_INFO_STREAM("I heard: [ " << msg->data << "] in thread [" 
22.         << boost::this_thread::get_id() << "]");
    
23. }
    
24.  
    
25. /** 
    
26.  * The custom queue used for one of the subscription callbacks
    
27.  */
    
28. ros::CallbackQueue g_queue; //第一步:用于订阅回调的自定义队列   
    
29. 
    
30. void callbackThread()
    
31. {
    
32.   ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());
    
33.  
    
34.   ros::NodeHandle n;
    
35.   while (n.ok())
    
36.   {
    
37.     //第四步: 执行自定义队列中的回调函数. 
38.         // CallbackQueue类有两种调用内部回调的方法:callAvailable()和callOne()。
39.         // callAvailable()将获取队列中当前的所有内容并调用它们。callOne()将简单地调用队列上最古老的回调。
40.     g_queue.callAvailable(ros::WallDuration(0.01));  
    
41.   }
    
42. }
    
43.  
    
44. int main(int argc, char **argv)
    
45. {
    
46.   ros::init(argc, argv, "listener_with_custom_callback_processing");
    
47.   ros::NodeHandle n;
    
48.  
    
49.   /**
    
50.    * The SubscribeOptions structure lets you specify a custom queue to use for a specific 
51.    * subscription. SubscribeOptions结构允许您指定用于特定订阅的自定义队列
    
52.    * You can also set a default queue on a NodeHandle using the 
53.    * NodeHandle::setCallbackQueue() function. 
54.        * 还可以使用NodeHandle::setCallbackQueue()函数在节点句柄上设置默认队列
    
55.    *
    
56.    * AdvertiseOptions and AdvertiseServiceOptions offer similar functionality.
    
57.    * 对于话题发布者, 有 AdvertiseOptions 和 AdvertiseServiceOptions  可以使用
    
58.    */
    
59.   //第二步: 声明订阅或者发布选项, 然后和订阅器/发布器绑定在一起  
    
60.   ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::String>
61.         ("chatter", 1000, chatterCallbackCustomQueue, ros::VoidPtr(), &g_queue);
62.   ros::Subscriber sub = n.subscribe(ops);   
    
63.  
    
64.   ros::Subscriber sub2 = n.subscribe("chatter", 1000, chatterCallbackMainQueue);
    
65.  
    
66.   //第三步: 声明线程.
67.   boost::thread chatter_thread(callbackThread); 
68.   ROS_INFO_STREAM("Main thread id=" << boost::this_thread::get_id());
    
69.  
    
70.   ros::Rate r(1);
    
71.   while (n.ok())
    
72.   {
    
73.     ros::spinOnce();
    
74.     r.sleep(); 
    
75.   }
    
76.  
    
77.   chatter_thread.join();
    
78.  
    
79.   return 0;
    
80. }

MultiThreadedSpinner对象
    允许您指定用于调用回调的线程数。如果没有指定显式的#，它将使用系统上可用的硬件线程的#。这里我们显式地指定4个线程

点击(此处)折叠或打开

1. ros::MultiThreadedSpinner s(4);
   
2. ros::spin(s)  //主要是 spin

AsyncSpinner 
    可以认为是写在while外边的spinOnce. while内仅仅需要sleep即可. 这样主线程,子线程可以并行执行.

点击(此处)折叠或打开

1.   ros::AsyncSpinner s(4);
   
2.   s.start();
   
3. 
   
4.   ros::Rate r(5);
   
5.   while (ros::ok())
   
6.   {
   
7.     ...
   
8.     r.sleep();
   
9.   }

多个不同的topic 用一个回调处理

点击(此处)折叠或打开

1. void callback(const ImageConstPtr& image_color_msg,
   
2.         const ImageConstPtr& image_depth_msg,
   
3.         const CameraInfoConstPtr& info_color_msg,
   
4.         const CameraInfoConstPtr& info_depth_msg)
5. {
   
6.   ....
   
7. }
   
8. 
   
9. int main(int argc, char** argv)
   
10. {
    
11.     ros::init(argc, argv, "vision_node");
    
12.  
    
13.     ros::NodeHandle nh;
    
14.  
    
15.     //不同类型的topic订阅
    
16.     message_filters::Subscriber<Image> image_color_sub(nh,"/camera/rgb/image_raw", 1);
    
17.     message_filters::Subscriber<Image> image_depth_sub(nh,"/camera/depth_registered/image_raw", 1);
    
18.     message_filters::Subscriber<CameraInfo> info_color_sub(nh,"/camera/rgb/camera_info", 1);
    
19.     message_filters::Subscriber<CameraInfo> info_depth_sub(nh,"/camera/depth_registered/camera_info", 1);
    
20.     pointcloud_pub = nh.advertise<PointCloud> ("mypoints", 1);
    
21.  
    
22.     //定义规则
23.     typedef sync_policies::ApproximateTime<Image, Image, CameraInfo, CameraInfo> MySyncPolicy;
    
24.     //规则化实例
    
25.     Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_color_sub, image_depth_sub, info_color_sub, info_depth_sub);
    
26.     //绑定回调
    
27.     sync.registerCallback(boost::bind(&callback, _1, _2, _3, _4));  // _1 代表实力的第一个对象.
    
28.  
    
29.     ros::spin();
    
30.  
    
31.     return 0;
    
32. }

利用boost_bind向消息回调函数传入多个参数

点击(此处)折叠或打开

1. void init()
   
2.   {
   
3.     node_handle_.subscribe<std_msgs::String>("chatter", 1000, boost::bind(chatterCallback, _1, "User 1"));
   
4.     node_handle_.subscribe<std_msgs::String>("chatter", 1000, boost::bind(chatterCallback, _1, "User 2"));
   
5.     node_handle_.subscribe<std_msgs::String>("chatter", 1000, boost::bind(chatterCallback, _1, "User 3"));
   
6.   }
   
7.  
   
8.   void chatterCallback(const std_msgs::String::ConstPtr& msg, std::string user_string)
   
9.   {
   
10.     ROS_INFO("I heard: [%s] with user string [%s]", msg->data.c_str(), user_string.c_str());
    
11.   }

点击(此处)折叠或打开

1. #include "ros/ros.h"
   
2.  
   
3. /**
   
4.  * This tutorial demonstrates the use of timer callbacks.
   
5.  */
   
6.  
   
7. void callback1(const ros::TimerEvent&)
   
8. {
   
9.   ROS_INFO("Callback 1 triggered");
   
10. }
    
11.  
    
12. void callback2(const ros::TimerEvent&)
    
13. {
    
14.   ROS_INFO("Callback 2 triggered");
    
15. }
    
16.  
    
17. int main(int argc, char **argv)
    
18. {
    
19.   ros::init(argc, argv, "talker");
    
20.   ros::NodeHandle n;
    
21.  
    
22.   ros::Timer timer1 = n.createTimer(ros::Duration(0.1), callback1);  //0.1秒一次
    
23.   ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2);  //1秒一次
    
24.  
    
25.   ros::spin();
    
26.  
    
27.   return 0;
    
28. }

定时器回调带入参数

点击(此处)折叠或打开

1. void timerCallback(const ros::TimerEvent&, ros::Publisher vel_pub)
   
2. {
   
3. ...
   
4. }
   
5. 
   
6. ros::Publisher vel_pub = nh.advertise<han_agv::VelEncoder>("HansAGV/encoder_vel", 1);
   
7. ros::Timer decoder_timer = nh.createTimer(ros::Duration(0.1), boost::bind(timerCallback, _1, vel_pub))

话题发布时的连接和断开时间出发 notify_connect

点击(此处)折叠或打开

1. #include "ros/ros.h"
   
2. #include "std_msgs/String.h"
   
3.  
   
4. #include <sstream>
   
5.  
   
6. /**
   
7.  * This tutorial demonstrates how to get a callback when a new subscriber connects
   
8.  * to an advertised topic, or a subscriber disconnects.
   
9.  */
   
10.  
    
11. uint32_t g_count = 0;
    
12.  
    
13. void chatterConnect(const ros::SingleSubscriberPublisher& pub)
    
14. {
    
15.   std_msgs::String msg;
    
16.   std::stringstream ss;
    
17.   ss << "chatter connect #" << g_count++;
    
18.   ROS_INFO("%s", ss.str().c_str());
    
19.   msg.data = ss.str();
    
20.  
    
21.   pub.publish(msg); // This message will get published only to the subscriber that just connected
    
22. }
    
23.  
    
24. void chatterDisconnect(const ros::SingleSubscriberPublisher&)
    
25. {
    
26.   ROS_INFO("chatter disconnect");
    
27. }
    
28.  
    
29. int main(int argc, char **argv)
    
30. {
    
31.   ros::init(argc, argv, "notify_connect");
    
32.   ros::NodeHandle n;
    
33.  
    
34.   /**
    
35.    * This version of advertise() optionally takes a connect/disconnect callback
    
36.     * advertise 有连接和断开的回调注册
    
37.    */
    
38.   ros::Publisher pub = n.advertise<std_msgs::String>("chatter", 1000, chatterConnect, chatterDisconnect);  
    
39.  
    
40.   ros::spin();
    
41.  
    
42.   return 0;
    
43. }