00001 #include <opencv/cv.h>
00002 #include <opencv/highgui.h>
00003
00004 #include <ros/ros.h>
00005 #include <cv_bridge/CvBridge.h>
00006 #include <sensor_msgs/Image.h>
00007 #include <message_transport/message_transport.h>
00008
00009 std::string transport;
00010 unsigned int npoints = 0;
00011 FILE * fp=NULL;
00012
00013 void callback(const sensor_msgs::ImageConstPtr& image)
00014 {
00015 double tnow = ros::Time::now().toSec();
00016 double tstamp = image->header.stamp.toSec();
00017 if (!fp) {
00018 char fname[512];
00019 sprintf(fname,"received_%d_%s_%dx%dx%s.txt",
00020 getpid(),transport.c_str(),image->width,image->height,
00021 image->encoding.c_str());
00022 ROS_INFO("Saving data in %s",fname);
00023 fp = fopen(fname,"w");
00024 }
00025 fprintf(fp,"%d %f %f %f\n",npoints,tnow,tstamp,tnow-tstamp);
00026 npoints ++;
00027
00028 ROS_INFO("%d: Image received at %f, delay %f",
00029 getpid(),tstamp,tnow-tstamp);
00030
00031 if (npoints > 1000) {
00032 fclose(fp);
00033 ros::shutdown();
00034 }
00035 }
00036
00037 int main(int argc, char** argv)
00038 {
00039 ros::init(argc, argv, "test_receiver", ros::init_options::AnonymousName);
00040 ros::NodeHandle nh;
00041 message_transport::MessageTransport<sensor_msgs::Image>
00042 it(nh,"imagem_transport","sensor_msgs::Image");
00043
00044 transport = std::string((argc > 1) ? argv[1] : "raw");
00045 message_transport::Subscriber sub = it.subscribe("image_source", 1, callback,
00046 transport);
00047 ROS_INFO("test_receiver started");
00048
00049 ros::spin();
00050 }
00051
