#include <ros/ros.h>
#include<image_transport/image_transport.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/highgui/highgui.hpp>
#include <iostream>
#include <opencv2/core/core.hpp>
 

#include </home/l/software/opencv-2.4.11/modules/stitching/include/opencv2/stitching/stitcher.hpp>   //這個只有opencv2才有，所以安裝一個，然後用絕對路徑

using namespace std;
using namespace cv;
bool try_use_gpu = false;
//static const std::string OPENCV_WINDOW = "Image window";
Mat img1;
Mat img2;
vector<Mat> imgs;
string
 
 result_name = "dst1.jpg";
class ImageConverter
{
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
image_transport::Subscriber image_sub1_;
image_transport::Subscriber image_sub2_;
image_transport::Publisher image_pub_;

public:
ImageConverter()
: it_(nh_)
{
// Subscrive to input video feed and publish output video feed
 

image_sub1_ = it_.subscribe("/1/usb_cam/image_raw", 1, &ImageConverter::imageCb1, this);
image_sub2_ = it_.subscribe("/2/usb_cam/image_raw", 1, &ImageConverter::imageCb2, this);
image_pub_ = it_.advertise("/image_converter/output_video", 1);

//namedWindow(OPENCV_WINDOW);
}

~ImageConverter()
{
//destroyWindow(OPENCV_WINDOW);
}

///////1/
void imageCb1(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr1;
    try
    {
    cv_ptr1 = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
    }
//imshow("1111",cv_ptr1->image);
img1= cv_ptr1->image;
waitKey(3);
}
//2////////////////



///////2
//***
void imageCb2(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr2;
    try
    {
    cv_ptr2 = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
    }
//Mat img2;
//imshow("2222",img1);
img2= cv_ptr2->image;
waitKey(3);

//***/
imshow("1111",img1);
imshow("2222",img2);
////////////////////
//**

///**/
}
};

int main(int argc, char** argv)
{
ros::init(argc, argv, "video_stitching");
ImageConverter ic;
ros::Rate loop_rate(10);  //5/100
///////video_stitching

//imshow("1111",cv_ptr1->image);
/**
if (!img2.empty()) 
{
imshow("3333", img2);
} 
else
{
cout << "image3 is empty" << endl;
}
**/
ros::spin();

return 0;
}