ROS多線程訂閱

原文地址：http://blog.csdn.net/yaked/article/details/50776224

背景

因爲在一些點雲處理的程序中，出現多個訂閱者訂閱同一個topic，由於內部處理的時間不同，最後造成顯示界面出現卡頓，現象就是用鼠標拖動點雲的視角會感覺非常卡，不順暢。爲此，決定先走一遍官方的多線程系列教程。

https://github.com/ros/ros_tutorials/tree/jade-devel/roscpp_tutorials

http://wiki.ros.org/roscpp_tutorials/Tutorials

關於Spin和SpinOnce的解釋來自英文書《A Gentle Introduction to ROS》P59頁

什麼時候用ros::spin()和ros::spinOnce()呢，如果僅僅只是響應topic，就用ros::spin()。當程序中除了響應回調函數還有其他重複性工作的時候，那就在循環中做那些工作，然後調用ros::spinOnce()。如下面的用spinOnce，循環調用print( )函數。

這裏一直在打印while 中的部分，並且處理message queue。

而下面的打印輸出不會更新，相當於只執行了一次，但是它會不斷處理ROS 的message queue（下圖右邊的subscriber 開頭的打印函數只調用了一次，然後一直響應訂閱的消息）。

這裏只在一開始進入main的時候調用了一次print函數，其餘的都在響應topic 去了。

spinOnce( )較常用的做法是while裏放publisher所要發佈的msg的賦值處理，然後一直循環發佈topic。

ros::Rate loop_rate(10);

  while (ros::ok())
  {
    std_msgs::String msg;

    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();

    chatter_pub.publish(msg);

    ros::spinOnce();
    loop_rate.sleep();
  }

1. customer_callback_processing

自定義消息隊列處理線程，關鍵字：JOIN，在主線程spinOnce進入自定義處理線程。

http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning這裏官網給出了補充說明

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

#include <boost/thread.hpp>

/**
 * This 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())
  {
    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.
   * You can also set a default queue on a NodeHandle using the NodeHandle::setCallbackQueue() function.
   *
   * AdvertiseOptions and AdvertiseServiceOptions offer similar functionality.
   */
  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;
}

2. listenner_multiple_spin 

思想：利用一個類的不同成員函數實現同一個topic的不同處理(在一個線程中)。

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

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system, with
 * multiple callbacks for the same topic.  See the "listener" tutorial for more
 * information.
 */

class Listener
{
public:
  void chatter1(const std_msgs::String::ConstPtr& msg) { ROS_INFO("chatter1: [%s]", msg->data.c_str()); }
  void chatter2(const std_msgs::String::ConstPtr& msg) { ROS_INFO("chatter2: [%s]", msg->data.c_str()); }
  void chatter3(const std_msgs::String::ConstPtr& msg) { ROS_INFO("chatter3: [%s]", msg->data.c_str()); }
};

void chatter4(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("chatter4: [%s]", msg->data.c_str());
}

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

  Listener l;
  ros::Subscriber sub1 = n.subscribe("chatter", 10, &Listener::chatter1, &l);
  ros::Subscriber sub2 = n.subscribe("chatter", 10, &Listener::chatter2, &l);
  ros::Subscriber sub3 = n.subscribe("chatter", 10, &Listener::chatter3, &l);
  ros::Subscriber sub4 = n.subscribe("chatter", 10, chatter4);

  ros::spin();

  return 0;
}

3. listener_threaded_spin 

思想：利用一個類的不同成員函數實現同一個topic的不同處理(在四個不同線程中)。

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

#include <boost/thread.hpp>

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system, using
 * a threaded Spinner object to receive callbacks in multiple threads at the same time.
 */

ros::Duration d(0.01);

class Listener
{
public:
  void chatter1(const std_msgs::String::ConstPtr& msg)
  {
    ROS_INFO_STREAM("chatter1: [" << msg->data << "] [thread=" << boost::this_thread::get_id() << "]");
    d.sleep();
  }
  void chatter2(const std_msgs::String::ConstPtr& msg)
  {
    ROS_INFO_STREAM("chatter2: [" << msg->data << "] [thread=" << boost::this_thread::get_id() << "]");
    d.sleep();
  }
  void chatter3(const std_msgs::String::ConstPtr& msg)
  {
    ROS_INFO_STREAM("chatter3: [" << msg->data << "] [thread=" << boost::this_thread::get_id() << "]");
    d.sleep();
  }
};

