/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include "hwdrivers-precomp.h" // Precompiled headers // #include #include #include #include #include // in library mrpt-maps #include #include #include #include IMPLEMENTS_GENERIC_SENSOR(CHokuyoURG, mrpt::hwdrivers) using namespace mrpt::obs; using namespace mrpt::hwdrivers; using namespace mrpt::comms; using namespace mrpt::system; using namespace mrpt::opengl; using namespace std; const int MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE = 10; CHokuyoURG::CHokuyoURG() : m_rx_buffer(40000) { m_sensorLabel = "Hokuyo"; } CHokuyoURG::~CHokuyoURG() { try { m_win.reset(); closeStreamConnection(); } catch (const std::exception& e) { std::cerr << "[~CHokuyoURG] Exception:\n" << mrpt::exception_to_str(e); } } void CHokuyoURG::closeStreamConnection() { if (m_stream) { turnOff(); m_stream.reset(); } } void CHokuyoURG::sendCmd(const char* str) { MRPT_START ASSERT_(str != nullptr); ASSERT_(m_stream != nullptr); const size_t N = strlen(str); const size_t nWriten = m_stream->Write(str, N); ASSERT_EQUAL_(nWriten, N); MRPT_LOG_DEBUG_STREAM("[Hokuyo] sendCmd(): `" << str << "`"); m_lastSentMeasCmd = std::string(str); // for echo verification MRPT_END } void CHokuyoURG::doProcessSimple( bool& outThereIsObservation, mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError) { #if 0 static mrpt::system::CTimeLogger timLogger("CHokuyoURG"); mrpt::system::CTimeLoggerEntry tle(timLogger, "doProcessSimple"); #endif outThereIsObservation = false; hardwareError = false; // Bound? If not, connect to the device and set it up (again) in // continuous read mode: if (!ensureStreamIsOpen()) { m_timeStartUI = 0; m_timeStartSynchDelay = 0; hardwareError = true; return; } // Wait for a message: int nRanges = m_lastRange - m_firstRange + 1; int expectedSize = nRanges * 3 + 4; if (m_intensity) expectedSize += nRanges * 3; m_rcv_data.clear(); m_rcv_data.reserve(expectedSize + 1000); m_state = ssWorking; if (!parseResponse(false)) { if (!internal_notifyNoScanReceived()) { // It seems the sensor needs to be reset (?), let this know // to the caller: m_state = ssError; hardwareError = true; // And on our side, close the connection to ensure initialization // is called again to set-up the laser in the next call to // ensureStreamIsOpen() above. closeStreamConnection(); return; } // No new data return; } // DECODE: if (m_rcv_status0 != '0' && m_rcv_status0 != '9') { hardwareError = true; return; } // ----------------------------------------------- // Extract the observation: // ----------------------------------------------- outObservation.timestamp = mrpt::Clock::now(); if ((size_t)expectedSize != m_rcv_data.size()) { MRPT_LOG_ERROR_STREAM( "[CHokuyoURG::doProcess] ERROR: Expected " << expectedSize << " data bytes, received " << m_rcv_data.size() << "instead!"); hardwareError = true; return; } // Delay the sync of timestamps due to instability in the constant rate // during the first few packets. bool do_timestamp_sync = !m_disable_firmware_timestamp; if (do_timestamp_sync && m_timeStartSynchDelay < MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE) { do_timestamp_sync = false; m_timeStartSynchDelay++; } if (do_timestamp_sync) { // Extract the timestamp of the sensor: uint32_t nowUI = ((m_rcv_data[0] - 0x30) << 18) + ((m_rcv_data[1] - 0x30) << 12) + ((m_rcv_data[2] - 0x30) << 6) + (m_rcv_data[3] - 0x30); uint32_t AtUI = 0; if (m_timeStartUI == 0) { m_timeStartUI = nowUI; m_timeStartTT = mrpt::Clock::now(); } else AtUI = nowUI - m_timeStartUI; auto AtDO = std::chrono::milliseconds(AtUI); outObservation.timestamp = m_timeStartTT + AtDO; } // And the scan ranges: outObservation.rightToLeft = true; outObservation.aperture = nRanges * 2 * M_PI / m_sensor_info.scans_per_360deg; outObservation.maxRange = m_sensor_info.d_max; outObservation.stdError = 0.010f; outObservation.sensorPose = m_sensorPose; outObservation.sensorLabel = m_sensorLabel; outObservation.resizeScan(nRanges); char* ptr = (char*)&m_rcv_data[4]; if (m_intensity) outObservation.setScanHasIntensity(true); for (int i = 0; i < nRanges; i++) { int b1 = (*ptr++) - 0x30; int b2 = (*ptr++) - 0x30; int b3 = (*ptr++) - 0x30; int range_mm = ((b1 << 12) | (b2 << 6) | b3); outObservation.setScanRange(i, range_mm * 0.001f); outObservation.setScanRangeValidity( i, range_mm >= 20 && (outObservation.getScanRange(i) <= outObservation.maxRange)); if (m_intensity) { int b4 = (*ptr++) - 0x30; int b5 = (*ptr++) - 0x30; int b6 = (*ptr++) - 0x30; outObservation.setScanIntensity(i, ((b4 << 12) | (b5 << 6) | b6)); } } // Do filter: C2DRangeFinderAbstract::filterByExclusionAreas(outObservation); C2DRangeFinderAbstract::filterByExclusionAngles(outObservation); // Do show preview: C2DRangeFinderAbstract::processPreview(outObservation); outThereIsObservation = true; internal_notifyGoodScanNow(); } /*------------------------------------------------------------- loadConfig_sensorSpecific -------------------------------------------------------------*/ void CHokuyoURG::loadConfig_sensorSpecific( const mrpt::config::CConfigFileBase& c, const std::string& s) { m_reduced_fov = DEG2RAD(c.read_float(s, "reduced_fov", 0)), m_motorSpeed_rpm = c.read_int(s, "HOKUYO_motorSpeed_rpm", 0); m_sensorPose.setFromValues( c.read_float(s, "pose_x", 0), c.read_float(s, "pose_y", 0), c.read_float(s, "pose_z", 0), DEG2RAD(c.read_float(s, "pose_yaw", 0)), DEG2RAD(c.read_float(s, "pose_pitch", 0)), DEG2RAD(c.read_float(s, "pose_roll", 0))); m_highSensMode = c.read_bool(s, "HOKUYO_HS_mode", m_highSensMode); #ifdef _WIN32 const char* comVarName = "COM_port_WIN"; #else const char* comVarName = "COM_port_LIN"; #endif m_com_port = c.read_string(s, comVarName, m_com_port); m_ip_dir = c.read_string(s, "IP_DIR", m_ip_dir); m_port_dir = c.read_int(s, "PORT_DIR", m_port_dir); ASSERTMSG_( !m_com_port.empty() || !m_ip_dir.empty(), "Either COM_port or IP_DIR must be defined in the configuration file!"); ASSERTMSG_( m_com_port.empty() || m_ip_dir.empty(), "Both COM_port and IP_DIR set! Please, define only one of them."); if (!m_ip_dir.empty()) { ASSERTMSG_( m_port_dir, "A TCP/IP port number `PORT_DIR` must be specified for Ethernet " "connection"); } m_disable_firmware_timestamp = c.read_bool(s, "disable_firmware_timestamp", m_disable_firmware_timestamp); m_intensity = c.read_bool(s, "intensity", m_intensity), MRPT_LOAD_HERE_CONFIG_VAR(scan_interval, int, m_scan_interval, c, s); MRPT_LOAD_HERE_CONFIG_VAR(comms_timeout_ms, int, m_comms_timeout_ms, c, s); MRPT_LOAD_HERE_CONFIG_VAR(comms_between_timeout_ms, int, m_comms_between_timeout_ms, c, s); // Parent options: C2DRangeFinderAbstract::loadCommonParams(c, s); } /*------------------------------------------------------------- turnOn -------------------------------------------------------------*/ bool CHokuyoURG::turnOn() { MRPT_START // Bound? if (!ensureStreamIsOpen()) return false; // If we are over a serial link, set it up: if (m_ip_dir.empty()) { auto* COM = dynamic_cast(m_stream.get()); if (COM != nullptr) { // It is a COM: COM->setConfig(19200); COM->setTimeouts(m_comms_timeout_ms, 0, m_comms_timeout_ms, 0, 0); // Assure the laser is off and quiet: switchLaserOff(); std::this_thread::sleep_for(10ms); COM->purgeBuffers(); std::this_thread::sleep_for(10ms); COM->setConfig(115200); switchLaserOff(); std::this_thread::sleep_for(10ms); COM->purgeBuffers(); std::this_thread::sleep_for(10ms); COM->setConfig(19200); } if (COM != nullptr) { // Set 115200 baud rate: setHighBaudrate(); enableSCIP20(); COM->setConfig(115200); } } else { auto* COM = dynamic_cast(m_stream.get()); if (COM != nullptr) { // Assure the laser is off and quiet: switchLaserOff(); std::this_thread::sleep_for(10ms); purgeBuffers(); std::this_thread::sleep_for(10ms); switchLaserOff(); std::this_thread::sleep_for(10ms); purgeBuffers(); } } if (!enableSCIP20()) return false; // Turn on the laser: if (!switchLaserOn()) return false; // Set the motor speed: if (m_motorSpeed_rpm) if (!setMotorSpeed(m_motorSpeed_rpm)) return false; // Set HS mode: setHighSensitivityMode(m_highSensMode); // Display sensor information: if (!displaySensorInfo(&m_sensor_info)) return false; // Set for scanning angles: m_firstRange = m_sensor_info.scan_first; m_lastRange = m_sensor_info.scan_last; // Artificially reduced FOV? if (m_reduced_fov > 0 && m_reduced_fov < 2 * M_PI) { int center = (m_lastRange + m_firstRange) >> 1; const int half_range = static_cast((m_sensor_info.scans_per_360deg / 360.0) * RAD2DEG(m_reduced_fov)) >> 1; m_firstRange = center - half_range; m_lastRange = center + half_range; MRPT_LOG_INFO_STREAM( "[HOKUYO::turnOn] Using reduced FOV: ranges [" << m_firstRange << "-" << m_lastRange << "] for " << RAD2DEG(m_reduced_fov) << " deg. FOV"); } if (!displayVersionInfo()) { // return false; // It's not SO important } // Start! if (!startScanningMode()) return false; return true; MRPT_END } /*------------------------------------------------------------- turnOff -------------------------------------------------------------*/ bool CHokuyoURG::turnOff() { // Turn off the laser: if (!switchLaserOff()) return false; return true; } bool CHokuyoURG::setHighBaudrate() { if (!ensureStreamIsOpen()) return false; MRPT_LOG_DEBUG("[CHokuyoURG::setHighBaudrate] Changing baudrate to 115200..."); // Send command: sendCmd("SS115200\x0A"); // Receive response: if (!parseResponse()) { MRPT_LOG_ERROR("[CHokuyoURG::setHighBaudrate] Error waiting for response"); return false; } MRPT_LOG_DEBUG("OK\n"); return true; } /*------------------------------------------------------------- ensureBufferHasBytes -------------------------------------------------------------*/ bool CHokuyoURG::ensureBufferHasBytes(size_t nDesiredBytes, bool additionalWaitForData) { ASSERT_LT_(nDesiredBytes, m_rx_buffer.capacity()); if (m_rx_buffer.size() >= nDesiredBytes) return true; // Try to read more bytes: std::array buf; const size_t to_read = std::min(m_rx_buffer.available(), buf.size()); try { const size_t nRead = [this, &buf, additionalWaitForData, to_read]() { if (auto sock = dynamic_cast(m_stream.get()); sock) { // socket: auto timeout = m_comms_timeout_ms; if (additionalWaitForData) mrpt::keep_max(timeout, 100); return sock->readAsync(&buf[0], to_read, timeout, m_comms_between_timeout_ms); } else { // serial port: return m_stream->Read(&buf[0], to_read); } }(); m_rx_buffer.push_many(&buf[0], nRead); } catch (std::exception&) { // 0 bytes read } return (m_rx_buffer.size() >= nDesiredBytes); } bool CHokuyoURG::parseResponse(bool additionalWaitForData) { m_rcv_status0 = '\0'; m_rcv_status1 = '\0'; m_rcv_data.clear(); if (!ensureStreamIsOpen()) return false; ASSERT_(!m_lastSentMeasCmd.empty()); try { // Process response: // --------------------------------- char tmp_rcv_status0 = '\0', tmp_rcv_status1 = '\0'; // COMMAND ECHO --------- size_t peekIdx = 0; const unsigned int verifLen = m_lastSentMeasCmd.size(); if (verifLen) { unsigned int i = 0; do { if (!ensureBufferHasBytes(verifLen - i, additionalWaitForData)) return false; // If matches the echo, go on: if (m_rx_buffer.peek(peekIdx++) == m_lastSentMeasCmd[i]) { // Match is ok: i++; } else { // Skip one byte and keep trying to match: m_rx_buffer.pop(); i = 0; peekIdx = 0; } } while (i < verifLen); } // Now, the status bytes: if (!ensureBufferHasBytes(peekIdx + 2, additionalWaitForData)) return false; tmp_rcv_status0 = m_rx_buffer.peek(peekIdx++); tmp_rcv_status1 = m_rx_buffer.peek(peekIdx++); // In SCIP2.0, there is an additional sum char: if (tmp_rcv_status1 != 0x0A) { // Yes, it is SCIP2.0 if (!ensureBufferHasBytes(peekIdx + 1, additionalWaitForData)) return false; // Ignore this byte: sumStatus peekIdx++; } else { // Continue, it seems a SCIP1.1 response... } // After the status bytes, there must be a LF: if (!ensureBufferHasBytes(peekIdx + 1, additionalWaitForData)) return false; char nextChar = m_rx_buffer.peek(peekIdx++); if (nextChar != 0x0A) return false; // ----------------------------------------------------------------------------- // Now the data: // There's a problem here, we don't know in advance how many bytes to // read, // so rely on the serial port class implemented buffer and call many // times // the read method with only 1 byte each time: // ----------------------------------------------------------------------------- bool lastWasLF = false; std::string tmp_rx; for (;;) { if (!ensureBufferHasBytes(peekIdx + 1, additionalWaitForData)) { // Do not empty the queue, it seems we need to wait for more // data: return false; } tmp_rx.push_back(m_rx_buffer.peek(peekIdx++)); // No data? if (tmp_rx.size() == 1 && tmp_rx[0] == 0x0A) { tmp_rx.clear(); m_rcv_status0 = tmp_rcv_status0; m_rcv_status1 = tmp_rcv_status1; // Empty read bytes so far: for (size_t k = 0; k < peekIdx; k++) m_rx_buffer.pop(); return true; } // Is it a LF? if (*tmp_rx.rbegin() != 0x0A) { lastWasLF = false; continue; } // This was a single LF: if (!lastWasLF) { // Discard SUM+LF ASSERT_(tmp_rx.size() >= 2); tmp_rx.resize(tmp_rx.size() - 2); } else { // This was a double LF. // Discard this last LF: tmp_rx.resize(tmp_rx.size() - 1); // Done! m_rcv_data = tmp_rx; m_rcv_status0 = tmp_rcv_status0; m_rcv_status1 = tmp_rcv_status1; // Empty read bytes so far: for (size_t k = 0; k < peekIdx; k++) m_rx_buffer.pop(); MRPT_LOG_DEBUG_STREAM("[Hokuyo] parseResponse(): RX `" << m_rcv_data << "`"); if (m_rcv_status0 != '0' && (m_rcv_status0 != '9' && m_rcv_status1 != '9')) { MRPT_LOG_ERROR_STREAM( "[Hokuyo] Error LIDAR status: " << (int)m_rcv_status0 << " after command: `" << m_lastSentMeasCmd << "`"); return false; } return true; } lastWasLF = true; } } catch (const std::exception& e) { MRPT_LOG_ERROR_FMT("[Hokuyo] parseResponse() Exception: %s", e.what()); return false; } catch (...) { return false; // Serial port timeout,... } } bool CHokuyoURG::enableSCIP20() { if (!ensureStreamIsOpen()) return false; MRPT_LOG_DEBUG("[CHokuyoURG::enableSCIP20] Changing protocol to SCIP2.0..."); // Send command: sendCmd("SCIP2.0\x0A"); // Receive response: if (!parseResponse()) { MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); return false; } MRPT_LOG_DEBUG("OK\n"); return true; } bool CHokuyoURG::switchLaserOn() { if (!ensureStreamIsOpen()) return false; MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOn] Switching laser ON..."); // Send command: sendCmd("BM\x0A"); // Receive response: if (!parseResponse()) { MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); return false; } MRPT_LOG_DEBUG("OK\n"); return true; } bool CHokuyoURG::switchLaserOff() { if (!ensureStreamIsOpen()) return false; MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOff] Switching laser OFF..."); // Send command: sendCmd("QT\x0A"); // Receive response: if (!parseResponse()) { MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); return false; } MRPT_LOG_DEBUG("OK\n"); return true; } void CHokuyoURG::setScanInterval(unsigned int skipScanCount) { m_scan_interval = skipScanCount; } unsigned int CHokuyoURG::getScanInterval() const { return m_scan_interval; } bool CHokuyoURG::setMotorSpeed(int motoSpeed_rpm) { if (!ensureStreamIsOpen()) return false; MRPT_LOG_DEBUG_FMT("[CHokuyoURG::setMotorSpeed] Setting to %i rpm...", motoSpeed_rpm); // Send command: int motorSpeedCode = (600 - motoSpeed_rpm) / 6; if (motorSpeedCode < 0 || motorSpeedCode > 10) { MRPT_LOG_ERROR_STREAM( __CURRENT_FUNCTION_NAME__ << " Motorspeed must be in the range 540-600 rpm"); return false; } char cmd[20]; os::sprintf(cmd, 20, "CR%02i\x0A", motorSpeedCode); sendCmd(cmd); // Receive response: if (!parseResponse()) { MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); return false; } MRPT_LOG_DEBUG("OK\n"); return true; } /*------------------------------------------------------------- setHighSensitivityMode -------------------------------------------------------------*/ bool CHokuyoURG::setHighSensitivityMode(bool enabled) { if (!ensureStreamIsOpen()) return false; MRPT_LOG_DEBUG_FMT( "[CHokuyoURG::setHighSensitivityMode] Setting HS mode to: %s...", enabled ? "true" : "false"); // Send command: char cmd[20]; os::sprintf(cmd, 20, "HS%i\x0A", enabled ? 1 : 0); sendCmd(cmd); // Receive response: if (!parseResponse()) { MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); return false; } MRPT_LOG_DEBUG("OK\n"); return true; } /*------------------------------------------------------------- setIntensityMode -------------------------------------------------------------*/ bool CHokuyoURG::setIntensityMode(bool enabled) { m_intensity = enabled; return true; } bool CHokuyoURG::displayVersionInfo() { if (!ensureStreamIsOpen()) return false; MRPT_LOG_DEBUG("[CHokuyoURG::displayVersionInfo] Asking info..."); // Send command: sendCmd("VV\x0A"); // Receive response: if (!parseResponse()) { MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); return false; } MRPT_LOG_DEBUG("OK\n"); // PRINT: for (auto& c : m_rcv_data) if (c == ';') c = '\n'; m_rcv_data[m_rcv_data.size()] = '\0'; if (!m_rcv_data.empty()) { MRPT_LOG_INFO_STREAM( "\n------------- HOKUYO Scanner: Version Information ------\n" << &m_rcv_data[0] << "\n" "-------------------------------------------------------\n\n"); } return true; } /*------------------------------------------------------------- displaySensorInfo -------------------------------------------------------------*/ bool CHokuyoURG::displaySensorInfo(TSensorInfo* out_data) { if (!ensureStreamIsOpen()) return false; MRPT_LOG_DEBUG("[CHokuyoURG::displaySensorInfo] Asking for info..."); // Send command: sendCmd("PP\x0A"); // Receive response: if (!parseResponse()) { MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); return false; } MRPT_LOG_DEBUG("OK\n"); // PRINT: for (auto& c : m_rcv_data) if (c == ';') c = '\n'; m_rcv_data[m_rcv_data.size()] = '\0'; if (!m_rcv_data.empty()) { MRPT_LOG_INFO_STREAM( "\n------------- HOKUYO Scanner: Product Information ------\n" << &m_rcv_data[0] << "\n" "-------------------------------------------------------\n\n"); } // Parse the data: if (out_data) { const char* ptr; if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMAX:"))) out_data->d_max = 0.001 * atoi(ptr + 5); else MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMIN:"))) out_data->d_min = 0.001 * atoi(ptr + 5); else MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); if (nullptr != (ptr = strstr(&m_rcv_data[0], "ARES:"))) out_data->scans_per_360deg = atoi(ptr + 5); else MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); if (nullptr != (ptr = strstr(&m_rcv_data[0], "SCAN:"))) out_data->motor_speed_rpm = atoi(ptr + 5); else MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMIN:"))) out_data->scan_first = atoi(ptr + 5); else MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMAX:"))) out_data->scan_last = atoi(ptr + 5); else MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); if (nullptr != (ptr = strstr(&m_rcv_data[0], "AFRT:"))) out_data->scan_front = atoi(ptr + 5); else MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); if (nullptr != (ptr = strstr(&m_rcv_data[0], "MODL:"))) { char aux[30]; memcpy(aux, ptr + 5, 8); aux[8] = '\0'; out_data->model = aux; } else MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error"); } return true; } bool CHokuyoURG::startScanningMode() { if (!ensureStreamIsOpen()) return false; MRPT_LOG_DEBUG("[CHokuyoURG::startScanningMode] Starting scanning mode..."); // Send command: // 'M' 'D' // 'XXXX' (starting step) // 'XXXX' (end step) // 'XX' (cluster count) // 'X' (scan interval) // 'XX' (number of scans) char cmd[50]; unsigned int scan_interval = m_scan_interval; if (scan_interval > 9) scan_interval = 9; os::sprintf( cmd, 50, "M%c%04u%04u01%u00\x0A", m_intensity ? 'E' : 'D', m_firstRange, m_lastRange, scan_interval); sendCmd(cmd); // Receive response: if (!parseResponse()) { MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response"); return false; } MRPT_LOG_DEBUG("OK\n"); return true; } bool CHokuyoURG::ensureStreamIsOpen() { MRPT_START if (m_stream) { // Socket or USB connection? if (!m_ip_dir.empty() && m_port_dir) { // Has the port been disconected (USB serial ports)?? auto* COM = dynamic_cast(m_stream.get()); if (COM != nullptr) { if (COM->isConnected()) return true; // It has been disconnected... try to reconnect: MRPT_LOG_ERROR( "[CHokuyoURG] Socket connection lost! trying to " "reconnect..."); try { COM->connect(m_ip_dir, m_port_dir); // OK, reconfigure the laser: turnOn(); return true; } catch (...) { // Not yet.. return false; } } else { return true; // Assume OK } } else { // Has the port been disconected (USB serial ports)?? auto* COM = dynamic_cast(m_stream.get()); if (COM != nullptr) { if (COM->isOpen()) return true; // It has been disconnected... try to reconnect: MRPT_LOG_ERROR_STREAM( __CURRENT_FUNCTION_NAME__ << ": Serial port connection lost! Trying to reconnect..."); try { COM->open(); // OK, reconfigure the laser: turnOn(); return true; } catch (...) { // Not yet.. return false; } } else { return true; // Assume OK } } } else { if (m_com_port.empty() && (m_ip_dir.empty() || !m_port_dir)) { THROW_EXCEPTION( "No stream bound to the laser nor COM serial port or ip and " "port provided in 'm_com_port','m_ip_dir' and 'm_port_dir'"); } if (!m_ip_dir.empty()) { // Connect to the TCP/IP port: auto theCOM = std::make_shared(); MRPT_LOG_INFO_STREAM( __CURRENT_FUNCTION_NAME__ << " Connecting to " << m_ip_dir << ":" << m_port_dir); theCOM->connect(m_ip_dir, m_port_dir); if (!theCOM->isConnected()) { MRPT_LOG_ERROR_STREAM( __CURRENT_FUNCTION_NAME__ << " Cannot connect with the server '" << m_com_port << "'"); return false; } // Bind: bindIO(theCOM); } else { // Try to open the serial port: auto theCOM = std::make_shared(m_com_port, true); if (!theCOM->isOpen()) { MRPT_LOG_ERROR_STREAM( __CURRENT_FUNCTION_NAME__ << " Cannot open serial port '" << m_com_port << "'"); return false; } // Bind: bindIO(theCOM); } // (re)connected to the sensor. Configure the laser: turnOn(); return true; } MRPT_END } void CHokuyoURG::initialize() { if (m_verbose) this->setMinLoggingLevel(mrpt::system::LVL_DEBUG); if (!ensureStreamIsOpen() || !turnOn()) { m_state = ssError; MRPT_LOG_ERROR("[Hokuyo] Error initializing HOKUYO scanner"); return; } } void CHokuyoURG::purgeBuffers() { if (!ensureStreamIsOpen()) return; if (m_ip_dir.empty()) { auto* COM = dynamic_cast(m_stream.get()); if (COM != nullptr) { COM->purgeBuffers(); } } else // Socket connection { auto* COM = dynamic_cast(m_stream.get()); size_t to_read = COM->getReadPendingBytes(); if (to_read) { void* buf = malloc(sizeof(uint8_t) * to_read); size_t nRead = m_stream->Read(buf, to_read); if (nRead != to_read) THROW_EXCEPTION( "Error in purge buffers: read and expected number of bytes " "are different."); free(buf); } } }