Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00024 #include <unistd.h>
00025 #include <sys/socket.h>
00026 #include <arpa/inet.h>
00027 #include <poll.h>
00028 #include <errno.h>
00029 #include <fcntl.h>
00030 #include <sys/file.h>
00031 #include <velodyne_driver/input.h>
00032
00033 namespace velodyne_driver
00034 {
00035 static const size_t packet_size = sizeof(velodyne_msgs::VelodynePacket::data);
00036
00038
00040
00046 InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t udp_port):
00047 Input()
00048 {
00049 sockfd_ = -1;
00050
00051
00052 ROS_INFO_STREAM("Opening UDP socket: port " << udp_port);
00053 sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
00054 if (sockfd_ == -1)
00055 {
00056 perror("socket");
00057 return;
00058 }
00059
00060 sockaddr_in my_addr;
00061 memset(&my_addr, 0, sizeof(my_addr));
00062 my_addr.sin_family = AF_INET;
00063 my_addr.sin_port = htons(udp_port);
00064 my_addr.sin_addr.s_addr = INADDR_ANY;
00065
00066 if (bind(sockfd_, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
00067 {
00068 perror("bind");
00069 return;
00070 }
00071
00072 if (fcntl(sockfd_,F_SETFL, O_NONBLOCK|FASYNC) < 0)
00073 {
00074 perror("non-block");
00075 return;
00076 }
00077
00078 ROS_DEBUG("Velodyne socket fd is %d\n", sockfd_);
00079 }
00080
00082 InputSocket::~InputSocket(void)
00083 {
00084 (void) close(sockfd_);
00085 }
00086
00088 int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt)
00089 {
00090 double time1 = ros::Time::now().toSec();
00091
00092 struct pollfd fds[1];
00093 fds[0].fd = sockfd_;
00094 fds[0].events = POLLIN;
00095 static const int POLL_TIMEOUT = 1000;
00096
00097 while (true)
00098 {
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117 do
00118 {
00119 int retval = poll(fds, 1, POLL_TIMEOUT);
00120 if (retval < 0)
00121 {
00122 if (errno != EINTR)
00123 ROS_ERROR("poll() error: %s", strerror(errno));
00124 return 1;
00125 }
00126 if (retval == 0)
00127 {
00128 ROS_WARN("Velodyne poll() timeout");
00129 return 1;
00130 }
00131 if ((fds[0].revents & POLLERR)
00132 || (fds[0].revents & POLLHUP)
00133 || (fds[0].revents & POLLNVAL))
00134 {
00135 ROS_ERROR("poll() reports Velodyne error");
00136 return 1;
00137 }
00138 } while ((fds[0].revents & POLLIN) == 0);
00139
00140
00141
00142 ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0],
00143 packet_size, 0, NULL, NULL);
00144
00145 if (nbytes < 0)
00146 {
00147 if (errno != EWOULDBLOCK)
00148 {
00149 perror("recvfail");
00150 ROS_INFO("recvfail");
00151 return 1;
00152 }
00153 }
00154 else if ((size_t) nbytes == packet_size)
00155 {
00156
00157 break;
00158 }
00159
00160 ROS_DEBUG_STREAM("incomplete Velodyne packet read: "
00161 << nbytes << " bytes");
00162 }
00163
00164
00165
00166 double time2 = ros::Time::now().toSec();
00167 pkt->stamp = ros::Time((time2 + time1) / 2.0);
00168
00169 return 0;
00170 }
00171
00173
00175
00185 InputPCAP::InputPCAP(ros::NodeHandle private_nh,
00186 double packet_rate,
00187 std::string filename,
00188 bool read_once,
00189 bool read_fast,
00190 double repeat_delay):
00191 Input(),
00192 packet_rate_(packet_rate)
00193 {
00194 filename_ = filename;
00195 fp_ = NULL;
00196 pcap_ = NULL;
00197 empty_ = true;
00198
00199
00200 private_nh.param("read_once", read_once_, read_once);
00201 private_nh.param("read_fast", read_fast_, read_fast);
00202 private_nh.param("repeat_delay", repeat_delay_, repeat_delay);
00203
00204 if (read_once_)
00205 ROS_INFO("Read input file only once.");
00206 if (read_fast_)
00207 ROS_INFO("Read input file as quickly as possible.");
00208 if (repeat_delay_ > 0.0)
00209 ROS_INFO("Delay %.3f seconds before repeating input file.",
00210 repeat_delay_);
00211
00212
00213 ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
00214 if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_) ) == NULL)
00215 {
00216 ROS_FATAL("Error opening Velodyne socket dump file.");
00217 return;
00218 }
00219 }
00220
00221
00223 InputPCAP::~InputPCAP(void)
00224 {
00225 pcap_close(pcap_);
00226 }
00227
00228
00230 int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt)
00231 {
00232 struct pcap_pkthdr *header;
00233 const u_char *pkt_data;
00234
00235 while (true)
00236 {
00237 int res;
00238 if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
00239 {
00240
00241
00242
00243 if (read_fast_ == false)
00244 packet_rate_.sleep();
00245
00246 memcpy(&pkt->data[0], pkt_data+42, packet_size);
00247 pkt->stamp = ros::Time::now();
00248 empty_ = false;
00249 return 0;
00250 }
00251
00252 if (empty_)
00253 {
00254 ROS_WARN("Error %d reading Velodyne packet: %s",
00255 res, pcap_geterr(pcap_));
00256 return -1;
00257 }
00258
00259 if (read_once_)
00260 {
00261 ROS_INFO("end of file reached -- done reading.");
00262 return -1;
00263 }
00264
00265 if (repeat_delay_ > 0.0)
00266 {
00267 ROS_INFO("end of file reached -- delaying %.3f seconds.",
00268 repeat_delay_);
00269 usleep(rint(repeat_delay_ * 1000000.0));
00270 }
00271
00272 ROS_DEBUG("replaying Velodyne dump file");
00273
00274
00275
00276
00277 pcap_close(pcap_);
00278 pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
00279 empty_ = true;
00280 }
00281 }
00282
00283 }