void hokuyo::Laser::close () { int retval = 0; if (portOpen()) { //Try to be a good citizen and completely shut down the laser before we shutdown communication try { reset(); } catch (hokuyo::Exception& e) { //Exceptions here can be safely ignored since we are closing the port anyways } #if HOKUYO_INTERFACE_ETHERNET tcpclient_close(&tcpclient); #else retval = ::close(laser_fd_); // Automatically releases the lock. #endif } laser_fd_ = -1; if (retval != 0) HOKUYO_EXCEPT(hokuyo::Exception, "Failed to close port properly -- error = %d: %s\n", errno, strerror(errno)); }
void hokuyo::Laser::open(const char * tcphost, const int tcpport) { if (portOpen()) close(); // Make IO non blocking. This way there are no race conditions that // cause blocking when a badly behaving process does a read at the same // time as us. Will need to switch to blocking for writes or errors // occur just after a replug event. laser_fd_ = tcpclient_open(&tcpclient,tcphost, tcpport);//::open(port_name, O_RDWR | O_NONBLOCK | O_NOCTTY); //laser_fd_ = tcpclient_open(&tcpclient,"127.0.0.1", 10001);//::open(port_name, O_RDWR | O_NONBLOCK | O_NOCTTY); read_buf_start = read_buf_end = 0; if (laser_fd_ < 0) { HOKUYO_EXCEPT(hokuyo::Exception, "Failed to open tcp_client: %s:%d. %s (errno = %d)", tcphost,tcpport, strerror(errno), errno); } laser_fd_ = tcpclient.sock_desc; try { // Some models (04LX) need to be told to go into SCIP2 mode... laserFlush(); // Just in case a previous failure mode has left our Hokuyo // spewing data, we send reset the laser to be safe. try { reset(); } catch (hokuyo::Exception &e) { // This might be a device that needs to be explicitely placed in // SCIP2 mode. // Note: Not tested: a device that is currently scanning in SCIP1.1 // mode might not manage to switch to SCIP2.0. setToSCIP2(); // If this fails then it wasn't a device that could be switched to SCIP2. reset(); // If this one fails, it really is an error. } querySensorConfig(); queryVersionInformation(); // In preparation for calls to get various parts of the version info. } catch (hokuyo::Exception& e) { // These exceptions mean something failed on open and we should close if (laser_fd_ != -1) tcpclient_close(&tcpclient); laser_fd_ = -1; throw e; } }
int tcpclient_open(urg_tcpclient_t* cli, const char* ip_str, int port_num) { enum { Connect_timeout_second = 2 }; fd_set rmask, wmask; struct timeval tv = { Connect_timeout_second, 0 }; #if defined(URG_WINDOWS_OS) u_long flag; #else int flag; int sock_optval = -1; int sock_optval_size = sizeof(sock_optval); #endif int ret; cli->sock_desc = Invalid_desc; cli->pushed_back = -1; // no pushed back char. #if defined(URG_WINDOWS_OS) { static int is_initialized = 0; WORD wVersionRequested = 0x0202; WSADATA WSAData; int err; if (!is_initialized) { err = WSAStartup(wVersionRequested, &WSAData); if (err != 0) { return -1; } is_initialized = 1; } } #endif tcpclient_buffer_init(cli); cli->sock_addr_size = sizeof (struct sockaddr_in); if ((cli->sock_desc = (int)socket(AF_INET, SOCK_STREAM, 0)) < 0) { return -1; } memset((char*)&(cli->server_addr), 0, sizeof(cli->sock_addr_size)); cli->server_addr.sin_family = AF_INET; cli->server_addr.sin_port = htons(port_num); if (!strcmp(ip_str, "localhost")) { ip_str = "127.0.0.1"; } /* bind is not required, and port number is dynamic */ if ((cli->server_addr.sin_addr.s_addr = inet_addr(ip_str)) == INADDR_NONE) { return -1; } #if defined(URG_WINDOWS_OS) // Configures non-blocking mode flag = 1; ioctlsocket(cli->sock_desc, FIONBIO, &flag); if (connect(cli->sock_desc, (const struct sockaddr *)&(cli->server_addr), cli->sock_addr_size) == SOCKET_ERROR) { int error_number = WSAGetLastError(); if (error_number != WSAEWOULDBLOCK) { tcpclient_close(cli); return -1; } FD_ZERO(&rmask); FD_SET((SOCKET)cli->sock_desc, &rmask); wmask = rmask; ret = select((int)cli->sock_desc + 1, &rmask, &wmask, NULL, &tv); if (ret == 0) { // Operation timed out tcpclient_close(cli); return -2; } } // Returns to blocking mode set_block_mode(cli); #else // Configures non-blocking mode flag = fcntl(cli->sock_desc, F_GETFL, 0); fcntl(cli->sock_desc, F_SETFL, flag | O_NONBLOCK); if (connect(cli->sock_desc, (const struct sockaddr *)&(cli->server_addr), cli->sock_addr_size) < 0) { if (errno != EINPROGRESS) { tcpclient_close(cli); return -1; } // EINPROGRESS: a connection request was already received and not completed yet FD_ZERO(&rmask); FD_SET(cli->sock_desc, &rmask); wmask = rmask; ret = select(cli->sock_desc + 1, &rmask, &wmask, NULL, &tv); if (ret <= 0) { // Operation timed out tcpclient_close(cli); return -2; } if (getsockopt(cli->sock_desc, SOL_SOCKET, SO_ERROR, (int*)&sock_optval, (socklen_t*)&sock_optval_size) != 0) { // Connection failed tcpclient_close(cli); return -3; } if (sock_optval != 0) { // Connection failed tcpclient_close(cli); return -4; } // Returns to blocking mode set_block_mode(cli); } #endif return 0; }
int tcpclient_open(tcpclient_t* cli, const char* ip_str, int port_num) { enum { Connect_timeout_second = 2 }; fd_set rmask, wmask; struct timeval tv = { Connect_timeout_second, 0 }; int flag; int sock_optval = -1; int sock_optval_size = sizeof(sock_optval); int ret; cli->sock_desc = Invalid_desc; cli->pushed_back = -1; // no pushed back char. tcpclient_buffer_init(cli); cli->sock_addr_size = sizeof (struct sockaddr_in); if ((cli->sock_desc = (int)socket(AF_INET, SOCK_STREAM, 0)) < 0) { return -1; } memset((char*)&(cli->server_addr), 0, sizeof(cli->sock_addr_size)); cli->server_addr.sin_family = AF_INET; cli->server_addr.sin_port = htons(port_num); if (!strcmp(ip_str, "localhost")) { ip_str = "127.0.0.1"; } /* bind is not required, and port number is dynamic */ if ((cli->server_addr.sin_addr.s_addr = inet_addr(ip_str)) == INADDR_NONE) { return -1; } flag = fcntl(cli->sock_desc, F_GETFL, 0); fcntl(cli->sock_desc, F_SETFL, flag | O_NONBLOCK); //printf("cli->sock_desc: %d\n",cli->sock_desc); if (connect(cli->sock_desc, (const struct sockaddr *)&(cli->server_addr), cli->sock_addr_size) < 0) { if (errno != EINPROGRESS) { tcpclient_close(cli); return -1; } // EINPROGRESS: FD_ZERO(&rmask); FD_SET(cli->sock_desc, &rmask); wmask = rmask; ret = select(cli->sock_desc + 1, &rmask, &wmask, NULL, &tv); if (ret <= 0) { tcpclient_close(cli); return -2; } if (getsockopt(cli->sock_desc, SOL_SOCKET, SO_ERROR, (int*)&sock_optval, (socklen_t*)&sock_optval_size) != 0) { tcpclient_close(cli); return -3; } if (sock_optval != 0) { tcpclient_close(cli); return -4; } set_block_mode(cli); } //printf("connection ok!\n"); return 0; }