猪事大吉 - ROS 学习 - BigRobot - 第二日 - ROS多线程部分

ros::spin()和ros::spinOnce()

什么时候用ros::spin()和ros::spinOnce()
当只有话题订阅时, 则程序只需要在话题消息来临时响应回调函数即可, 即为使用 ros::spin(), 即为死循环.

当还有其他事情要做,是仅仅把时间片给ROS系统用,可以用来响应一些订阅回调(也可能什么也没做)或者给ROS系统做一些其他事情, 完毕后ROS系统再把时间片回到程序中来.
一般的用法:
    ros::Rate loop_rate(10);
    while (ros::ok()) {
        do_something();
        ros::spinOnce();
        loop_rate.sleep();
    }

自定义消息队列 + 处理线程 

#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "std_msgs/String.h"

#include <boost/thread.hpp>
/**
 * tutorial demonstrates the use of custom separate callback queues that can be processed
 * independently, whether in different threads or just at different times.
 * 演示了自定义独立回调队列的使用，
 * 这些回调队列可以在不同的线程中独立处理，也可以在不同的时间进行处理。
 */

void chatterCallbackMainQueue(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO_STREAM("I heard: [ " << msg->data << "] in thread [" << boost::this_thread::get_id() << "] (Main thread)");
}

//主线程中的调用
void chatterCallbackCustomQueue(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO_STREAM("I heard: [ " << msg->data << "] in thread ["
        << boost::this_thread::get_id() << "]");
}

/**
 * The custom queue used for one of the subscription callbacks
 */
ros::CallbackQueue g_queue; //第一步:用于订阅回调的自定义队列

void callbackThread()
{
  ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());

  ros::NodeHandle n;
  while (n.ok())
  {
    //第四步: 执行自定义队列中的回调函数.
        // CallbackQueue类有两种调用内部回调的方法:callAvailable()和callOne()。
        // callAvailable()将获取队列中当前的所有内容并调用它们。callOne()将简单地调用队列上最古老的回调。
    g_queue.callAvailable(ros::WallDuration(0.01));
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener_with_custom_callback_processing");
  ros::NodeHandle n;

  /**
   * The SubscribeOptions structure lets you specify a custom queue to use for a specific
   * subscription. SubscribeOptions结构允许您指定用于特定订阅的自定义队列
   * You can also set a default queue on a NodeHandle using the
   * NodeHandle::setCallbackQueue() function.
       * 还可以使用NodeHandle::setCallbackQueue()函数在节点句柄上设置默认队列
   *
   * AdvertiseOptions and AdvertiseServiceOptions offer similar functionality.
   * 对于话题发布者, 有 AdvertiseOptions 和 AdvertiseServiceOptions  可以使用
   */
  //第二步: 声明订阅或者发布选项, 然后和订阅器/发布器绑定在一起
  ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::String>
        ("chatter", 1000, chatterCallbackCustomQueue, ros::VoidPtr(), &g_queue);
  ros::Subscriber sub = n.subscribe(ops);

  ros::Subscriber sub2 = n.subscribe("chatter", 1000, chatterCallbackMainQueue);

  //第三步: 声明线程.
  boost::thread chatter_thread(callbackThread);
  ROS_INFO_STREAM("Main thread id=" << boost::this_thread::get_id());

  ros::Rate r(1);
  while (n.ok())
  {
    ros::spinOnce();
    r.sleep();
  }

  chatter_thread.join();

  return 0;
}

MultiThreadedSpinner对象
    允许您指定用于调用回调的线程数。如果没有指定显式的#，它将使用系统上可用的硬件线程的#。这里我们显式地指定4个线程

ros::MultiThreadedSpinner s(4);
ros::spin(s)  //主要是 spin

AsyncSpinner 
    可以认为是写在while外边的spinOnce. while内仅仅需要sleep即可. 这样主线程,子线程可以并行执行.

  ros::AsyncSpinner s(4);
  s.start();

  ros::Rate r(5);
  while (ros::ok())
  {
    ...
    r.sleep();
  }

