使用ROS读取话题图片和深度图并且生存点云数据输出在日rviz下显示(1)

1、创建功能包

#1、在src目录下创建功能包
catkin_make_pkg picture2pcl sensor_msgs cv_bridge roscpp std_msgs image_transport pcl_conversions pcl_ros
cd .. 
catkin_make
#2、配置cmakeLists.txt pcl ioencv 需要添加的地方
find_package(PCL REQUIRED)
find_package(OpenCV REQUIRED)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
        ${PCL_INCLUDE_DIRS}
        ${OpenCV_INCLUDE_DIRS}
)
target_link_libraries(opencv2pcl
   ${catkin_LIBRARIES}
         ${PCL_LIBRARIES}
         ${OpenCV_LIBRARIES}
 )
#3、需要加载的库
#include "ros/ros.h"
#include <image_transport/image_transport.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

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

#include <pcl/point_cloud.h>

#include<pcl/point_types.h>
#include<pcl_conversions/pcl_conversions.h>
#include<pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>

##1.2 、读取同时读取深度图和对应的图

using namespace std;
using namespace sensor_msgs;
using namespace message_filters;
using namespace cv;
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> testcloud;
class picture2Point
{
public:
    ros::NodeHandle nh;
    ros::Publisher pcl_pub;

    picture2Point(): nh("~"){
        message_filters::Subscriber<sensor_msgs::Image>rgb_sub(nh,"/camera/rgb/image_raw",1);
        message_filters::Subscriber<sensor_msgs::Image>depth_sub(nh,"/camera/depth/image_raw",1);
// 需要用message_filters 对
        typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
        message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), rgb_sub, depth_sub);
        sync.registerCallback(boost::bind(&picture2Point::callback, this, _1, _2));

        ros::spin();
    }
    ~picture2Point(){};

    sensor_msgs::PointCloud2 output;

    void callback(const sensor_msgs::ImageConstPtr &RGB_image,const sensor_msgs::ImageConstPtr &depth_image)
    {
        try {
            cv_bridge::CvImageConstPtr image_ptr=cv_bridge::toCvShare(RGB_image);
            cv::Mat RGB_image=image_ptr->image;

            cv_bridge::CvImageConstPtr image_Ptr1=cv_bridge::toCvShare(depth_image);
            cv::Mat Depth_image=image_Ptr1->image;

        }
        catch (cv_bridge::Exception &e)
        {
            ROS_ERROR("cv_bridge Exception %s",e.what());
        }
    }
};



int main(int argc,char *argv[])
{
    ros::init(argc,argv,"image2point");
    picture2Point node;


    return 0;
}

#1、3