ros多線程數據處理方法（在主函數中循環處理消息與回調函數循環衝突解決方法）

在ros節點中，主函數中不能設置長時間的循環，否則會影響回調函數的執行

主函數如下

#include <iostream>
#include "senser_bridge.h"
#include "nav.h"

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"nav_test");
    ROS_INFO("*******");
    SenserBridge senser_bridge;
    

    while(true){
        cout << "主函數" <<endl;
        sleep(1);
    }

    ros::spin();

}

SenserBridge類如下

SenserBridge::SenserBridge()
{
    imu_sub_ = nh_.subscribe("/insprobe/imu0", 200, &SenserBridge::ImuDataCollect, this);
    pose_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/pose",200);
}

//回調函數
void SenserBridge::ImuDataCollect(const sensor_msgs::Imu::ConstPtr &msg)
{
    sensor_msgs::Imu imu_data;
    imu_data = *msg;
    imu_data_.push_back(imu_data);
    UpdataImuNow();
    cout << "***回調函數***"<<endl;
}

執行該節點則會出現如下問題:

[ INFO] [1576593799.328774366]: *******
Start time is 1575361874
主函數
主函數
主函數
主函數
主函數
主函數
主函數
主函數
....

如果將主函數中ros：：spin（）放到while循環前面，則會導致while()循環不執行，主函數代碼修改如下：

#include "senser_bridge.h"
#include "nav.h"

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"nav_test");
    ROS_INFO("*******");
    SenserBridge senser_bridge;

    ros::spin();//放到while()循環前面了
    while(true){
        cout << "主函數" <<endl;
        sleep(1);
    }
    
}

則輸出結果只有回調函數中的值了

[ INFO] [1576594011.172570488]: *******
Start time is 1575361874
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***

ps.在同一個節點中，ros::spin()相當於單線程的subcriber循環，所有的subcriber和對應的回調函數都在這個線程中循環執行，當subcriber接受的topic頻率很高時，可能會出現上一個subcriber的回調函數還沒執行完，下一個subcriber的msg就來的情況，造成阻塞。可能通過多線程方式避免這種情況。參考：http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning

https://blog.csdn.net/wengge987/article/details/50619851

http://m.blog.chinaunix.net/uid-27875-id-5817906.html

https://yuzhangbit.github.io/tools/several-ways-of-writing-a-ros-node/

使用boost::thread的C++多線程

#include <iostream>
#include <string>
#include <boost/thread.hpp>
#include "senser_bridge.h"
#include "nav.h"

void A()
{
    while(true)
    {
        sleep(1);
        cout<< "線程函數" <<endl;
    }

}

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"nav_test");
    ROS_INFO("*******");
    SenserBridge senser_bridge;

    boost::thread server(A);//創建額外的線程，即可一起執行了

    ros::spin();

}

結果如下

[ INFO] [1576674662.700959004]: *******
Start time is 1575361874
線程函數
線程函數
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
***回調函數***
線程函數
***回調函數***

可見在執行回調函數中也會有線程函數的執行，即可將回調函數獲取的數據，放到線程函數中處理，而不用擔心計算阻塞導致消息接收不到。

參考https://www.boost.org/doc/libs/1_67_0/doc/html/thread/thread_management.html#thread.thread_management.tutorial

https://www.jianshu.com/p/5e394f6aa4dc

https://www.cnblogs.com/liaocheng/p/4978398.html