CSickLaserUSB.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"  // Precompiled headers

#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/os.h>
#include <mrpt/system/crc.h>
#include <mrpt/hwdrivers/CSickLaserUSB.h>

#ifdef _WIN32
#include <windows.h>
#endif

IMPLEMENTS_GENERIC_SENSOR(CSickLaserUSB, mrpt::hwdrivers)

using namespace std;
using namespace mrpt;
using namespace mrpt::comms;
using namespace mrpt::obs;
using namespace mrpt::hwdrivers;
using namespace mrpt::poses;

/*-------------------------------------------------------------
                        CSickLaserUSB
-------------------------------------------------------------*/
CSickLaserUSB::CSickLaserUSB()
    : m_usbConnection(nullptr), m_serialNumber("LASER001"), m_timeStartUI(0)
{
    MRPT_START
    m_sensorLabel = "SICKLMS";
    m_usbConnection = new CInterfaceFTDI();
    MRPT_END
}

/*-------------------------------------------------------------
                        ~CSickLaserUSB
-------------------------------------------------------------*/
CSickLaserUSB::~CSickLaserUSB()
{
    delete m_usbConnection;
    m_usbConnection = nullptr;
}

/*-------------------------------------------------------------
                        doProcessSimple
-------------------------------------------------------------*/
void CSickLaserUSB::doProcessSimple(
    bool& outThereIsObservation,
    mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError)
{
    outThereIsObservation = false;
    hardwareError = false;

    if (!checkControllerIsConnected())
    {
        hardwareError = true;
        return;
    }

    vector<float> ranges;
    unsigned char LMS_stat;
    uint32_t board_timestamp;
    bool is_mm_mode;

    m_state = ssWorking;

    // Wait for a scan:
    if (!waitContinuousSampleFrame(
            ranges, LMS_stat, board_timestamp, is_mm_mode))
        return;

    // Yes, we have a new scan:

    // -----------------------------------------------
    //   Extract the observation:
    // -----------------------------------------------
    uint32_t AtUI = 0;
    if (m_timeStartUI == 0)
    {
        m_timeStartUI = board_timestamp;
        m_timeStartTT = mrpt::system::now();
    }
    else
        AtUI = board_timestamp - m_timeStartUI;

    /* Board time is ms, -50ms aprox delay for scan sending from the scanner*/
    auto AtDO = std::chrono::milliseconds(AtUI - 50);
    outObservation.timestamp = m_timeStartTT + AtDO;

    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 = m_sensorPose;

    outObservation.resizeScan(ranges.size());

    for (size_t i = 0; i < ranges.size(); i++)
    {
        outObservation.setScanRange(i, ranges[i]);
        outObservation.setScanRangeValidity(
            i, (ranges[i] <= outObservation.maxRange));
    }

    // Do filter:
    C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
    C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
    // Do show preview:
    C2DRangeFinderAbstract::processPreview(outObservation);

    outThereIsObservation = true;
}

/*-------------------------------------------------------------
                        loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CSickLaserUSB::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& configSource,
    const std::string& iniSection)
{
    m_serialNumber = configSource.read_string(
        iniSection, "SICKUSB_serialNumber", m_serialNumber);
    m_sensorPose = CPose3D(
        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)));
    // Parent options:
    C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
}

/*-------------------------------------------------------------
                        turnOn
-------------------------------------------------------------*/
bool CSickLaserUSB::turnOn() { return true; }
/*-------------------------------------------------------------
                        turnOff
-------------------------------------------------------------*/
bool CSickLaserUSB::turnOff() { return true; }
/*-------------------------------------------------------------
                    checkControllerIsConnected
-------------------------------------------------------------*/
bool CSickLaserUSB::checkControllerIsConnected()
{
    // If device is already open, thats ok:
    if (m_usbConnection->isOpen()) return true;

    // If it isn't, try to open it now:
    try
    {
        m_usbConnection->OpenBySerialNumber(m_serialNumber);
        m_usbConnection->ResetDevice();
        std::this_thread::sleep_for(10ms);
        m_usbConnection->SetTimeouts(10, 20);  // read, write, in milliseconds
        std::this_thread::sleep_for(10ms);
        m_usbConnection->SetLatencyTimer(1);  // 1ms, the minimum
        std::this_thread::sleep_for(10ms);

        MRPT_LOG_INFO_FMT(
            "[CSickLaserUSB] USB DEVICE S/N:'%s' OPEN SUCCESSFULLY!!!\n",
            m_serialNumber.c_str());
        return true;
    }
    catch (std::exception& e)
    {
        MRPT_LOG_ERROR_FMT(
            "[CSickLaserUSB] ERROR TRYING TO OPEN USB DEVICE S/N:'%s'\n%s",
            m_serialNumber.c_str(), e.what());
        return false;
    }
}

