多线程订阅发布SBG、图像与点云话题

多线程订阅SBG、图像与点云话题并以当前时间戳发布新SBG、图像与点云话题

//*****************************************************************************************************
//
//     以当前时间发布新SBG、图像与点云话题:points_raw  gps_raw  quat_raw  euler_raw  image_raw
//     source /home/nefu/catkin_gap/devel/setup.bash && rosrun point_imu new_SBG_image_lidar
//
//**************************************************************************************************

#include <stdio.h>
#include "std_msgs/String.h"
#include <ros/time.h>
#include <ros/ros.h>
#include <boost/thread.hpp>


#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/impl/io.hpp>

#include <sbg_driver/SbgImuData.h>
#include <sbg_driver/SbgEkfQuat.h>
#include <sbg_driver/SbgEkfEuler.h>
#include <sbg_driver/SbgGpsPos.h>
#include <sbg_driver/SbgEkfNav.h>

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



using namespace std;
using namespace pcl;



class ThreadListener

{

public:

  ThreadListener() //  ros::TransportHints().tcpNoDelay()
   {
      cout<< "Publisher: points_raw  gps_raw  quat_raw  euler_raw  image_raw"<<endl;

      sub1 = n.subscribe<sensor_msgs::PointCloud2>("velodyne_points", 1, &ThreadListener::myCallback1,this,ros::TransportHints().tcpNoDelay());
      sub2 = n.subscribe<sbg_driver::SbgEkfNav>("/sbg/ekf_nav", 1, &ThreadListener::myCallback2,this,ros::TransportHints().tcpNoDelay());
      sub3 = n.subscribe<sbg_driver::SbgEkfQuat>("/sbg/ekf_quat", 1, &ThreadListener::myCallback3,this,ros::TransportHints().tcpNoDelay());
      sub4 = n.subscribe<sbg_driver::SbgEkfEuler>("/sbg/ekf_euler", 1, &ThreadListener::myCallback4,this,ros::TransportHints().tcpNoDelay());
      sub5 = n.subscribe<sensor_msgs::Image>("/usb_cam/image_raw", 1, &ThreadListener::myCallback5,this,ros::TransportHints().tcpNoDelay());

      cloud_pub = n.advertise<sensor_msgs::PointCloud2>("points_raw", 1);//发布点云
      gps_pub   = n.advertise<sbg_driver::SbgEkfNav>("gps_raw", 1);//发布gps
      quat_pub  = n.advertise<sbg_driver::SbgEkfQuat>("quat_raw", 1);
      euler_pub = n.advertise<sbg_driver::SbgEkfEuler>("euler_raw", 1);
      image_pub = n.advertise<sensor_msgs::Image>("image_raw", 1);
    }

  void myCallback1(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg);
  void myCallback2(const  boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg);
  void myCallback3(const  boost::shared_ptr<const sbg_driver::SbgEkfQuat>& quatmsg);
  void myCallback4(const  boost::shared_ptr<const sbg_driver::SbgEkfEuler>& eulermsg);
  void myCallback5(const  boost::shared_ptr<const sensor_msgs::Image>& imagemsg);


private:
   ros::NodeHandle n;

   ros::Subscriber sub1;
   ros::Subscriber sub2;
   ros::Subscriber sub3;
   ros::Subscriber sub4;
   ros::Subscriber sub5;

   ros::Publisher cloud_pub;
   ros::Publisher gps_pub;
   ros::Publisher quat_pub;
   ros::Publisher euler_pub;
   ros::Publisher image_pub;


};

void ThreadListener::myCallback1(const  boost::shared_ptr<const sensor_msgs::PointCloud2 >& cloudmsg)
{
    sensor_msgs::PointCloud2 output;//发布点云话题消息
    output.header.stamp = ros::Time::now ();
    output.header.frame_id = "map";
    output.height=cloudmsg->height;
    output.width=cloudmsg->width;
    output.fields=cloudmsg->fields;
    output.is_bigendian=cloudmsg->is_bigendian;
    output.point_step=cloudmsg->point_step;
    output.data=cloudmsg->data;
    output.is_dense =cloudmsg->is_dense ;
    cloud_pub.publish(output);
    //cout << "lidar:"<<output.header.stamp<<endl;


}


void ThreadListener::myCallback2(const  boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg)
{
    sbg_driver::SbgEkfNav outgps;
    outgps.header.stamp = ros::Time::now ();
    outgps.header.frame_id = "gps";
    outgps.position=gpsmsg->position;
    gps_pub.publish(outgps);
    //cout << "GPS:"<<outgps.header.stamp<<endl;



}

void ThreadListener::myCallback3(const  boost::shared_ptr<const sbg_driver::SbgEkfQuat>& quatmsg)
{
    sbg_driver::SbgEkfQuat outquat;
    outquat.header.stamp = ros::Time::now ();
    outquat.header.frame_id = "quat";
    outquat.quaternion=quatmsg->quaternion;
    quat_pub.publish(outquat);
    //cout << "quat:"<<outquat.header.stamp<<endl;

}

void ThreadListener::myCallback4(const  boost::shared_ptr<const sbg_driver::SbgEkfEuler>& eulermsg)
{
    sbg_driver::SbgEkfEuler outeuler;
    outeuler.header.stamp = ros::Time::now ();
    outeuler.header.frame_id = "euler";
    outeuler.angle=eulermsg->angle;
    euler_pub.publish(outeuler);
    //cout << "euler:"<<outeuler.header.stamp<<endl;

}

void ThreadListener::myCallback5(const  boost::shared_ptr<const sensor_msgs::Image>& imagemsg)
{
    sensor_msgs::Image outimage;
    outimage.header.stamp = ros::Time::now ();
    outimage.header.frame_id = "image";
    outimage.height=imagemsg->height;
    outimage.width=imagemsg->width;
    outimage.encoding=imagemsg->encoding;
    outimage.is_bigendian=imagemsg->is_bigendian;
    outimage.step=imagemsg->step;
    outimage.data=imagemsg->data;
    image_pub.publish(outimage);
    //cout << "image:"<<outimage.header.stamp<<endl;

}



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

  ThreadListener listener;
  ros::AsyncSpinner spinner(10); 
  spinner.start();
  ros::waitForShutdown();

}