00001
00002
00003
00004
00005
00006
00007
00008
00009
00015 #include <string>
00016
00017 #include <ros/ros.h>
00018
00019 #include <velodyne/input.h>
00020 #include <velodyne_common/RawScan.h>
00021 #include <velodyne_msgs/VelodyneScan.h>
00022
00023 #define NODE "velodyne_read"
00024
00025
00026 static velodyne::Input *input;
00027
00028
00029 static int qDepth = 1;
00030
00031 void displayHelp()
00032 {
00033 std::cerr << "read and publish Velodyne raw packets"
00034 << std::endl << std::endl
00035 << "Usage: rosrun velodyne_read read <options>"
00036 << std::endl << std::endl
00037 << "Options:" << std::endl
00038 << "\t -h, -? print usage message\n"
00039 << "\t -q <integer> set ROS topic queue depth (default: 1)\n"
00040 << "\t -f <input-file> set PCAP dump input file"
00041 << std::endl;
00042 }
00043
00048 int getParameters(int argc, char *argv[])
00049 {
00050
00051
00052 char ch;
00053 std::string dump_file = "";
00054 const char* optflags = "f:hq:?";
00055 while(-1 != (ch = getopt(argc, argv, optflags)))
00056 {
00057 switch(ch)
00058 {
00059 case 'f':
00060 dump_file = std::string(optarg);
00061 break;
00062 case 'q':
00063 qDepth = atoi(optarg);
00064 if (qDepth < 1)
00065 qDepth = 1;
00066 break;
00067 default:
00068 ROS_WARN("unknown parameter: %c", ch);
00069
00070 case 'h':
00071 case '?':
00072 displayHelp();
00073 return 1;
00074 }
00075
00076 ROS_INFO("velodyne/rawscan queue depth = %d", qDepth);
00077 }
00078
00079
00080 ros::NodeHandle private_nh("~");
00081 private_nh.getParam("pcap", dump_file);
00082
00083 using namespace velodyne;
00084 if (dump_file != "")
00085 {
00086 ROS_INFO("reading data from file: %s", dump_file.c_str());
00087 input = new InputPCAP(dump_file);
00088 }
00089 else
00090 {
00091 ROS_INFO("reading data from socket");
00092 input = new InputSocket();
00093 }
00094
00095 return 0;
00096 }
00097
00098
00099 int main(int argc, char** argv)
00100 {
00101 ros::init(argc, argv, NODE);
00102 ros::NodeHandle node;
00103
00104 if (getParameters(argc, argv) != 0)
00105 return 1;
00106
00107 using namespace velodyne_common;
00108
00109
00110 int rc = input->vopen();
00111 if(rc)
00112 {
00113 ROS_FATAL("Cannot open Velodyne input.");
00114 return 2;
00115 }
00116
00117 ros::Publisher output =
00118 node.advertise<velodyne_msgs::VelodyneScan>("velodyne/packets", qDepth);
00119 ros::Publisher raw_output =
00120 node.advertise<velodyne_common::RawScan>("velodyne/rawscan", qDepth);
00121
00122 int npackets = RawScan::PACKETS_PER_REVOLUTION;
00123
00124
00125 while(ros::ok())
00126 {
00127
00128 velodyne_msgs::VelodyneScanPtr scan(new velodyne_msgs::VelodyneScan);
00129 scan->packets.resize(npackets);
00130
00131
00132
00133 for (int i = 0; i < npackets; ++i)
00134 {
00135 while (true)
00136 {
00137
00138 int rc = input->getPacket(&scan->packets[i]);
00139 if (rc == 0)
00140 break;
00141 if (rc < 0)
00142 goto terminate;
00143 }
00144 }
00145
00146
00147 ROS_DEBUG("Publishing a full Velodyne scan.");
00148 scan->header.stamp = ros::Time(scan->packets[npackets - 1].stamp);
00149 scan->header.frame_id = "/velodyne";
00150
00151 if (output.getNumSubscribers() > 0)
00152 {
00153 output.publish(scan);
00154 }
00155
00156 if (raw_output.getNumSubscribers() > 0)
00157 {
00158
00159 velodyne_common::RawScanPtr raw(new velodyne_common::RawScan);
00160 size_t psize = velodyne_common::RawScan::PACKET_SIZE;
00161 raw->data.resize(npackets * psize);
00162 raw->header = scan->header;
00163 for (int i = 0; i < npackets; ++i)
00164 {
00165 memcpy(&raw->data[i*psize], &scan->packets[i].data[0], psize);
00166 }
00167 raw_output.publish(raw);
00168 }
00169 }
00170
00171 terminate:
00172 input->vclose();
00173 delete input;
00174 return 0;
00175 }