【ROS】在类中实现自定义多线程回调

在ros中，我们常用的回调处理是ros::spin()或者ros::spinOnce()，但是，这两个是阻塞式单线程处理的，如果在所有的回调队列里面，其中有一个耗时过长，则会影响其他的回调。

以图像处理为例，比如在机器人运动过程中，我们需要对机器人采集到的图像进行处理，同时还需要订阅其他的话题做出其他的决策行为，在这里，由于图像处理占用时间较大，用ros::spin()或者ros::spinOnce()进行回调的话就可能出现很多问题。

此时我们可以考虑使用多线程回调，具体讲解见官网wiki，如下。

http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning

官网wiki尤其有指出：

Separating out callbacks into different queues can be useful for a number of reasons. Some examples include:

1.Long-running services. Assigning a service its own callback queue that gets serviced in a separate thread means that service is guaranteed not to block other callbacks.
2.Threading specific computationally expensive callbacks. Similar to the long-running service case, this allows you to thread specific callbacks while keeping the simplicity of single-threaded callbacks for the rest your application.

这里的意思非常明显，就是让我们把图像队列分离开来，再另起一个线程处理该回调。

如何实现以上内容呢？网上很多文章其实也有类似的答案，但是如何用类的方式实现，似乎并未见到相关的文章， 结合到本人的相关工作，我将用类实现的方式来写本篇文章。

既然需要进行自定义多线程的回调，首先我们就需要在订阅的时候进行一些自定义的处理，具体的说就是要充分利用ros::SubscribeOptions这个类进行实现。该类是封装可用于创建订阅服务器的所有选项。通过设置这个类，我们可以将图像的队列分离开来，以便于后续进行自定义线程的处理。

1、先在类的声明中提供自定义的回调队列(假设我们的类名为Rotation)：

ros::CallbackQueue m_image_queue;

2、然后在类定义中初始化阶段将该回调队列初始化到Subscriber中（getSubscribeOptions函数的定义实现见第3步）：

ros::SubscribeOptions ops = getSubscribeOptions(image_topic,1,&Rotation::image_callback,this, &m_image_queue);//image_topic为订阅的图像话题名称
m_img_subscriber = nh_.subscribe(ops);

注：这里很多其他的博客都是提示用以下这个函数进行实现，但是由于本篇文章是用类的方式进行实现，且SubscribeOptions不支持将类成员函数设置为回调函数，此方法并不能够奏效。所以，我们需要进行一些其他的处理。

ros::SubscribeOptions ops=ros::SubscribeOptions::create<bhand_controller::State>(
                "/bhand_node/state",
                1,
                state_callback,
                ros::VoidPtr(),
                &state_callback_queue
                )

3、通过查看SubscribeOptions类源码，发现可以用模板进行一个简单的转换处理，将如下代码添加进类声明中：

template <class M, class T>
ros::SubscribeOptions getSubscribeOptions(
    const std::string &topic, uint32_t queue_size,
    void (T::*fp)(const boost::shared_ptr<M const> &),
    T* obj,
    ros::CallbackQueueInterface* queue,
    const ros::TransportHints &transport_hints = ros::TransportHints()) {
    ros::SubscribeOptions ops;
    ops.template init<M>(topic, queue_size, boost::bind(fp, obj, _1));
    ops.callback_queue = queue;
    ops.transport_hints = transport_hints;
    return ops;
}

到了这里，我们就自定义了图像的一个队列了，接下来我们就需要定义一个线程对该图像队列进行回调处理工作了。这里，有两个方法实现自定义线程处理回调队列：

1、第一种方法使直接另起一个线程，利用回调队列的callAvailable或者callOne函数实现。

首先，在类声明中声明一个线程以及回调线程处理函数：

void image_callback_thread();
boost::thread* m_image_thread;

然后，在初始化Subscriber后开辟该线程：

m_image_thread = new boost::thread(boost::bind(&Rotation::image_callback_thread,this));//这里，Rotation是该类的类名

线程处理函数的类定义如下：

void Rotation::image_callback_thread()
{
    while (nh_.ok())
    {
        m_image_queue.callAvailable(ros::WallDuration(0.04));
    }
  
}

其中的timeout可自行设置；

这样用该方法的自定义的线程处理回调就可以实现了，不过执行完后要记得释放内存。

if (NULL != m_image_thread) 
{
    m_image_thread->interrupt();
    m_image_thread->join();
    delete m_image_thread;
}

2、第二种办法是利用ros::AsyncSpinner异步多线程处理特定回调队列。

首先在类声明中声明一个指向该多线程订阅器的指针：

ros::AsyncSpinner* m_image_spinner;

接下来，在初始化Subscriber后开辟该线程即可：

m_image_spinner = new ros::AsyncSpinner(1, &m_image_queue);
if (m_image_spinner->canStart())
{
    m_image_spinner->start();
}

这样就相当于开辟了另一个异步线程进行处理，同样执行完后要记得释放内存：

if (NULL != m_image_spinner) 
{
    //m_image_spinner->stop();
    delete m_image_spinner;
    ROS_ERROR("delete image spinner-----------");
}