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; } }
void hokuyo::Laser::open(const char * port_name) { if (portOpen()) close(); laser_port_ = fopen(port_name, "r+"); if (laser_port_ == NULL) { const char *extra_msg = ""; switch (errno) { case EACCES: extra_msg = "You probably don't have premission to open the port for reading and writing."; break; case ENOENT: extra_msg = "The requested port does not exist. Is the hokuyo connected? Was the port name misspelled?"; break; } HOKUYO_EXCEPT(hokuyo::Exception, "Failed to open port: %s. %s (errno = %d). %s", port_name, strerror(errno), errno, extra_msg); } try { laser_fd_ = fileno (laser_port_); if (laser_fd_ == -1) HOKUYO_EXCEPT(hokuyo::Exception, "Failed to get file descriptor -- error = %d: %s", errno, strerror(errno)); // 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. // No error checking. This really shouldn't fail, and even if it does, // we aren't so badly off. fcntl(laser_fd_, F_SETFL, fcntl(laser_fd_,F_GETFL,0) | O_NONBLOCK); struct flock fl; fl.l_type = F_WRLCK; fl.l_whence = SEEK_SET; fl.l_start = 0; fl.l_len = 0; fl.l_pid = getpid(); if (fcntl(laser_fd_, F_SETLK, &fl) != 0) HOKUYO_EXCEPT(hokuyo::Exception, "Device %s is already locked. Try 'lsof | grep %s' to find other processes that currently have the port open.", port_name, port_name); // Settings for USB? struct termios newtio; memset (&newtio, 0, sizeof (newtio)); newtio.c_cflag = CS8 | CLOCAL | CREAD; newtio.c_iflag = IGNPAR; newtio.c_oflag = 0; newtio.c_lflag = ICANON; // activate new settings tcflush (laser_fd_, TCIFLUSH); if (tcsetattr (laser_fd_, TCSANOW, &newtio) < 0) HOKUYO_EXCEPT(hokuyo::Exception, "Unable to set serial port attributes. The port you specified (%s) may not be a serial port.", port_name); /// @todo tcsetattr returns true if at least one attribute was set. Hence, we might not have set everything on success. usleep (200000); // 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_port_ != NULL) fclose(laser_port_); laser_port_ = NULL; laser_fd_ = -1; throw e; } }