int hokuyo::Laser::stopScanning() { try { return laserOff(); } catch (hokuyo::Exception &e) { // Ignore exception because we might have gotten part of a scan // instead of the expected response, which shows up as a bad checksum. laserFlush(); } return laserOff(); // This one should work because the scan is stopped. }
void hokuyo::Laser::reset () { if (!portOpen()) HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); laserFlush(); try { sendCmd("TM2", 1000); } catch (hokuyo::Exception &e) {} // Ignore. If the laser was scanning TM2 would fail try { sendCmd("RS", 1000); last_time_ = 0; // RS resets the hokuyo clock. wrapped_ = 0; // RS resets the hokuyo clock. } catch (hokuyo::Exception &e) {} // Ignore. If the command coincided with a scan we might get garbage. laserFlush(); sendCmd("RS", 1000); // This one should just work. }
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; } }