Revision history [back]

I'll be glad to help you figure this out! First thing though... Don't mix namespaces... You have ROS, CV and std there... So, it is recommended that you not do using namespace std; and instead do std::cout as opposed to cout.

Next, it looks like you are not fixing the rate. Does this help at all? I doubt it's this simple, but it's good to give it a go.

#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <string.h>
#include <ros/callback_queue.h>


// using namespace std; <- Don't mix namespaces!

char filename[50];
int imgtype;

bool done=false;

static const int MY_ROS_QUEUE_SIZE = 100;

void depth_node_callback(const sensor_msgs::Image::ConstPtr& msg)
{
    std::cout <<"Entering Callback"<< std::endl;

    cv_bridge::CvImageConstPtr cv_ptr;
    cv_ptr = cv_bridge::toCvShare(msg);

    double max = 0, min=0;

    cv::minMaxLoc(cv_ptr->image, &min, &max, 0, 0);
    cv::Mat normalized,op;
    cv_ptr->image.convertTo(normalized, CV_32F, 255/(max-min), -min) ;

    cv::imwrite(filename,normalized);

    done=true;
}

int main(int argc, char **argv)
{
    std::string type;
    std::strcpy(filename,argv[1]);

    imgtype=atoi(argv[2]);

    ros::init(argc, argv, "depth_node");

    ros::Rate loop_rate(100);

    cout << "Ready to save image" << endl;
    ros::NodeHandle nh;

    if(imgtype==1)
        type="camera/rgb/image_color";

    else if(imgtype==0)
        type="camera/depth_registered/image_raw";

    ros::Subscriber sub = nh.subscribe(type,MY_ROS_QUEUE_SIZE, depth_node_callback);


    while(!done && ros::ok()) {
        ros::spinOnce();
        loop_rate.sleep();
    }

    std::cout<< filename <<" Saved"<< std::endl;

    return 0;
}