00001
00002
00003
00004
00005
00006
00007
00008
00009
00017 #include <ros/ros.h>
00018
00019 #include <velodyne/data.h>
00020
00021 #define NODE "velodyne_print"
00022
00023 using namespace velodyne_common;
00024
00025
00026 static int numPackets = 0;
00027 static std::string ofile = "";
00028 static int qDepth = 1;
00029
00030
00031 static velodyne::Data *data = NULL;
00032
00034 void printPoints(size_t npackets)
00035 {
00036 static int packetCount = 0;
00037
00038 data->print();
00039
00040
00041 packetCount += npackets;
00042 if ((numPackets > 0) && (packetCount >= numPackets))
00043 {
00044 ROS_INFO("no more packets needed");
00045 data->shutdown();
00046 }
00047 }
00048
00050 void processScans(const std::vector<velodyne::laserscan_t> &scan)
00051 {
00052 printPoints(scan.size() / velodyne::SCANS_PER_PACKET);
00053 }
00054
00056 void processXYZ(const std::vector<velodyne::laserscan_xyz_t> &scan)
00057 {
00058 printPoints(scan.size() / velodyne::SCANS_PER_PACKET);
00059 }
00060
00061 void displayHelp()
00062 {
00063 std::cerr << "write Velodyne data in other formats\n"
00064 << std::endl
00065 << "Usage: rosrun velodyne_file print <options>\n"
00066 << std::endl
00067 << "Options:\n"
00068 << "\t -h, -? print usage message\n"
00069 << "\t -t <type> define output type \n"
00070
00071 << "\t\t -t2 laser, direction, x, y, z, intensity (default) \n"
00072 << "\t\t -t3 theta,phi,distance,intensity\n"
00073
00074 << "\t -n <integer> read this many packets (default: all)\n"
00075 << "\t -q <integer> set ROS topic queue depth (default: 1)\n"
00076 << "\t -f <filename> write output to this file (default: none)\n"
00077 << "\t -f- write data to stdout\n"
00078 << std::endl
00079 << "Example:\n"
00080 << " rosrun velodyne_file print -t2 -n260 -f onerev.vxyz\n"
00081 << std::endl;
00082 }
00083
00084
00089 int getParameters(int argc, char *argv[])
00090 {
00092 static int type = 2;
00093
00094
00095 char ch;
00096 const char* optflags = "f:n:q:t:h?";
00097 while(-1 != (ch = getopt(argc, argv, optflags)))
00098 {
00099 switch(ch)
00100 {
00101 case 'f':
00102 ofile = std::string(optarg);
00103 break;
00104 case 'n':
00105 numPackets = atoi(optarg);
00106 break;
00107 case 'q':
00108 qDepth = atoi(optarg);
00109 if (qDepth < 1)
00110 qDepth = 1;
00111 break;
00112 case 't':
00113 type = atoi(optarg);
00114 break;
00115 default:
00116 ROS_WARN("unknown parameter: %c", ch);
00117
00118 case 'h':
00119 case '?':
00120 displayHelp();
00121 return 1;
00122 }
00123 }
00124
00125 ROS_INFO("options: -n%d -t%d -f %s -q%d", numPackets, type,
00126 ofile.c_str(), qDepth);
00127
00128 switch (type)
00129 {
00130 case 2:
00131 {
00132 velodyne::DataXYZ *dxyz = new velodyne::DataXYZ(ofile);
00133 dxyz->getParams();
00134 dxyz->subscribeXYZ(processXYZ);
00135 data = dxyz;
00136 break;
00137 }
00138 case 3:
00139 {
00140 velodyne::DataScans *dscans = new velodyne::DataScans(ofile);
00141 dscans->getParams();
00142 dscans->subscribeScans(processScans);
00143 data = dscans;
00144 break;
00145 }
00146 default:
00147 {
00148 ROS_FATAL("invalid -t option value: %d", type);
00149 displayHelp();
00150 return 1;
00151 }
00152 }
00153
00154 return 0;
00155 }
00156
00157 int main(int argc, char *argv[])
00158 {
00159 ros::init(argc, argv, NODE);
00160 ros::NodeHandle node;
00161
00162 if (0 != getParameters(argc, argv))
00163 return 9;
00164
00165 if (0 != data->setup())
00166 return 2;
00167
00168
00169
00170
00171 ros::Subscriber velodyne_scan =
00172 node.subscribe("velodyne/rawscan", qDepth,
00173 &velodyne::Data::processRawScan, data,
00174 ros::TransportHints().tcpNoDelay(true));
00175
00176 ros::spin();
00177
00178 data->shutdown();
00179 delete data;
00180
00181 return 0;
00182 }