ROS中的多线程使用方法

1、ROS的线程问题

在ROS使用中，经常会需要订阅各种消息以及处理各种队列。而ROS本身默认的是单线程的。一般情况下每次数据处理时间不是很久的话能够满足程序的正常运行，但是在某些特殊情况下，例如程序中存在两个服务，其中一个服务开始后会循环执行，直到另一个服务信号对其进行中止时，这种单线程的模式就无法正常使用了。

例如下面这个例子：

bool service_two::get_img(std_srvs::Empty::Request  &req,std_srvs::Empty::Response &res)
{
    int cnt = 0;
    while (1)
    {
      if (cnt > 399) {
        break;
      }      
      cnt +=1;
      std::cout<<"while photo_flag:"<<cnt<<std::endl;      
      sleep(1);
      if(photo_flag==false)
          break;
    }
  return true;
}

bool service_two::set_flag(std_srvs::SetBool::Request  &req,std_srvs::SetBool::Response &res)
{
  if (req.data){
    photo_flag=true;
  }else{
    photo_flag=false;
  }
  std::cout<<"photo_flag:"<<photo_flag<<std::endl;
  return true;
}

这里是两个不同服务的执行端，可以看到上面一个服务的执行退出条件是受到下面一个程序影响的。如果我们使用单线程去运行，会发生这样子的情况：

上述3个终端，左边第一个终端为两个服务的服务端，右上方的为第一个服务的客户端，右下方的终端为第二个服务的客户端。程序开始时，先打开服务端，然后打开第一个服务的客户端，则服务端开始循环输出消息。此时如果打开第二个服务的客户端，会发现第二个客户端并不能中断第一个客户端的循环。而最终的程序需要循环执行400次直到满足计数条件后整个程序才会退出，而也就是到这个时候第二个服务的请求才会开始执行。

可以看到整个程序的线程在第一个服务结束之前其实是被阻塞了，后面的服务请求直到上一个服务完全执行完才会执行。因此无法实现我们需要的逻辑。

2、ROS自带的多线程指令

其实ROS本身开发了两个非常简单的多线程指令，只需要简单的几行代码就能解决上面的问题：

2.1、ros::MultiThreadedSpinner

MultiThreadedSpinner类似于ros::spin()，在构造过程中可以指定它所用线程数，但如果不指定线程数或者线程数设置为0，它将在每个cpu内核开辟一个线程。

用法如下：

ros::MultiThreadedSpinner spinner(4); // Use 4 threads
spinner.spin(); // spin() will not return until the node has been shutdown

2.2、ros::AsyncSpinner

AsyncSpinner比MultiThreadedSpinner更优，它有start() 和stop() 函数，并且在销毁的时候会自动停止。下面的用法等价于上面的MultiThreadedSpinner例子：

ros::AsyncSpinner spinner(4); // Use 4 threads
spinner.start();
ros::waitForShutdown();

注意：ros::waitForShutdown() 函数，不会触发单独spin()，所以该例子将会4个线程同时循环。

Please note that the ros::waitForShutdown() function does not spin on its own, so the example above will spin with 4 threads in total.

比如上面的代码如果我们采用多线程执行的话，则需要在main函数中增加对应的几行代码即可，然后我们就可以在任意需要的时间停止我们的循环：

贴一个小代码：

#include "ros/ros.h"
#include "std_srvs/Empty.h"
#include "std_srvs/SetBool.h"

using namespace std;
class service_two
{
    public:
    service_two();
    bool photo_flag;
    ros::ServiceServer service_once;
    ros::ServiceServer service_on;
    private:
    bool get_img(std_srvs::Empty::Request  &req,std_srvs::Empty::Response &res);
    bool set_flag(std_srvs::SetBool::Request  &req,std_srvs::SetBool::Response &res);
};
service_two::service_two()
{
    ros::NodeHandle n;
    ros::NodeHandle nh;
    photo_flag = true;  
    service_once = n.advertiseService("photo_once", &service_two::get_img,this);
    service_on = n.advertiseService("photo_on", &service_two::set_flag,this);
}
bool service_two::get_img(std_srvs::Empty::Request  &req,std_srvs::Empty::Response &res)
{
    int cnt = 0;
    while (1)
    {
      if (cnt > 399) {
        break;
      }      
      cnt +=1;
      std::cout<<"while photo_flag:"<<cnt<<std::endl;      
      sleep(1);
      if(photo_flag==false)
          break;
    }
  return true;
}

bool service_two::set_flag(std_srvs::SetBool::Request  &req,std_srvs::SetBool::Response &res)
{
  if (req.data){
    photo_flag=true;
  }else{
    photo_flag=false;
  }
  std::cout<<"photo_flag:"<<photo_flag<<std::endl;
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "idmvs_server");
  service_two service_two;
  //启动多线程只要启动下面三行代码就可以了
  ros::AsyncSpinner spinner(2);           //非阻塞式的spinner, 可以使用start和stop进行启停
  spinner.start();                        //启动线程
  ros::waitForShutdown();                 //等待退出
  ros::spin();                            //这个应该可以省略
  return 0;
}

完整的代码在：

参考：

https://blog.csdn.net/DinnerHowe/article/details/80045188