00001 /* -*- mode: C++ -*- 00002 * 00003 * Copyright (C) 2007 Austin Robot Technology, Yaxin Liu, Patrick Beeson 00004 * Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin 00005 * 00006 * License: Modified BSD Software License Agreement 00007 * 00008 * $Id$ 00009 */ 00010 00031 #ifndef __VELODYNE_INPUT_H 00032 #define __VELODYNE_INPUT_H 00033 00034 #include <unistd.h> 00035 #include <stdio.h> 00036 #include <pcap.h> 00037 00038 #include <ros/ros.h> 00039 #include <velodyne_msgs/VelodynePacket.h> 00040 00041 namespace velodyne_driver 00042 { 00043 static uint16_t UDP_PORT_NUMBER = 2368; 00044 00046 class Input 00047 { 00048 public: 00049 Input() {} 00050 00059 virtual int getPacket(velodyne_msgs::VelodynePacket *pkt) = 0; 00060 }; 00061 00063 class InputSocket: public Input 00064 { 00065 public: 00066 InputSocket(ros::NodeHandle private_nh, 00067 uint16_t udp_port = UDP_PORT_NUMBER); 00068 ~InputSocket(); 00069 00070 virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); 00071 00072 private: 00073 00074 int sockfd_; 00075 }; 00076 00077 00083 class InputPCAP: public Input 00084 { 00085 public: 00086 InputPCAP(ros::NodeHandle private_nh, 00087 double packet_rate, 00088 std::string filename="", 00089 bool read_once=false, 00090 bool read_fast=false, 00091 double repeat_delay=0.0); 00092 ~InputPCAP(); 00093 00094 virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); 00095 00096 private: 00097 00098 std::string filename_; 00099 FILE *fp_; 00100 pcap_t *pcap_; 00101 char errbuf_[PCAP_ERRBUF_SIZE]; 00102 bool empty_; 00103 bool read_once_; 00104 bool read_fast_; 00105 double repeat_delay_; 00106 ros::Rate packet_rate_; 00107 }; 00108 00109 } // velodyne_driver namespace 00110 00111 #endif // __VELODYNE_INPUT_H