7.26-roscpp_overview详解

roscpp_overview详细解读

roscpp_overview详细解读
参考
前言
为什么需要多线程？
学习记录
Callbacks and Spinning
ROS内部的线程模型
单线程Spinning
多线程Spinning
高级：使用多个回调函数队列
总结
原理总结
多线程使用
程序示例
帖子答案
The theory
Your case
Conclusion
Proposed solution

TOC

参考

• roscpp wiki
• ROS callbacks, threads, and spinning

前言

• 很多模块，比较基础的模块需要深入理解
• 有一些关于多线程编程的知识需要知道
• 主要学习一下模块的内部机理
  • Callbacks and Spinning
  • Publishers and Subscribers
  • Timers
  • NodeHandles
  • Logging
  • Time
  • CppStyleGuide

为什么需要多线程？

• 比如说，ROS需要接受相机传来的点云，并且检测平面，得到法向量，显示在RViz上，这样需要订阅sensormsgs::point2消息并且注册点云处理函数foo()，由于点云处理函数需要较长时间执行，所以如果没有多线程（是单线程spin），则在接收到一个点云消息数据后，订阅函数会阻塞，会一直执行处理函数，不会接收到下一个点云消息，此时如果没有定义消息接收队列，则点云数据会被丢弃，在RViz显示上也会很卡顿。

学习记录

Callbacks and Spinning

ROS内部的线程模型

• roscpp并不把线程模型暴露给程序员，这意味着，在代码背后，程序其实有很多线程在运行，比如网络管理，任务调度等。
• 但是roscpp却允许我们使用多线程来调用回调函数，这又意味着你必须写一句ros::spin()来使得其可以被调用

  They will have an effect on the subscription queue, since how fast you process your callbacks and how quickly messages are arriving determines whether or not messages will be dropped.

单线程Spinning

ros::init(argc, argv, "my_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe(...);
...
ros::spin();//如果不Ctrl+C或者出现异常，这个循环不会结束
ros::Rate r(10); // 10 hz
while (should_continue)
{
  ... do some work, publish some messages, etc. ...
  ros::spinOnce();
  r.sleep();
}//这个循环依然不会结束不是么
#include <ros/callback_queue.h>
ros::NodeHandle n;
while (ros::ok())
{
  ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1));//从一个全局队列中取出
}
#include <ros/callback_queue.h>

ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0));

多线程Spinning

• 从多个线程来调用回调函数
• 分阻塞型（同步）和非阻塞型（异步），主要表现为是否阻塞调用它的主调线程

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

高级：使用多个回调函数队列

• 默认情况下，回调函数都在一个全局队列里面，在特殊情况下，可以自定义多个回调函数队列以提高性能。
• 适用范围
  • 长时间运行的服务
  • 运行特别费时的回调函数

总结

原理总结

• spin()的目的是启动一个新的线程去获取队列中的回调函数并调用
  • 分单线程，同步多线程和异步多线程，这些都有内置的语句
  • 还可以自定义回调函数队列

多线程使用

• 非常耗时的运算建议放在主线程中

while(ros::ok())
{ 
     leg_detector.process(latest_scan);
     ros::spinOnce();
}

• 回调函数建议放简单函数，最多放数据复制函数

程序示例

• 建议查看 wall_extrator_pcl,cpp by Willow Garage Inc.
• 该程序接收点云数据和一个检测墙体的服务，有两个回调函数cloudCB()和wall_detect()，cloudCB()简单地将点云消息指针传递给类的私有常量指针，而服务回调函数根据需要执行检测墙体功能，向RViz发送Marker消息标记。
• 该程序在主线程中使用了同步2个线程spinner，两个线程分别执行两个回调函数（表面上看是这样没错，但实际上还得看操作系统调度）
• 两个线程间通过条件变量结合互斥量来实现线程同步

void cloudCb(const boost::shared_ptr<const sensor_msgs::PointCloud2>& cloud)
  {
    boost::mutex::scoped_lock lock(cloud_mutex_);
    cloud_msg_ = cloud;
    cloud_condition_.notify_all();
  }
  bool detect_wall (stereo_wall_detection::DetectWall::Request &req, stereo_wall_detection::DetectWall::Response &resp)
  {
    ros::Time start_time = ros::Time::now();

    // wait for new pointcloud
    boost::mutex::scoped_lock lock(cloud_mutex_);
    cloud_condition_.wait(lock);
    //......
  }
  int main (int argc, char** argv)
{
  ros::init (argc, argv, "wall_extractor");

  PlanarFit p;
  ros::MultiThreadedSpinner spinner(2);  // extra thread so we can receive cloud messages while in the service call
  spinner.spin();

  return (0);
}

帖子答案

• Briefly: It won't work as you expect and you probably do not need such a complex way of designing your node.

The theory

• When it comes to communication in roscpp, two kind of objects are handling callbacks:
  callback queuesspinnersA spinner is an object that has the ability to call the callbacks contained in a callback queue. A callback queue is the object in which each subscriber will add an element each time a message is received by resolving which kind of message should call which callbacks (and with which arguments).
• Regarding the spinners, there is currently three implementations available in roscpp:
  Single thread spinner: the default one, takes the messages contained in a callback queue and process the callbacks one by one while blocking the execution of the thread that called it.Multi-threaded spinner: spawns a couple of threads (configurable) that will execute callbacks in parallel when messages are received but blocks the execution of the thread that called it.Asynchronous spinner: spawns a couple of threads (configurable) that will execute callbacks in parallel while not blocking the thread that called it. The start/stop method allows to control when the callbacks start being processed and when it should stop.These objects may be instantiated manually in advanced ROS nodes but to avoid verbose initialization, analternative, object-less, API is provided through functions in the ROS namespace. Aka ros::spin(),ros::spinOnce() and so on. This API rely on a default callback queue implemented as a singleton which is accessible through the ros::getGlobalCallbackQueue() function.
• So basically when you call the ros::spinOnce() function, a single-thread spinner is created and its spin method is called once using the default callback queue (see init.cpp from roscpp).
• And to finish, when you create a Subscriber, you pass it a NodeHandle and each NodeHandle has an associated callbackqueue that default to the global one but which can be overridden using thegetCallbackQueue/setCallbackQueue methods.

Your case

• If you take a look at spinner.cpp you will see there is a mutex that make the SingleThreader thread-safe whilediscouraging you to use it in this case (line 48).

Conclusion

• what you do is safe but will trigger regularly ROS error messages as there is chances that several instances of ros::SpinOnce will be executed in parallel.

Proposed solution

• Considering your applications, I think your callbacks are just not written as they should. A callback should stay as simple and fast as possible. In most of the cases, a callback is just feeding ROS data to your own classes which is usually as simple as copying data (and maybe converting them). It will help you ensuring that your callbacks are thread-safe (if you want to convert your node to a nodelet for instance, one day) and avoid making "ros::Spin" blocking for a long time, even in the case you are using the single-threaded spinner.
• Typically, if you want to do time-consuming computations such as "leg detection", the callbacks are definitively not the place to do it.
• Just, copy your data to a LegDetector object instance and call in your main thread the method that will do the heavy work. In particular here, you really don't care about losing old messages if your node is not fast enough so there is really no reason to pay the extra-burden of all the multi-thread infrastructure. Use Boost.Bind for instance, to pass a reference to your LegDetector class to the callback that will just copy the relevant laser data into it.
• If, at some point, you need to synchronise the received data, use the message_filters API. In this case again, trying to use multi-thread to do so is definitively a bad idea.