ros之Callbacks and Spinning

callback() 和spin()函数

#include <ros/ros.h>
#include <iostream>

void callback()
{
 std::cout << "enter callback function!" << std::endl;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "test_node");
  ros::NodeHandle nh;
  ros::NodeHandle nh_pri("~");
  
  ros::Subscriber sub = nh.subscribe(topic, 10, callback);
  ros::spin();
  return 0;
}

下面是ros wiki的解释。

Initialize the ROS system

Subscribe to the chatter topic

Spin, waiting for messages to arrive
4.When a message arrives, the chatterCallback() function is called

意思大概是，subscribe一个topic之后，并不会直接进入callback函数，而是往下执行，直到遇见了ros::spin()。这也就是说， ros::spin()后面的内容不会执行，除非ros::ok() return false（用户按下ctrl+c，或者调用了ros::shutdown()函数）。还有就是，如果程序中没有ros::spin(), 那么就永远不会进入callback函数了。

ros::spin() will exit once ros::ok() returns false, which means ros::shutdown() has been called, either by the default Ctrl-C handler, the master telling us to shutdown, or it being called manually.

关于变量的作用域问题

#include <ros/ros.h>
#include <iostream>

void callback1()
{
 std::cout << "enter callback1 function!" << std::endl;
}

void callback2()
{
 std::cout << "enter callback2 function!" << std::endl;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "test_node");
  ros::NodeHandle nh;
  ros::NodeHandle nh_pri("~");
  
  ros::Subscriber sub1 = nh.subscribe(topic1, 10, callback1);
  if(1)
  {
    ros::Subscriber sub2 = nh.subscribe(topic2, 10, callback2);
  }
  ros::spin();
  return 0;
}

问题：运行程序后可以直到，并没有进入callback2函数，并且查看rosnode info，发现并没有subscribe topic2。
解答：sub2是局部变量，也就是说作用域只能在if语句里面，出了if语句该局部变量就会注销掉，因此运行程序不会subscribe topic2。解决方法就是在if语句外面定义，在if语句里面声明。

#include <ros/ros.h>
#include <iostream>

void callback1()
{
 std::cout << "enter callback1 function!" << std::endl;
}

void callback2()
{
 std::cout << "enter callback2 function!" << std::endl;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "test_node");
  ros::NodeHandle nh;
  ros::NodeHandle nh_pri("~");
  
  ros::Subscriber sub1 = nh.subscribe(topic1, 10, callback1);
  ros::Subscriber sub2；
  if(1)
  {
    sub2 = nh.subscribe(topic2, 10, callback2);
  }
  ros::spin();
  return 0;
}

谨记谨记！

单线程Spining

方法一：

ros::init(argc, argv, "my_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe(...);
...
ros::spin();

方法二：

ros::Rate r(10); // 10 hz
while (should_continue)
{
  ... do some work, publish some messages, etc. ...
  ros::spinOnce();
  r.sleep();
}

多线程Spining

方法一：ros::MultiThreadedSpinner

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

MultiThreadedSpinner is a blocking spinner, similar to ros::spin(). You can specify a number of threads in its constructor, but if unspecified (or set to 0), it will use a thread for each CPU core.

方法二： ros::AsyncSpinner (since 0.10)

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

A more useful threaded spinner is the AsyncSpinner. Instead of a blocking spin() call, it has start() and stop() calls, and will automatically stop when it is destroyed. An equivalent use of AsyncSpinner to the MultiThreadedSpinner

单线程和多线程调用callback实例

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/CompressedImage.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr input_msg)
{
  std::cout << "Enter into cloud callback function!" << std::endl;
  while(1)
  {
    int i = 0;
  }
}

void image0Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image0 callback function!" << std::endl;
  for (size_t i = 0; i < 100000; ++i)
  {
    int sum;
    sum += i;
  }
}

void image1Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image1 callback function!" << std::endl;
  for (size_t i = 0; i < 100000; ++i)
  {
    int sum;
    sum += i;
  }
}

void image2Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image2 callback function!" << std::endl;
  for (size_t i = 0; i < 100000; ++i)
  {
    int sum;
    sum += i;
  }
}

void image3Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image3 callback function!" << std::endl;
  for (size_t i = 0; i < 100000; ++i)
  {
    int sum;
    sum += i;
  }
}
void image4Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image4 callback function!" << std::endl;
  for (size_t i = 0; i < 100000; ++i)
  {
    int sum;
    sum += i;
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "single threaded spining");
  ros::NodeHandle nh;
  ros::NodeHandle priv_nh("~");
  
  ros::Subscriber image0_sub = nh.subscribe("/camera_front/image_color/compressed", 10, image0Callback);
  ros::Subscriber image1_sub = nh.subscribe("/camera_left1/image_color/compressed", 10, image1Callback);
  ros::Subscriber image2_sub = nh.subscribe("/camera_left2/image_color/compressed", 10, image2Callback);
  ros::Subscriber image3_sub = nh.subscribe("/camera_right1/image_color/compressed", 10, image3Callback);
  ros::Subscriber image4_sub = nh.subscribe("/camera_right2/image_color/compressed", 10, image4Callback);
  ros::Subscriber cloud_sub = nh.subscribe("/rslidar_points", 10, cloudCallback);
  ros::MultiThreadedSpinner spinner(5);
  spinner.spin();
  // ros::spin();
  return 0;
}

注：本程序通过设计一个死循环来判断是不是调用了多线程，对于单线程来说会堵塞，但是对于多线程来说只有一个线程堵塞。

参考

ROS/Tutorials/WritingPublisherSubscriber(c++) - ROS Wiki

Callbacks and Spinning