io.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 /*****************************************************************************
00035 ** Includes
00036 *****************************************************************************/
00037 
00038 #include <ros/io.h>
00039 #include <ros/assert.h> // don't need if we dont call the pipe functions.
00040 #include <errno.h> // for EFAULT and co.
00041 #include <iostream>
00042 #include <sstream>
00043 #ifdef WIN32
00044 #else
00045   #include <cstring> // strerror
00046   #include <fcntl.h> // for non-blocking configuration
00047 #endif
00048 
00049 /*****************************************************************************
00050 ** Namespaces
00051 *****************************************************************************/
00052 
00053 namespace ros {
00054 
00055 int last_socket_error() {
00056         #ifdef WIN32
00057                 return WSAGetLastError();
00058         #else
00059                 return errno;
00060         #endif
00061 }
00062 const char* last_socket_error_string() {
00063         #ifdef WIN32
00064                 // could fix this to use FORMAT_MESSAGE and print a real string later,
00065                 // but not high priority.
00066                 std::stringstream ostream;
00067                 ostream << "WSA Error: " << WSAGetLastError();
00068                 return ostream.str().c_str();
00069         #else
00070                 return strerror(errno);
00071         #endif
00072 }
00073 
00074 bool last_socket_error_is_would_block() {
00075 #if defined(WIN32)
00076         if ( WSAGetLastError() == WSAEWOULDBLOCK ) {
00077                 return true;
00078         } else {
00079                 return false;
00080         }
00081 #else
00082         if ( ( errno == EAGAIN  ) || ( errno == EWOULDBLOCK ) ) { // posix permits either
00083                 return true;
00084         } else {
00085                 return false;
00086         }
00087 #endif
00088 }
00089 
00090 /*****************************************************************************
00091 ** Service Robotics/Libssh Functions
00092 *****************************************************************************/
00104 int poll_sockets(socket_pollfd *fds, nfds_t nfds, int timeout) {
00105 #if defined(WIN32)
00106         fd_set readfds, writefds, exceptfds;
00107         struct timeval tv, *ptv;
00108         socket_fd_t max_fd;
00109         int rc;
00110         nfds_t i;
00111 
00112         if (fds == NULL) {
00113                 errno = EFAULT;
00114                 return -1;
00115         }
00116 
00117         FD_ZERO (&readfds);
00118         FD_ZERO (&writefds);
00119         FD_ZERO (&exceptfds);
00120 
00121         /*********************
00122         ** Compute fd sets
00123         **********************/
00124         // also find the largest descriptor.
00125         for (rc = -1, max_fd = 0, i = 0; i < nfds; i++) {
00126                 if (fds[i].fd == INVALID_SOCKET) {
00127                         continue;
00128                 }
00129                 if (fds[i].events & (POLLIN | POLLRDNORM)) {
00130                         FD_SET (fds[i].fd, &readfds);
00131                 }
00132                 if (fds[i].events & (POLLOUT | POLLWRNORM | POLLWRBAND)) {
00133                         FD_SET (fds[i].fd, &writefds);
00134                 }
00135                 if (fds[i].events & (POLLPRI | POLLRDBAND)) {
00136                         FD_SET (fds[i].fd, &exceptfds);
00137                 }
00138                 if (fds[i].fd > max_fd &&
00139                           (fds[i].events & (POLLIN | POLLOUT | POLLPRI |
00140                                                                 POLLRDNORM | POLLRDBAND |
00141                                                                 POLLWRNORM | POLLWRBAND))) {
00142                         max_fd = fds[i].fd;
00143                         rc = 0;
00144                 }
00145         }
00146 
00147         if (rc == -1) {
00148                 errno = EINVAL;
00149                 return -1;
00150         }
00151         /*********************
00152         ** Setting the timeout
00153         **********************/
00154         if (timeout < 0) {
00155                 ptv = NULL;
00156         } else {
00157                 ptv = &tv;
00158                 if (timeout == 0) {
00159                         tv.tv_sec = 0;
00160                         tv.tv_usec = 0;
00161                 } else {
00162                         tv.tv_sec = timeout / 1000;
00163                         tv.tv_usec = (timeout % 1000) * 1000;
00164                 }
00165         }
00166 
00167         rc = select (max_fd + 1, &readfds, &writefds, &exceptfds, ptv);
00168         if (rc < 0) {
00169                 return -1;
00170         } else if ( rc == 0 ) {
00171                 return 0;
00172         }
00173 
00174         for (rc = 0, i = 0; i < nfds; i++) {
00175                 if (fds[i].fd != INVALID_SOCKET) {
00176                         fds[i].revents = 0;
00177 
00178                         if (FD_ISSET(fds[i].fd, &readfds)) {
00179                                 int save_errno = errno;
00180                                 char data[64] = {0};
00181                                 int ret;
00182 
00183                                 /* support for POLLHUP */
00184                                 // just check if there's incoming data, without removing it from the queue.
00185                                 ret = recv(fds[i].fd, data, 64, MSG_PEEK);
00186                                 #ifdef WIN32
00187                                 if ((ret == -1) &&
00188                                                 (errno == WSAESHUTDOWN || errno == WSAECONNRESET ||
00189                                                 (errno == WSAECONNABORTED) || errno == WSAENETRESET))
00190                                 #else
00191                                 if ((ret == -1) &&
00192                                                 (errno == ESHUTDOWN || errno == ECONNRESET ||
00193                                                 (errno == ECONNABORTED) || errno == ENETRESET))
00194                                 #endif
00195                                 {
00196                                         fds[i].revents |= POLLHUP;
00197                                 } else {
00198                                         fds[i].revents |= fds[i].events & (POLLIN | POLLRDNORM);
00199                                 }
00200                                 errno = save_errno;
00201                         }
00202                         if (FD_ISSET(fds[i].fd, &writefds)) {
00203                                 fds[i].revents |= fds[i].events & (POLLOUT | POLLWRNORM | POLLWRBAND);
00204                         }
00205 
00206                         if (FD_ISSET(fds[i].fd, &exceptfds)) {
00207                                 fds[i].revents |= fds[i].events & (POLLPRI | POLLRDBAND);
00208                         }
00209 
00210                         if (fds[i].revents & ~POLLHUP) {
00211                                 rc++;
00212                         }
00213                 } else {
00214                                 fds[i].revents = POLLNVAL;
00215                 }
00216         }
00217         return rc;
00218 #else
00219         // use an existing poll implementation
00220         int result = poll(fds, nfds, timeout);
00221         if ( result < 0 ) {
00222                 // EINTR means that we got interrupted by a signal, and is not an error
00223                 if(errno == EINTR) {
00224                         result = 0;
00225                 }
00226         }
00227         return result;
00228 #endif // poll_sockets functions
00229 }
00230 /*****************************************************************************
00231 ** Socket Utilities
00232 *****************************************************************************/
00237 int set_non_blocking(socket_fd_t &socket) {
00238 #ifdef WIN32
00239     u_long non_blocking = 1;
00240         if(ioctlsocket( socket, FIONBIO, &non_blocking ) != 0 )
00241         {
00242       return WSAGetLastError();
00243         }
00244 #else
00245     if(fcntl(socket, F_SETFL, O_NONBLOCK) == -1)
00246     {
00247       return errno;
00248     }
00249 #endif
00250     return 0;
00251 }
00252 
00258 int close_socket(socket_fd_t &socket) {
00259 #ifdef WIN32
00260         if(::closesocket(socket) == SOCKET_ERROR ) {
00261                 return -1;
00262         } else {
00263                 return 0;
00264         }
00265 #else
00266         if (::close(socket) < 0) {
00267                 return -1;
00268         } else {
00269                 return 0;
00270         }
00271 #endif //WIN32
00272 }
00273 
00274 /*****************************************************************************
00275 ** Signal Pair
00276 *****************************************************************************/
00282 int create_signal_pair(signal_fd_t signal_pair[2]) {
00283 #ifdef WIN32 // use a socket pair
00284         signal_pair[0] = INVALID_SOCKET;
00285         signal_pair[1] = INVALID_SOCKET;
00286 
00287     union {
00288        struct sockaddr_in inaddr;
00289        struct sockaddr addr;
00290     } a;
00291     socklen_t addrlen = sizeof(a.inaddr);
00292 
00293     /*********************
00294         ** Listen Socket
00295         **********************/
00296         socket_fd_t listen_socket = INVALID_SOCKET;
00297     listen_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
00298         if (listen_socket == INVALID_SOCKET) {
00299                 return -1;
00300         }
00301 
00302     // allow it to be bound to an address already in use - do we actually need this?
00303     int reuse = 1;
00304     if (setsockopt(listen_socket, SOL_SOCKET, SO_REUSEADDR, (char*) &reuse, (socklen_t) sizeof(reuse)) == SOCKET_ERROR ) {
00305         ::closesocket(listen_socket);
00306                 return -1;
00307     }
00308 
00309     memset(&a, 0, sizeof(a));
00310     a.inaddr.sin_family = AF_INET;
00311     a.inaddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
00312     // For TCP/IP, if the port is specified as zero, the service provider assigns
00313     // a unique port to the application from the dynamic client port range.
00314     a.inaddr.sin_port = 0;
00315 
00316     if  (bind(listen_socket, &a.addr, sizeof(a.inaddr)) == SOCKET_ERROR) {
00317         ::closesocket(listen_socket);
00318                 return -1;
00319     }
00320     // we need this below because the system auto filled in some entries, e.g. port #
00321     if  (getsockname(listen_socket, &a.addr, &addrlen) == SOCKET_ERROR) {
00322         ::closesocket(listen_socket);
00323                 return -1;
00324     }
00325     // max 1 connection permitted
00326     if (listen(listen_socket, 1) == SOCKET_ERROR) {
00327         ::closesocket(listen_socket);
00328                 return -1;
00329     }
00330 
00331     /*********************
00332         ** Connection
00333         **********************/
00334     // do we need io overlapping?
00335     // DWORD flags = (make_overlapped ? WSA_FLAG_OVERLAPPED : 0);
00336     DWORD overlapped_flag = 0;
00337     signal_pair[0] = WSASocket(AF_INET, SOCK_STREAM, 0, NULL, 0, overlapped_flag);
00338     if (signal_pair[0] == INVALID_SOCKET) {
00339         ::closesocket(listen_socket);
00340         ::closesocket(signal_pair[0]);
00341         return -1;
00342     }
00343     // reusing the information from above to connect to the listener
00344     if (connect(signal_pair[0], &a.addr, sizeof(a.inaddr)) == SOCKET_ERROR) {
00345         ::closesocket(listen_socket);
00346         ::closesocket(signal_pair[0]);
00347         return -1;
00348     }
00349     /*********************
00350         ** Accept
00351         **********************/
00352     signal_pair[1] = accept(listen_socket, NULL, NULL);
00353     if (signal_pair[1] == INVALID_SOCKET) {
00354         ::closesocket(listen_socket);
00355         ::closesocket(signal_pair[0]);
00356         ::closesocket(signal_pair[1]);
00357         return -1;
00358     }
00359         /*********************
00360         ** Nonblocking
00361         **********************/
00362     // should we do this or should we set io overlapping?
00363     if ( (set_non_blocking(signal_pair[0]) != 0) || (set_non_blocking(signal_pair[1]) != 0)  ) {
00364                 ::closesocket(listen_socket);
00365                 ::closesocket(signal_pair[0]);
00366                 ::closesocket(signal_pair[1]);
00367         return -1;
00368     }
00369         /*********************
00370         ** Cleanup
00371         **********************/
00372     ::closesocket(listen_socket);  // the listener has done its job.
00373     return 0;
00374 #else // use a pipe pair
00375         // initialize
00376         signal_pair[0] = -1;
00377         signal_pair[1] = -1;
00378 
00379         if(pipe(signal_pair) != 0) {
00380                 ROS_FATAL( "pipe() failed");
00381                 return -1;
00382         }
00383         if(fcntl(signal_pair[0], F_SETFL, O_NONBLOCK) == -1) {
00384                 ROS_FATAL( "fcntl() failed");
00385                 return -1;
00386         }
00387         if(fcntl(signal_pair[1], F_SETFL, O_NONBLOCK) == -1) {
00388                 ROS_FATAL( "fcntl() failed");
00389                 return -1;
00390         }
00391         return 0;
00392 #endif // create_pipe
00393 }
00394 
00395 } // namespace ros


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Tue Mar 7 2017 03:44:47