首先定义一个类函数,在类中定义用于接收回调函数的两个变量,用于在data_fusion函数中显示和处理.
两种不同类型消息: sensor_msgs::ImageConstPtr 图像消息; std_msgs::UInt16MultiArray::ConstPtr 向量消息
#include<ros/ros.h> //ros标准库头文件
#include<iostream> //C++标准输入输出库
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>
#include<image_transport/image_transport.h>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include"std_msgs/UInt16MultiArray.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
using namespace std;
using namespace cv;
class SubscribeAndPublish
{
private:
cv::Mat image;
std::vector<unsigned short> depth;
ros::NodeHandle nh;
ros::Subscriber sub_rgb;
ros::Subscriber sub_depth;
public:
SubscribeAndPublish()
{
sub_rgb = nh.subscribe("/stereo_camera/RGB", 10, &SubscribeAndPublish::callback_rgb, this);
sub_depth = nh.subscribe("/stereo_camera/disparity16", 10000, &SubscribeAndPublish::callback_depth, this);
}
~SubscribeAndPublish()
{
}
//相机的回调函数,这里的car_sensor是我自己的package,image_detect是自己定义的msg消息
void callback_depth(const std_msgs::UInt16MultiArray::ConstPtr& msg)
{
depth = msg->data;
}
void callback_rgb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr; // 声明一个CvImage指针的实例
try //对错误异常进行捕获,检查数据的有效性
{
//获得Mat类型图像数据
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); //将ROS消息中的图象信息提取,生成新cv类型的图象,复制给CvImage指针
image=cv_ptr->image;
// cv::imshow("INPUT", cv_ptr->image);
// cv::waitKey(25);
}
catch(cv_bridge::Exception& e) //异常处理
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
data_fusion();
}
//我们在这里处理RGB图像和深度数据的融合
void data_fusion()
{
if(image.empty())
{
std::cout<<"There is no image data in data_fusion!"<<std::endl;
}
else if(depth.size()==0)
{
std::cout<<"there is no depth image data in data_fusion!"<<std::endl;
}
//when we get both the image data and the depth data.we begin to do data-fusion
else
{
int rows=720;
int cols=1280;
Mat img(Size(rows,cols), CV_16UC1);
for (int i = 0; i <rows; ++i)
{
for (int j = 0; j < cols; ++j)
{
img.at<unsigned short>(j, i) = depth[i * cols + j] ;
}
}
std::cout << "size():" << img[559930]<< std::endl;
cv::imshow("rgb_camera",image);
cv::waitKey(10);
}
}
};//End of class SubscribeAndPublish
int main(int argc, char **argv)
{
//Initiate ROS
ros::init(argc, argv, "stereo_camera_listener");
//Create an object of class SubscribeAndPublish that will take care of everything
SubscribeAndPublish image2depth;
ros::spin();
return 0;
}