/* +------------------------------------------------------------------------+ | 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 | +------------------------------------------------------------------------+ */ // This file contains portions of code from sicklms200.cc from the Player/Stage // project. #include "hwdrivers-precomp.h" // Precompiled headers // #include #include #include #include #include #include IMPLEMENTS_GENERIC_SENSOR(CSickLaserSerial, mrpt::hwdrivers) #define RET_ERROR(msg) \ { \ cout << "[" << __CURRENT_FUNCTION_NAME__ << "] " << msg << endl; \ return false; \ } using namespace std; using namespace mrpt; using namespace mrpt::comms; using namespace mrpt::obs; using namespace mrpt::poses; using namespace mrpt::math; using namespace mrpt::hwdrivers; using namespace mrpt::system; int CSickLaserSerial::CRC16_GEN_POL = 0x8005; /*------------------------------------------------------------- CSickLaserSerial -------------------------------------------------------------*/ CSickLaserSerial::CSickLaserSerial() : m_com_port() { m_sensorLabel = "SICKLMS"; memset(m_received_frame_buffer, 0, sizeof(m_received_frame_buffer)); } /*------------------------------------------------------------- ~CSickLaserSerial -------------------------------------------------------------*/ CSickLaserSerial::~CSickLaserSerial() { if (m_stream) { try { if (!m_skip_laser_config) { LMS_endContinuousMode(); } } catch (...) { } } } /*------------------------------------------------------------- doProcessSimple -------------------------------------------------------------*/ void CSickLaserSerial::doProcessSimple( bool& outThereIsObservation, mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError) { outThereIsObservation = false; hardwareError = false; if (!tryToOpenComms()) { hardwareError = true; return; } vector ranges; unsigned char LMS_stat; bool is_mm_mode; m_state = ssWorking; // Wait for a scan: if (!waitContinuousSampleFrame(ranges, LMS_stat, is_mm_mode)) { if (!internal_notifyNoScanReceived()) { hardwareError = true; } return; } // Yes, we have a new scan: // ----------------------------------------------- // Extract the observation: // ----------------------------------------------- outObservation.timestamp = mrpt::Clock::now(); outObservation.sensorLabel = m_sensorLabel; // Set label // Extract the timestamp of the sensor: // And the scan ranges: outObservation.rightToLeft = true; outObservation.aperture = M_PIf; outObservation.maxRange = is_mm_mode ? 32.7 : 81.0; outObservation.stdError = 0.003f; outObservation.sensorPose = mrpt::poses::CPose3D(m_sensorPose); outObservation.resizeScan(ranges.size()); for (size_t i = 0; i < ranges.size(); i++) { outObservation.setScanRange(i, ranges[i]); outObservation.setScanRangeValidity( i, (outObservation.getScanRange(i) <= outObservation.maxRange)); } // Do filter: C2DRangeFinderAbstract::filterByExclusionAreas(outObservation); C2DRangeFinderAbstract::filterByExclusionAngles(outObservation); // Do show preview: C2DRangeFinderAbstract::processPreview(outObservation); outThereIsObservation = true; internal_notifyGoodScanNow(); } /*------------------------------------------------------------- loadConfig_sensorSpecific -------------------------------------------------------------*/ void CSickLaserSerial::loadConfig_sensorSpecific( const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection) { m_sensorPose = TPose3D( configSource.read_float(iniSection, "pose_x", 0), configSource.read_float(iniSection, "pose_y", 0), configSource.read_float(iniSection, "pose_z", 0), DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)), DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)), DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0))); m_mm_mode = configSource.read_bool(iniSection, "mm_mode", m_mm_mode); #ifdef _WIN32 m_com_port = configSource.read_string(iniSection, "COM_port_WIN", m_com_port, true); #else m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, true); #endif m_com_baudRate = configSource.read_int(iniSection, "COM_baudRate", m_com_baudRate); m_nTries_connect = configSource.read_int(iniSection, "nTries_connect", m_nTries_connect); m_scans_FOV = configSource.read_int(iniSection, "FOV", m_scans_FOV); m_scans_res = configSource.read_int(iniSection, "resolution", m_scans_res); m_skip_laser_config = configSource.read_bool(iniSection, "skip_laser_config", m_skip_laser_config); // Parent options: C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection); } /*------------------------------------------------------------- turnOn -------------------------------------------------------------*/ bool CSickLaserSerial::turnOn() { return true; } /*------------------------------------------------------------- turnOff -------------------------------------------------------------*/ bool CSickLaserSerial::turnOff() { return true; } /*------------------------------------------------------------- Tries to open the com port and setup all the LMS protocol. Returns true if OK or already open. -------------------------------------------------------------*/ bool CSickLaserSerial::tryToOpenComms(std::string* err_msg) { if (err_msg) *err_msg = ""; try { if (!m_stream) { ASSERT_(m_mySerialPort == nullptr); // There is no COMMS port open yet... if (!m_com_port.empty()) { // Create the port myself: m_mySerialPort = std::make_shared(); m_stream = std::shared_ptr(m_mySerialPort); } else throw std::logic_error( "ERROR: No serial port attached with bindIO, neither it " "set with 'setSerialPort'"); } // We assure now we have a stream... try to open it, if it's not done // yet. bool just_open = false; auto* COM = dynamic_cast(m_stream.get()); if (COM != nullptr) { if (!COM->isOpen()) { // Try to open it now: COM->setSerialPortName(m_com_port); COM->open(); // will raise an exception on error. // Set basic params: COM->setConfig(9600); COM->setTimeouts(100, 0, 10, 0, 50); just_open = true; } } // It seems the port was open and working so we are done here. if (!just_open) return true; // ================================================================== // Otherwise, it was just opened now, we must send the // ** initialization commands ** // and put the laser in continuous measuring mode: // ================================================================== if (!m_skip_laser_config) { if (!LMS_setupSerialComms()) RET_ERROR("error"); bool res; for (int nTry = 0; nTry < 4; nTry++) if (true == (res = LMS_sendMeasuringMode_cm_mm())) break; if (!res) return false; for (int nTry = 0; nTry < 4; nTry++) if (true == (res = LMS_startContinuousMode())) break; return res; } else { // Skip setup: return true; } } catch (const std::exception& e) { std::string s = "[CSickLaserSerial] Error trying to open SICK at port "; s += e.what(); if (err_msg) *err_msg = s; MRPT_LOG_ERROR(s); return false; } } /*------------------------------------------------------------- waitContinuousSampleFrame -------------------------------------------------------------*/ bool CSickLaserSerial::waitContinuousSampleFrame( vector& out_ranges_meters, unsigned char& LMS_status, bool& is_mm_mode) { auto* COM = dynamic_cast(m_stream.get()); ASSERTMSG_(COM != nullptr, "No I/O channel bound to this object"); size_t nRead, nBytesToRead; size_t nFrameBytes = 0; size_t lengthField; unsigned char buf[2000]; buf[2] = buf[3] = buf[4] = 0; while (nFrameBytes < (lengthField = (6 + (buf[2] | (buf[3] << 8))))) { if (lengthField > 800) { cout << "#"; nFrameBytes = 0; // No es cabecera de trama correcta buf[2] = buf[3] = 0; } if (nFrameBytes < 4) nBytesToRead = 1; else nBytesToRead = (lengthField)-nFrameBytes; try { nRead = COM->Read(buf + nFrameBytes, nBytesToRead); } catch (const std::exception& e) { // Disconnected? MRPT_LOG_ERROR_FMT( "[CSickLaserSerial::waitContinuousSampleFrame] Disconnecting " "due to comms error: %s\n", e.what()); // m_usbConnection->Close(); return false; } if (!nRead && !nFrameBytes) return false; if (nRead < nBytesToRead) std::this_thread::sleep_for(1ms); // Lectura OK: // Era la primera? if (nFrameBytes > 1 || (!nFrameBytes && buf[0] == 0x02) || (nFrameBytes == 1 && buf[1] == 0x80)) { nFrameBytes += nRead; } else { nFrameBytes = 0; // No es cabecera de trama correcta buf[2] = buf[3] = 0; // cerr << "."; //"[CSickLaserSerial] Skipping non-header..." << // endl; } } // Frame received // -------------------------------------------------------------------------- // | STX | ADDR | L1 | L2 | COM | INF1 | INF2 | DATA | STA | CRC1 | CRC2 // | // -------------------------------------------------------------------------- // Trama completa: // Checkear que el byte de comando es 0xB0: if (buf[4] != 0xB0) return false; // GET FRAME INFO int info = buf[5] | (buf[6] << 8); // Little Endian int n_points = info & 0x01FF; is_mm_mode = 0 != ((info & 0xC000) >> 14); // 0x00: cm 0x01: mm out_ranges_meters.resize(n_points); // Copiar rangos: short mask = is_mm_mode ? 0x7FFF : 0x1FFF; float meters_scale = is_mm_mode ? 0.001f : 0.01f; for (int i = 0; i < n_points; i++) out_ranges_meters[i] = ((buf[7 + i * 2] | (buf[8 + i * 2] << 8)) & mask) * meters_scale; // Status LMS_status = buf[lengthField - 3]; // CRC: uint16_t CRC = mrpt::system::compute_CRC16(buf, lengthField - 2, CRC16_GEN_POL); uint16_t CRC_packet = buf[lengthField - 2] | (buf[lengthField - 1] << 8); if (CRC_packet != CRC) { cerr << mrpt::format( "[CSickLaserSerial::waitContinuousSampleFrame] bad CRC " "len=%u nptns=%u: %i != %i", unsigned(lengthField), unsigned(n_points), CRC_packet, CRC) << endl; return false; // Bad CRC } // All OK return true; } /*------------------------------------------------------------- initialize -------------------------------------------------------------*/ void CSickLaserSerial::initialize() { string err_str; memset(m_received_frame_buffer, 0, sizeof(m_received_frame_buffer)); if (!tryToOpenComms(&err_str)) { cerr << err_str << endl; throw std::logic_error(err_str); } } /*----------------------------------------------------------------- Assures laser is connected and operating at 38400, in its case returns true. -----------------------------------------------------------------*/ bool CSickLaserSerial::LMS_setupSerialComms() { ASSERT_(m_com_baudRate == 9600 || m_com_baudRate == 38400 || m_com_baudRate == 500000); auto* COM = dynamic_cast(m_stream.get()); if (COM == nullptr) return true; int detected_rate = 0; for (size_t reps = 0; !detected_rate && reps < m_nTries_connect; reps++) { m_nTries_current = reps; int rates[] = {0, 9600, 38400, 500000}; // Try first the desired rate to speed up the process, just in case // the laser is already setup from a previous run: rates[0] = m_com_baudRate; detected_rate = 0; for (size_t i = 0; !detected_rate && i < sizeof(rates) / sizeof(rates[0]); i++) { // Are we already receiving at 500k? // ------------------------------------------------ COM->setConfig(rates[i]); if (m_verbose) printf( "[CSickLaserSerial] Testing if the scanner is " "set to %i bauds...\n", rates[i]); LMS_endContinuousMode(); // Stop continuous mode. std::this_thread::sleep_for(100ms); COM->purgeBuffers(); for (int nTry = 0; nTry < 4 && !detected_rate; nTry++) { COM->purgeBuffers(); // Ask for the laser status at the current rate: if (LMS_statusQuery()) { detected_rate = rates[i]; break; } std::this_thread::sleep_for(20ms); } // for tries // There is no link, or the baudrate is wrong... } // Try again in a while: if (!detected_rate && reps != (m_nTries_connect - 1)) std::this_thread::sleep_for(5000ms); } // Are we connected at the right rate? if (detected_rate == m_com_baudRate) return true; // Switch to the desired rate now if (!this->LMS_setupBaudrate(m_com_baudRate)) RET_ERROR("error"); // Check response is OK: if (!(m_received_frame_buffer[2] == 0x03 && m_received_frame_buffer[4] == 0xA0 && m_received_frame_buffer[6] == 0x10)) return false; COM->setConfig(m_com_baudRate); COM->purgeBuffers(); // Wait... std::this_thread::sleep_for(500ms); // And check comms at the new baud rate: return LMS_statusQuery(); } /*----------------------------------------------------------------- Query to LMS a baudrate change command. Returns true if response is read ok. -----------------------------------------------------------------*/ bool CSickLaserSerial::LMS_setupBaudrate(int baud) { ASSERT_(m_stream); if (m_verbose) printf("[CSickLaserSerial::LMS_setupBaudrate] rate=%i\n", baud); uint8_t cmd[4]; cmd[0] = 0x20; switch (baud) { case 9600: cmd[1] = 0x42; break; case 19200: cmd[1] = 0x41; break; case 38400: cmd[1] = 0x40; break; case 500000: cmd[1] = 0x48; break; default: THROW_EXCEPTION("Invalid baud rate value"); } uint16_t cmd_len = 2; if (!SendCommandToSICK(cmd, cmd_len)) return false; return LMS_waitIncomingFrame(500); } /*----------------------------------------------------------------- Query to LMS a status query. Returns true if response is read ok. -----------------------------------------------------------------*/ bool CSickLaserSerial::LMS_statusQuery() { ASSERT_(m_stream); uint8_t cmd[1]; cmd[0] = 0x31; uint16_t cmd_len = 1; if (!SendCommandToSICK(cmd, cmd_len)) return false; return LMS_waitIncomingFrame(500); } // Returns false if timeout bool CSickLaserSerial::LMS_waitACK(uint16_t timeout_ms) { auto* COM = dynamic_cast(m_stream.get()); ASSERT_(COM); uint8_t b = 0; CTicTac tictac; tictac.Tic(); do { if (COM->Read(&b, 1)) { // Byte rx: if (b == 0x06) return true; } } while (tictac.Tac() < timeout_ms * 1e-3); if (b == 0x15) RET_ERROR("NACK received.") else if (b != 0) RET_ERROR(format("Unexpected code received: 0x%02X", b)) else return false; // RET_ERROR("Timeout") } // Returns false if timeout bool CSickLaserSerial::LMS_waitIncomingFrame(uint16_t timeout) { auto* COM = dynamic_cast(m_stream.get()); ASSERT_(COM); uint8_t b; unsigned int nBytes = 0; CTicTac tictac; tictac.Tic(); const double maxTime = timeout * 1e-3; while (nBytes < 6 || (nBytes < (6U + m_received_frame_buffer[2] + (uint16_t)(m_received_frame_buffer[3] << 8)))) { if (COM->Read(&b, 1)) { // First byte must be STX: if (nBytes > 1 || (!nBytes && b == 0x02) || (nBytes == 1 && b == 0x80)) { // Store in frame: m_received_frame_buffer[nBytes] = b; nBytes++; if (m_verbose) { printf("[CSickLaserSerial::Receive] RX: %02X\n", b); } } } if (tictac.Tac() >= maxTime) return false; // Timeout } const uint16_t lengthField = m_received_frame_buffer[2] + (m_received_frame_buffer[3] << 8); // Check len: if (4U + lengthField + 2U != nBytes) { printf( "[CSickLaserSerial::LMS_waitIncomingFrame] Error: expected %u " "bytes, received %u\n", 4U + lengthField + 2U, nBytes); return false; } // Check CRC uint16_t CRC = mrpt::system::compute_CRC16(m_received_frame_buffer, 4 + lengthField, CRC16_GEN_POL); uint16_t CRC_packet = m_received_frame_buffer[4 + lengthField + 0] | (m_received_frame_buffer[4 + lengthField + 1] << 8); if (CRC_packet != CRC) { printf( "[CSickLaserSerial::LMS_waitIncomingFrame] Error in CRC: rx: " "0x%04X, computed: 0x%04X\n", CRC_packet, CRC); return false; } #if 0 printf("RX: "); for (unsigned int i=0;i(m_stream.get()); ASSERT_(COM); // ************************** // Send command: Switch to Installation mode // ************************** uint8_t cmd[128]; // = // {0x02,0x00,0x0A,0x00,0x20,0x00,0x53,0x49,0x43,0x4B,0x5F,0x4C,0x4D,0x53,0xBE,0xC5}; cmd[0] = 0x20; /* mode change command */ cmd[1] = 0x00; /* configuration mode */ cmd[2] = 0x53; // S - the password cmd[3] = 0x49; // I cmd[4] = 0x43; // C cmd[5] = 0x4B; // K cmd[6] = 0x5F; // _ cmd[7] = 0x4C; // L cmd[8] = 0x4D; // M cmd[9] = 0x53; // S uint16_t cmd_len = 10; if (!SendCommandToSICK(cmd, cmd_len)) RET_ERROR("Error waiting ACK to installation mode"); if (!LMS_waitIncomingFrame(500)) RET_ERROR("Error in response to installation mode"); // Check response if (!(m_received_frame_buffer[4] == 0xA0 && m_received_frame_buffer[5] == 0x00)) RET_ERROR("Wrong response to installation mode"); // ************************** // Request LMS Configuration // ************************** cmd[0] = 0x74; cmd_len = 1; if (!SendCommandToSICK(cmd, cmd_len)) RET_ERROR("No ACK to 0x74 (req. config)"); if (!LMS_waitIncomingFrame(500)) RET_ERROR("No answer to 0x74 (req. config)"); // 2. Check response if (m_received_frame_buffer[4] != 0xF4) RET_ERROR("No expected 0xF4 in response to 0x74 (req. config)"); // *********************************************************************** // Configure 1/2: Measuremente Range, Measurement Unit, Master/Slave Role // *********************************************************************** // 4.a Modify some values... // Measuring mode: Measurement range 32m in mm mode, or 80m+reflectance info // in cm mode. // See page 98 in LMS2xx_list_datagrams.pdf. m_received_frame_buffer[10] = this->m_mm_mode ? 0x06 : 0x00; m_received_frame_buffer[11] = this->m_mm_mode ? 0x01 : 0x00; // 4.2 Build the output command m_received_frame_buffer[1] = 0x00; // Address m_received_frame_buffer[2] = 0x23; // Length (low byte) m_received_frame_buffer[3] = 0x00; // Length (high byte) m_received_frame_buffer[4] = 0x77; // Configure command memcpy(cmd, m_received_frame_buffer + 4, 0x23); cmd_len = 0x23; // 4.4 Send to the LMS if (!SendCommandToSICK(cmd, cmd_len)) RET_ERROR("No ACK for config command (0x77)"); if (!LMS_waitIncomingFrame(600)) RET_ERROR("No answer for config command (0x77)"); if (!(m_received_frame_buffer[4] == 0xF7 && m_received_frame_buffer[5] == 0x01)) RET_ERROR("Wrong answer for config command (0x77)"); // ************************** // Switch to Monitoring mode // ************************** cmd[0] = 0x20; cmd[1] = 0x25; cmd_len = 2; if (!SendCommandToSICK(cmd, cmd_len)) RET_ERROR("No ACK for set monitoring mode"); if (!LMS_waitIncomingFrame(500)) RET_ERROR("No answer for set monitoring mode"); if (!(m_received_frame_buffer[4] == 0xA0 && m_received_frame_buffer[5] == 0x00)) RET_ERROR("Wrong answer for set monitoring mode"); // All ok. return true; } /*----------------------------------------------------------------- Start continuous mode measurements. Returns true if response is read ok. -----------------------------------------------------------------*/ bool CSickLaserSerial::LMS_startContinuousMode() { ASSERT_(m_scans_FOV == 100 || m_scans_FOV == 180); ASSERT_(m_scans_res == 25 || m_scans_res == 50 || m_scans_res == 100); auto* COM = dynamic_cast(m_stream.get()); ASSERT_(COM); uint8_t cmd[40]; // Config angle/resolution cmd[0] = 0x3B; cmd[1] = m_scans_FOV; cmd[2] = 0x00; cmd[3] = m_scans_res; // 25,50 or 100 - 1/100th of deg cmd[4] = 0x00; uint16_t cmd_len = 5; if (!SendCommandToSICK(cmd, cmd_len)) RET_ERROR("Error waiting ack for change angle/resolution"); if (!LMS_waitIncomingFrame(500)) RET_ERROR("Error waiting answer for change angle/resolution"); // Start continuous mode: cmd[0] = 0x20; cmd[1] = 0x24; cmd_len = 2; if (!SendCommandToSICK(cmd, cmd_len)) RET_ERROR("Error waiting ack for start scanning"); if (!LMS_waitIncomingFrame(500)) RET_ERROR("Error waiting answer for start scanning"); return true; } bool CSickLaserSerial::LMS_endContinuousMode() { auto* COM = dynamic_cast(m_stream.get()); ASSERT_(COM); uint8_t cmd[40]; // End continuous mode: cmd[0] = 0x20; cmd[1] = 0x25; uint16_t cmd_len = 2; if (!SendCommandToSICK(cmd, cmd_len)) return false; return LMS_waitIncomingFrame(50); } bool CSickLaserSerial::SendCommandToSICK(const uint8_t* cmd, const uint16_t cmd_len) { uint8_t cmd_full[1024]; ASSERT_(sizeof(cmd_full) > cmd_len + 4U + 2U); auto* COM = dynamic_cast(m_stream.get()); ASSERT_(COM); // Create header cmd_full[0] = 0x02; // STX cmd_full[1] = 0; // ADDR cmd_full[2] = cmd_len & 0xFF; cmd_full[3] = cmd_len >> 8; memcpy(cmd_full + 4, cmd, cmd_len); const uint16_t crc = mrpt::system::compute_CRC16(cmd_full, 4 + cmd_len, CRC16_GEN_POL); cmd_full[4 + cmd_len + 0] = crc & 0xFF; cmd_full[4 + cmd_len + 1] = crc >> 8; const size_t toWrite = 4 + cmd_len + 2; if (m_verbose) { printf("[CSickLaserSerial::SendCommandToSICK] TX: "); for (unsigned int i = 0; i < toWrite; i++) printf("%02X ", cmd_full[i]); printf("\n"); } const int NTRIES = 3; for (int k = 0; k < NTRIES; k++) { if (toWrite != COM->Write(cmd_full, toWrite)) { cout << "[CSickLaserSerial::SendCommandToSICK] Error writing data " "to serial port." << endl; return false; } std::this_thread::sleep_for(15ms); if (LMS_waitACK(50)) return true; std::this_thread::sleep_for(10ms); } return false; }