WIKI de Bertrand VANDEPORTAELE

// from ueyecam_nodelet
#include <ros/package.h>
#include <camera_calibration_parsers/parse.h>
#include <sensor_msgs/fill_image.h>
#include <sensor_msgs/image_encodings.h>
 
#include <sstream>
#include "std_msgs/String.h"
 
// code sample image transport
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
 
// For time synchronizer
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
 
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <cstring>
 
using namespace sensor_msgs;
using namespace message_filters;
 
unsigned int k(0);
 
/*****************************************************/
/***                  SAVE_PPM                     ***/
/*****************************************************/
void save_ppm(char *filename, char *im, unsigned int width, unsigned int height)
{
    FILE *f;
    if(!(f=fopen(filename,"wb"))) {
        fprintf(stderr,"Could not open `%s'",filename);
        exit(1);
    }
 
    fprintf(f,"P6\n%d %d\n255\n",width,height);
    fwrite(im,1,width*height*3,f);
    fclose(f);
}
 
/*****************************************************/
/***              IMAGE CALLBACK                   ***/
/*****************************************************/
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
        cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
        cv::waitKey(30);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}
 
/*****************************************************/
/***              STEREO CALLBACK                  ***/
/*****************************************************/
void stereoCallback(const ImageConstPtr& image1, const ImageConstPtr& image2)
{
    // Solve all of perception here...
    try
    {
        cv_bridge::CvImagePtr cv_ptr_1 = cv_bridge::toCvCopy(image1, "bgr8");
        cv_bridge::CvImagePtr cv_ptr_2 = cv_bridge::toCvCopy(image2, "bgr8");
 
        // save left image
        std::stringstream sstm;
        std::string nameFileL;
        sstm << "/tmp/images/ueye_" << std::setfill('0') << std::setw(5) << k << "_left" << ".ppm" ;
        nameFileL = sstm.str();
        save_ppm((char*)nameFileL.c_str(), (char*)cv_ptr_1->image.data, cv_ptr_1->image.cols, cv_ptr_1->image.rows);
 
        // save right image
        std::string nameFileR;
        sstm.str(std::string());
        sstm << "/tmp/images/ueye_" << std::setfill('0') << std::setw(5) << k << "_right" << ".ppm" ;
        nameFileR = sstm.str();
        save_ppm((char*)nameFileR.c_str(), (char*)cv_ptr_2->image.data, cv_ptr_2->image.cols, cv_ptr_2->image.rows);
 
        // show image
        cv::imshow("view_left", cv_ptr_1->image);
        cv::imshow("view_right", cv_ptr_2->image);
        cv::waitKey(30);
        k++;
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", image1->encoding.c_str());
    }
}
 
/*****************************************************/
/***                    MAIN                       ***/
/*****************************************************/
int main(int argc, char **argv)
{
    // create folder where we save images
    system("mkdir /tmp/images");
 
    ros::init(argc, argv, "listenerUeye");
    ros::NodeHandle nh;
    bool stereo(true);
 
    if(!stereo){
        cv::namedWindow("view");
        cv::startWindowThread();
 
        image_transport::ImageTransport it(nh);
        image_transport::Subscriber sub = it.subscribe("/stereo/left/image_raw", 1, imageCallback);
 
        ros::spin();
 
        cv::destroyWindow("view");
    }
    else{
        cv::namedWindow("view_left");
        cv::namedWindow("view_right");
        cv::startWindowThread();
 
        message_filters::Subscriber<Image> image_left_sub(nh, "/stereo/left/image_raw", 1);
        message_filters::Subscriber<Image> image_right_sub(nh, "/stereo/right/image_raw", 1);
        TimeSynchronizer<Image, Image> sync(image_left_sub, image_right_sub, 10);
        sync.registerCallback(boost::bind(&stereoCallback, _1, _2));
 
        ros::spin();
 
        cv::destroyWindow("view_left");
        cv::destroyWindow("view_right");
    }
}