ROS学习 -- 多线程

ROS学习 – 多线程

ros::spin() 与 ros::spinOnce()联系与区别

• ros::spin()被动的接收topic，或者说纯粹的接收topic.

• ros::spinOnce()可以根据自己的需求设置接收频率。更加主动灵活。

• ros::spinOnce() 可以配合其它函数，一起放在while循环中处理。

  #include "ros/ros.h"
  #include "std_msgs/String.h"
   
  void chatterCallback(const std_msgs::String::ConstPtr& msg)
  {
      ROS_INFO("I heard: [%s]", msg->data.c_str());
  }
   
  int main(int argc, char **argv)
  {
      ros::init(argc, argv, "listener");
      ros::NodeHandle n;
      ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);//1000为回调函数的队列长度
      ros::spin();
      return 0;
  }
  
  

  #include "ros/ros.h"
  #include "std_msgs/String.h"
    
  void chatterCallback(const std_msgs::String::ConstPtr& msg)
  {
      /*...TODO...*/ 
  }
    
  int main(int argc, char **argv)
  {
      ros::init(argc, argv, "listener");
      ros::NodeHandle n;
      ros::Subscriber sub = n.subscribe("chatter", 2, chatterCallback);
    
      ros::Rate loop_rate(5);　//频率为５hz
      while (ros::ok())
      {
          /*...TODO...*/ 
          ros::spinOnce();
          loop_rate.sleep(); //配合执行频率，sleep一段时间，然后进入下一个循环。
      }
      return 0;
  }
  
  

C++

可以用 ros::spin() 或 ros::spinOnce()。

1 单线程

在只有一个Spinner thread的情况下，callback queue只能顺序执行
不管有多少个Subscriber，节点都只能顺序执行回调（回调频率取决于最低频率）。

#include <thread>
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 void CallbackA(const std_msgs::String::ConstPtr &msg)
 {
     std::this_thread::sleep_for(std::chrono::seconds(2));
     ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 void CallbackB(const std_msgs::String::ConstPtr &msg)
 {
     ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "listener");
     ros::NodeHandle n;
     ros::Subscriber sub_b = n.subscribe("B/message", 1, CallbackB);
 ​
     ros::Subscriber sub_a = n.subscribe("A/message", 1, CallbackA);
     ros::spin();
 ​
     return 0;
 }

2 多线程：订阅多个Topic，多个Spinner threads

单个线程之间调用频率相互不影响。
MultiThreadedSpinner可初始化线程的数量，这里因为有两个subscriber，就选择了2。Callback依然是2s调用一次。

#include <thread>
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 void CallbackA(const std_msgs::String::ConstPtr &msg)
 {
     std::this_thread::sleep_for(std::chrono::seconds(2));
     ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 void CallbackB(const std_msgs::String::ConstPtr &msg)
 {
     ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "listener");
     ros::NodeHandle n;
     ros::Subscriber sub_b = n.subscribe("B/message", 1, CallbackB);
 ​
     ros::Subscriber sub_a = n.subscribe("A/message", 1, CallbackA);
     //改变的地方
     ros::MultiThreadedSpinner spinner(2);
     spinner.spin();
 ​
     return 0;
 }

3 订阅一个Topic，多个Spinner threads

设置SubscribeOptions的allow_concurrent_callbacks为true，就可以让同一个Callback并行执行。
假设只有一个Topic, 发布端的频率比较高，我们又想尽可能多地响应消息，因此我们可以设置多个Spinner，但是单纯地像上一小节一样使用MultiThreadedSpinner是不行的，ros作了限制，默认阻止并行处理一个Callback，我们需要更改Suscriber的配置：

 #include <thread>
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 ​
 void ChatterCallback(const std_msgs::String::ConstPtr& msg) {
   ROS_INFO(" I heard: [%s]", msg->data.c_str());
   std::this_thread::sleep_for(std::chrono::seconds(2));
 }
 int main(int argc, char **argv) {
   ros::init(argc, argv, "listener");
   
   ros::NodeHandle n;
   ros::SubscribeOptions ops;
   ops.init<std_msgs::String>("A/message", 1, ChatterCallback);
   ops.allow_concurrent_callbacks = true;
   ros::Subscriber sub1 = n.subscribe(ops);
   ros::MultiThreadedSpinner spinner(2);
   
   spinner.spin();
   return 0;
 }

4 订阅多个Topic，每个Subscriber一个Callback queue

给每一个subscriber创建一个单独的callback queue，这样就解决了即使用了MultiThreadedSpinner但所有的callback依然在同一个queue排除执行的问题，此方法用来解决subscriber的优先级问题。

 #include <thread>
 #include <ros/callback_queue.h>
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 void CallbackA(const std_msgs::String::ConstPtr& msg) {
   ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 void CallbackB(const std_msgs::String::ConstPtr& msg) {
   ROS_INFO(" I heard: [%s]", msg->data.c_str());
 }
 int main(int argc, char **argv) {
   ros::init(argc, argv, "listener");
   ros::NodeHandle n;
   ros::Subscriber sub_b = n.subscribe("MessageB", 1, CallbackB);
   
   ros::NodeHandle n_a;
   ros::CallbackQueue callback_queue_a;
   n_a.setCallbackQueue(&callback_queue_a);
   ros::Subscriber sub_a = n_a.subscribe("MessageA", 1, CallbackA);
   std::thread spinner_thread_a([&callback_queue_a]() {
     ros::SingleThreadedSpinner spinner_a;
     spinner_a.spin(&callback_queue_a);
   });
   ros::spin();
   spinner_thread_a.join();
   return 0;
 }

Python

只能用ros::spin() 。
解决rospy.spin()一直循环，无法执行剩余程序：

• 解决方案1：多线程

  import rospy
  import std_msgs.msg
  import geometry_msgs.msg
  from sensor_msgs.msg import LaserScan
  import threading
   
  def thread_job():
      rospy.spin()
   
  laser = []
  def save_laser(laser_data):
      global laser
      laser = laser_data.ranges
   
  def listener():
      global laser
      rospy.init_node('save_image')
      add_thread = threading.Thread(target = thread_job)
      add_thread.start()
      
      rospy.Subscriber('/scan', LaserScan, save_laser, queue_size=1)
      rospy.sleep(1)
   
      while(1):
          print(laser)
          
   
  if __name__ == '__main__':
      listener()
  

• 解决方案2：使用rospy.wait_for_message(topic, topic_type, timeout)

  import rospy
  from sensor_msgs.msg import LaserScan
  def listener():
      rospy.init_node('get_laser')
      rospy.sleep(2)
      data = rospy.wait_for_message("/scan", LaserScan, timeout=None)
      print(data.ranges)
     
  if __name__ == '__main__':
      listener()
  

参考：

1. 知乎：精讲多线程之MultiThreadedSpinner
2. CSDN：解决rospy.spin()一直循环，无法执行剩余程序
3. CSDN：rosspin、rosspinOnce及多线程订阅（内容比较多，未细看，记录备用）

本文参考文档均以超链接形式在文中给出。
以上内容根据自己理解和实践所写，如有错误，请批评指正。