• callback() 和spin()函式
• 單執行緒callback模式
  • 簡介
  • 示例
    • 點雲callback
• 多執行緒callback模式
  • 簡介
  • 示例
    • 影象點雲多執行緒
• 參考

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的解釋。

1. Initialize the ROS system
2. Subscribe to the chatter topic
3. 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;
}

謹記謹記！

單執行緒callback模式

簡介

方法一：

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();
}

示例

點雲callback

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>

ros::Publisher g_cloud_pub;
int g_frame_num = 0;

void callback(const sensor_msgs::PointCloud2ConstPtr input_msg)
{
  std::cout << "enter into callback fuction!" << std::endl;
  pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::fromROSMsg(*input_msg, *input_cloud);

  pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);
  std::vector<int> indice;
  pcl::removeNaNFromPointCloud(*input_cloud, *output_cloud, indice);

  sensor_msgs::PointCloud2 output_msg;
  pcl::toROSMsg(*output_cloud, output_msg);
  output_msg.header = input_msg->header;
  g_cloud_pub.publish(output_msg);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "callback");
  ros::NodeHandle nh;
  ros::NodeHandle priv_nh("~");
  
  std::string cloud_topic;
  priv_nh.param<std::string>("cloud_topic", cloud_topic, "input_points");
  ros::Subscriber cloud_sub = nh.subscribe(cloud_topic, 10, callback);
  g_cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/output_points", 10);

  ros::spin();
  return 0;
}

多執行緒callback模式

簡介

方法一：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

示例

影象點雲多執行緒

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/CompressedImage.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <iostream>
#include <string>

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr input_msg)
{
  std::cout << "Enter into cloud callback function!" << std::endl;
}

void image0Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image0 callback function!" << std::endl;
}

void image1Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image1 callback function!" << std::endl;
}

void image2Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image2 callback function!" << std::endl;
}

void image3Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image3 callback function!" << std::endl;
}

void image4Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image4 callback function!" << std::endl;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "multi callback");
  ros::NodeHandle nh;
  ros::NodeHandle priv_nh("~");
  
  std::string cloud_topic;
  priv_nh.param<std::string>("cloud_topic", cloud_topic, " ");

  int camera_num = 0; 
  priv_nh.param<int>("camera_num", camera_num, 1);
  std::vector<std::string> camera_topic_v(camera_num);
  for (size_t i = 0; i < camera_num; ++i)
  {
    std::string camera_topic = "camera"+ std::to_string(i) + "_topic";
    priv_nh.param<std::string>(camera_topic, camera_topic_v[i], "/image_color/compressed");
  }
  
  ros::Subscriber cloud_sub = nh.subscribe(cloud_topic, 10, cloudCallback);
  ros::Subscriber image0_sub = nh.subscribe(camera_topic_v[0], 10,  image0Callback);
  ros::Subscriber image1_sub = nh.subscribe(camera_topic_v[1], 10,  image1Callback);
  ros::Subscriber image2_sub = nh.subscribe(camera_topic_v[2], 10,  image2Callback);
  ros::Subscriber image3_sub = nh.subscribe(camera_topic_v[3], 10,  image3Callback);
  ros::Subscriber image4_sub = nh.subscribe(camera_topic_v[4], 10,  image4Callback);
   
  ros::MultiThreadedSpinner spinner(camera_num + 1);
  spinner.spin();
  return 0;
}

注：本程式通過設計一個死迴圈來判斷是不是呼叫了多執行緒， 對於單執行緒來說會堵塞，但是對於多執行緒來說只有一個執行緒堵塞。

參考

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

Callbacks and Spinning