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