/*-------------------------------------------------------------
                    waitContinuousSampleFrame
-------------------------------------------------------------*/
bool CSickLaserUSB::waitContinuousSampleFrame(
    vector<float>& out_ranges_meters, unsigned char& LMS_status,
    uint32_t& out_board_timestamp, bool& is_mm_mode)
{
    size_t nRead, nBytesToRead;
    size_t nFrameBytes = 0;
    size_t lenghtField;
    unsigned char buf[2000];
    buf[2] = buf[3] = 0;

    while (nFrameBytes < (lenghtField = (6 + (buf[2] | (buf[3] << 8)))) +
                             5 /* for 32bit timestamp + end-flag */)
    {
        if (lenghtField > 800)
        {
            cout << "#";
            nFrameBytes = 0;  // No es cabecera de trama correcta
            buf[2] = buf[3] = 0;
        }

        if (nFrameBytes < 4)
            nBytesToRead = 1;
        else
            nBytesToRead =
                (5 /* for 32bit timestamp + end-flag */ + lenghtField) -
                nFrameBytes;

        try
        {
            nRead = m_usbConnection->ReadSync(buf + nFrameBytes, nBytesToRead);
        }
        catch (std::exception& e)
        {
            // Disconnected?
            MRPT_LOG_ERROR_FMT(
                "[CSickLaserUSB::waitContinuousSampleFrame] Disconnecting due "
                "to comms error: %s\n",
                e.what());
            m_usbConnection->Close();
            m_timeStartUI = 0;
            return false;
        }

        if (nRead == 0 && nFrameBytes == 0) return false;

        if (nRead > 0)
        {
            // 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;
            }
        }
    }

    // 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[lenghtField - 3];

    // End frame:
    if (buf[nFrameBytes - 1] != 0x55)
    {
// cerr << format("[CSickLaserUSB::waitContinuousSampleFrame] bad end flag") <<
// endl;
#ifdef _WIN32
        OutputDebugStringA(
            "[CSickLaserUSB::waitContinuousSampleFrame] bad end flag\n");
#endif
        return false;  // Bad CRC
    }

    // CRC:
    const uint16_t CRC = mrpt::system::compute_CRC16(buf, lenghtField - 2);
    const uint16_t CRC_packet =
        buf[lenghtField - 2] | (buf[lenghtField - 1] << 8);
    if (CRC_packet != CRC)
    {
        const string s = format(
            "[CSickLaserUSB::waitContinuousSampleFrame] bad CRC len=%u "
            "nptns=%u: %i != %i\n",
            unsigned(lenghtField), unsigned(n_points), CRC_packet, CRC);
        cerr << s;
#ifdef _WIN32
        OutputDebugStringA(s.c_str());
#endif
        return false;  // Bad CRC
    }

    // Get USB board timestamp:
    out_board_timestamp = (uint32_t(buf[nFrameBytes - 5]) << 24) |
                          (uint32_t(buf[nFrameBytes - 4]) << 16) |
                          (uint32_t(buf[nFrameBytes - 3]) << 8) |
                          uint32_t(buf[nFrameBytes - 2]);

    // All OK
    return true;
}