多个不同的topic 用一个回调处理

void callback(const ImageConstPtr& image_color_msg,
        const ImageConstPtr& image_depth_msg,
        const CameraInfoConstPtr& info_color_msg,
        const CameraInfoConstPtr& info_depth_msg)
{
  ....
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "vision_node");

    ros::NodeHandle nh;

    //不同类型的topic订阅
    message_filters::Subscriber<Image> image_color_sub(nh,"/camera/rgb/image_raw", 1);
    message_filters::Subscriber<Image> image_depth_sub(nh,"/camera/depth_registered/image_raw", 1);
    message_filters::Subscriber<CameraInfo> info_color_sub(nh,"/camera/rgb/camera_info", 1);
    message_filters::Subscriber<CameraInfo> info_depth_sub(nh,"/camera/depth_registered/camera_info", 1);
    pointcloud_pub = nh.advertise<PointCloud> ("mypoints", 1);

    //定义规则
    typedef sync_policies::ApproximateTime<Image, Image, CameraInfo, CameraInfo> MySyncPolicy;
    //规则化实例
    Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_color_sub, image_depth_sub, info_color_sub, info_depth_sub);
    //绑定回调
    sync.registerCallback(boost::bind(&callback, _1, _2, _3, _4));  // _1 代表实力的第一个对象.

    ros::spin();

    return 0;
}

利用boost_bind向消息回调函数传入多个参数

void init()
  {
    node_handle_.subscribe<std_msgs::String>("chatter", 1000, boost::bind(chatterCallback, _1, "User 1"));
    node_handle_.subscribe<std_msgs::String>("chatter", 1000, boost::bind(chatterCallback, _1, "User 2"));
    node_handle_.subscribe<std_msgs::String>("chatter", 1000, boost::bind(chatterCallback, _1, "User 3"));
  }

  void chatterCallback(const std_msgs::String::ConstPtr& msg, std::string user_string)
  {
    ROS_INFO("I heard: [%s] with user string [%s]", msg->data.c_str(), user_string.c_str());
  }

#include "ros/ros.h"

/**
 * This tutorial demonstrates the use of timer callbacks.
 */

void callback1(const ros::TimerEvent&)
{
  ROS_INFO("Callback 1 triggered");
}

void callback2(const ros::TimerEvent&)
{
  ROS_INFO("Callback 2 triggered");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;

  ros::Timer timer1 = n.createTimer(ros::Duration(0.1), callback1);  //0.1秒一次
  ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2);  //1秒一次

  ros::spin();

  return 0;
}

定时器回调带入参数

void timerCallback(const ros::TimerEvent&, ros::Publisher vel_pub)
{
...
}

ros::Publisher vel_pub = nh.advertise<han_agv::VelEncoder>("HansAGV/encoder_vel", 1);
ros::Timer decoder_timer = nh.createTimer(ros::Duration(0.1), boost::bind(timerCallback, _1, vel_pub))

话题发布时的连接和断开时间出发 notify_connect

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>

/**
 * This tutorial demonstrates how to get a callback when a new subscriber connects
 * to an advertised topic, or a subscriber disconnects.
 */

uint32_t g_count = 0;

void chatterConnect(const ros::SingleSubscriberPublisher& pub)
{
  std_msgs::String msg;
  std::stringstream ss;
  ss << "chatter connect #" << g_count++;
  ROS_INFO("%s", ss.str().c_str());
  msg.data = ss.str();

  pub.publish(msg); // This message will get published only to the subscriber that just connected
}

void chatterDisconnect(const ros::SingleSubscriberPublisher&)
{
  ROS_INFO("chatter disconnect");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "notify_connect");
  ros::NodeHandle n;

  /**
   * This version of advertise() optionally takes a connect/disconnect callback
    * advertise 有连接和断开的回调注册
   */
  ros::Publisher pub = n.advertise<std_msgs::String>("chatter", 1000, chatterConnect, chatterDisconnect);

  ros::spin();

  return 0;
}