void chatter4(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO_STREAM("chatter4: [" << msg->data << "] [thread=" << boost::this_thread::get_id() << "]");
  d.sleep();
}

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

  Listener l;
  ros::Subscriber sub1 = n.subscribe("chatter", 10, &Listener::chatter1, &l);
  ros::Subscriber sub2 = n.subscribe("chatter", 10, &Listener::chatter2, &l);
  ros::Subscriber sub3 = n.subscribe("chatter", 10, &Listener::chatter3, &l);
  ros::Subscriber sub4 = n.subscribe("chatter", 10, chatter4);

  /**
   * The MultiThreadedSpinner object allows you to specify a number of threads to use
   * to call callbacks.  If no explicit # is specified, it will use the # of hardware
   * threads available on your system.  Here we explicitly specify 4 threads.
   */
  ros::MultiThreadedSpinner s(4);
  ros::spin(s);

  return 0;
}

4. listener_async_spin 

因爲這裏用的是spinOnce，主線程每隔（1/5秒）打印，然後sleep把控制權交給系統，系統然後就把控制權分配到四個子線程，所以結果看到一共是五個線程。

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

#include <boost/thread.hpp>

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system, using
 * an asynchronous Spinner object to receive callbacks in multiple threads at the same time.
 */

ros::Duration d(0.01);

class Listener
{
public:
  void chatter1(const std_msgs::String::ConstPtr& msg)
  {
    ROS_INFO_STREAM("chatter1: [" << msg->data << "] [thread=" << boost::this_thread::get_id() << "]");
    d.sleep();
  }
  void chatter2(const std_msgs::String::ConstPtr& msg)
  {
    ROS_INFO_STREAM("chatter2: [" << msg->data << "] [thread=" << boost::this_thread::get_id() << "]");
    d.sleep();
  }
  void chatter3(const std_msgs::String::ConstPtr& msg)
  {
    ROS_INFO_STREAM("chatter3: [" << msg->data << "] [thread=" << boost::this_thread::get_id() << "]");
    d.sleep();
  }
};

void chatter4(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO_STREAM("chatter4: [" << msg->data << "] [thread=" << boost::this_thread::get_id() << "]");
  d.sleep();
}

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

  Listener l;
  ros::Subscriber sub1 = n.subscribe("chatter", 10, &Listener::chatter1, &l);
  ros::Subscriber sub2 = n.subscribe("chatter", 10, &Listener::chatter2, &l);
  ros::Subscriber sub3 = n.subscribe("chatter", 10, &Listener::chatter3, &l);
  ros::Subscriber sub4 = n.subscribe("chatter", 10, chatter4);

  /**
   * The AsyncSpinner object allows you to specify a number of threads to use
   * to call callbacks.  If no explicit # is specified, it will use the # of hardware
   * threads available on your system.  Here we explicitly specify 4 threads.
   */
  ros::AsyncSpinner s(4);
  s.start();

  ros::Rate r(5);
  while (ros::ok())
  {
    ROS_INFO_STREAM("Main thread [" << boost::this_thread::get_id() << "].");
    r.sleep();
  }

  return 0;
}

============2016.11====一次訂閱四個topic===============

http://wiki.ros.org/message_filters#ApproximateTime_Policy

https://github.com/tum-vision/rgbd_demo/blob/master/src/example.cpp

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>

// Note: for colorized point cloud, use PointXYZRGBA
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

using namespace std;
using namespace sensor_msgs;
using namespace message_filters;

ros::Publisher pointcloud_pub;

void callback(const ImageConstPtr& image_color_msg,
        const ImageConstPtr& image_depth_msg,
        const CameraInfoConstPtr& info_color_msg,
        const CameraInfoConstPtr& info_depth_msg)
{
    // Solve all of perception here...
    cv::Mat image_color = cv_bridge::toCvCopy(image_color_msg)->image;
    cv::Mat image_depth = cv_bridge::toCvCopy(image_depth_msg)->image;

    cvtColor(image_color,image_color, CV_RGB2BGR);

    // colorize the image
    cv::Vec3b color(255,0,0);
    for(int y=0;y<image_color.rows;y++)
       {
        for(int x=0;x<image_color.cols;x++)
          {
            float depth = image_depth.at<short int>(cv::Point(x,y)) / 1000.0;

            if( depth == 0)
                      {
                image_color.at<cv::Vec3b>(cv::Point(x,y)) = cv::Vec3b(0,0,255);
            }
            if( depth > 1.00)
                      {
                image_color.at<cv::Vec3b>(cv::Point(x,y)) = cv::Vec3b(255,0,0);
            }
        }
    }

    //cout <<"depth[320,240]="<< image_depth.at<short int>(cv::Point(320,240)) / 1000.0 <<"m"<< endl;
    image_color.at<cv::Vec3b>(cv::Point(320,240)) = cv::Vec3b(255,255,255);

    // get camera intrinsics
    float fx = info_depth_msg->K[0];
    float fy = info_depth_msg->K[4];
    float cx = info_depth_msg->K[2];
    float cy = info_depth_msg->K[5];
    //cout << "fx="<< fx << " fy="<<fy<<" cx="<<cx<<" cy="<<cy<<endl;

    // produce a point cloud
    PointCloud::Ptr pointcloud_msg (new PointCloud);
    pointcloud_msg->header = image_depth_msg->header;

    pcl::PointXYZ pt;
    for(int y=0;y<image_color.rows;y+=4)
        {
        for(int x=0;x<image_color.cols;x+=4)
              {
            float depth = image_depth.at<short int>(cv::Point(x,y)) / 1000.0;

            if(depth>0)
                  {
                pt.x = (x - cx) * depth / fx;
                pt.y = (y - cy) * depth / fy;
                pt.z = depth;
                //cout << pt.x<<" "<<pt.y<<" "<<pt.z<<endl;
                pointcloud_msg->points.push_back (pt);
            }
        }
    }
    pointcloud_msg->height = 1;
    pointcloud_msg->width = pointcloud_msg->points.size();
    pointcloud_pub.publish (pointcloud_msg);

    cv::imshow("color", image_color);

    cv::waitKey(3);
}

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

    ros::NodeHandle nh;

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

    ros::spin();

    return 0;
}

