00001 #include <ros/ros.h>
00002 #include <roscpp/SetLoggerLevel.h>
00003 #include <message_transport/message_transport.h>
00004 #include <sensor_msgs/PointCloud.h>
00005 #include "numpoints.h"
00006
00007 std::string transport;
00008 unsigned int npoints = 0;
00009
00010
00011
00012 #ifdef RECORD
00013 FILE * fp=NULL;
00014 #endif
00015
00016 void callback(const sensor_msgs::PointCloudConstPtr& pointcloud)
00017 {
00018 unsigned int i;
00019 double tnow = ros::Time::now().toSec();
00020 double tstamp = pointcloud->header.stamp.toSec();
00021 #ifdef RECORD
00022 if (!fp) {
00023 char fname[512];
00024 sprintf(fname,"received_%d_%s_%d.txt",
00025 getpid(),transport.c_str(),pointcloud->points.size());
00026 ROS_INFO("Saving data in %s",fname);
00027 fp = fopen(fname,"w");
00028 }
00029 fprintf(fp,"%d %f %f %f\n",npoints,tnow,tstamp,tnow-tstamp);
00030 #endif
00031 npoints ++;
00032
00033 assert(pointcloud->points.size() >= numpoints);
00034 assert(pointcloud->channels.size() == 1);
00035 assert(pointcloud->channels[0].values.size() >= numpoints);
00036 for (i=0;i<pointcloud->points.size();i++) {
00037 assert(round(pointcloud->points[i].x - i) < 1e-3);
00038 assert(round(pointcloud->points[i].y - i/10) < 1e-3);
00039 assert(round(pointcloud->points[i].z - i/100) < 1e-3);
00040 assert(round(pointcloud->channels[0].values[i] - i) < 1e-3);
00041 }
00042
00043 ROS_INFO("%d: Scan received at %f, delay %f",
00044 getpid(),tstamp,tnow-tstamp);
00045
00046 #ifdef RECORD
00047 if (npoints > 1000) {
00048 fclose(fp);
00049 ros::shutdown();
00050 }
00051 #endif
00052 }
00053
00054 void setDebugLevel(const std::string & logname) {
00055 log4cxx::LoggerPtr logger = log4cxx::Logger::getLogger(logname);
00056 logger->setLevel(log4cxx::Level::getDebug());
00057 ros::console::notifyLoggerLevelsChanged();
00058 }
00059
00060
00061 int main(int argc, char** argv)
00062 {
00063 ros::init(argc, argv, "test_receiver", ros::init_options::AnonymousName);
00064 ros::NodeHandle nh;
00065
00066 message_transport::MessageTransport<sensor_msgs::PointCloud>
00067 it(nh,"pointcloud_transport","sensor_msgs::PointCloud");
00068 std::string pkgname("pointcloud_transport");
00069 transport = std::string((argc > 1) ? argv[1] : "pointcloud_transport/raw");
00070 if (transport.compare(0,pkgname.length(),pkgname)) {
00071 transport = pkgname + "/" + transport;
00072 }
00073 message_transport::Subscriber sub = it.subscribe("pc_source", 1, callback,
00074 transport);
00075 ROS_INFO("test_receiver started");
00076
00077
00078
00079
00080 ros::spin();
00081 }
00082
