ros之callback函数

目录

• 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