5. listener_with_userdata 

利用boost_bind向消息回調函數傳入多個參數

關於boost_bind以前寫過一個用法簡介http://blog.csdn.net/yaked/article/details/44942773

這裏是boost::bind(&listener::chatterCallback, this,  _1, "User 1" )也即this->chatterCallback( "User1"), _1表示第一個輸入參數, 是個佔位標記。

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

/**
 * This tutorial demonstrates a simple use of Boost.Bind to pass arbitrary data into a subscription
 * callback.  For more information on Boost.Bind see the documentation on the boost homepage,
 * http://www.boost.org/
 */


class Listener
{
public:
  ros::NodeHandle node_handle_;
  ros::V_Subscriber subs_;

  Listener(const ros::NodeHandle& node_handle): node_handle_(node_handle)
  {
  }

  void init()
  {
    subs_.push_back(node_handle_.subscribe<std_msgs::String>("chatter", 1000, boost::bind(&Listener::chatterCallback, this, _1, "User 1")));
    subs_.push_back(node_handle_.subscribe<std_msgs::String>("chatter", 1000, boost::bind(&Listener::chatterCallback, this, _1, "User 2")));
    subs_.push_back(node_handle_.subscribe<std_msgs::String>("chatter", 1000, boost::bind(&Listener::chatterCallback, this, _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());
  }
};

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

  Listener l(n);
  l.init();

  ros::spin();

  return 0;
}

6. timers

可以參考官網http://wiki.ros.org/roscpp/Overview/Timers

#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);
  ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2);

  ros::spin();

  return 0;
}

======================2016.07=====定時器帶入附加參數===============

#include "ros/ros.h"
#include "cstring"
#include "iostream"
#include "han_agv/SocketClient.h"

#include "han_agv/VelEncoder.h"

using namespace std;

const float_t PI = 3.14159;
const int ENCODER_PER_LOOP = 900;
const float_t WHEEL_RADIUS = 0.075;

ClientSocket soc;

// ros_tutorials/turtlesim/tutorials/draw_square.cpp
void timerCallback(const ros::TimerEvent&, ros::Publisher vel_pub)
{
  ROS_INFO("Timer callback triggered");
  AGVDATA buffer;
  AGVSENSORS buffer_left_encoder_start;
  AGVSENSORS buffer_right_encoder_start;

//  if(soc.recvMsg(buffer) == 0)
//  {
//      cout << "Failed to receive message." << endl;
//      return;
//  }

  // Reverse the High and Low byte.
  double time_delay = 0.1;
  ros::Duration(time_delay).sleep ();

  buffer.SensorsData.WheelLeft_Encoder[0] = buffer.SensorsData.WheelLeft_Encoder[3];
  buffer.SensorsData.WheelLeft_Encoder[1] = buffer.SensorsData.WheelLeft_Encoder[2];

  buffer.SensorsData.WheelRight_Encoder[0] = buffer.SensorsData.WheelRight_Encoder[3];
  buffer.SensorsData.WheelRight_Encoder[1] = buffer.SensorsData.WheelRight_Encoder[2];

   han_agv::VelEncoder vel;
   vel.left_velocity = 1.0;
   vel.right_velocity = 2.0;

   vel_pub.publish(vel);
}

int main(int argc, char** argv)
{
        ros::init(argc, argv, "TCPDecode_node");
        ros::NodeHandle nh;
        ROS_INFO("create node successfully!");

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

         ros::spin();
}

7. 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
   */
  ros::Publisher pub = n.advertise<std_msgs::String>("chatter", 1000, chatterConnect, chatterDisconnect);

  ros::spin();

  return 0;
}

rosrun beginner_tutorials listener_with_userdata 和另外其他任何一個，因爲其他的node名字都重複了，都叫listener。

斷開連接，通知延後了一分多鐘。囧囧！！