SickScanAdapter.cpp

/**
* \file
* \brief Laser Scanner communication (TCP Helper Class)
* Copyright (C) 2013, Osnabrück University
* Copyright (C) 2017, Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2017, SICK AG, Waldkirch
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
*     * Redistributions of source code must retain the above copyright
*       notice, this list of conditions and the following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright
*       notice, this list of conditions and the following disclaimer in the
*       documentation and/or other materials provided with the distribution.
*     * Neither the name of Osnabrück University nor the names of its
*       contributors may be used to endorse or promote products derived from
*       this software without specific prior written permission.
*     * Neither the name of SICK AG nor the names of its
*       contributors may be used to endorse or promote products derived from
*       this software without specific prior written permission
*     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
*       contributors may be used to endorse or promote products derived from
*       this software without specific prior written permission
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*  Last modified: 12th Dec 2017
*
*      Authors:
*         Michael Lehning <michael.lehning@lehning.de>
*         Jochen Sprickerhof <jochen@sprickerhof.de>
*         Martin Günther <mguenthe@uos.de>
*
* Based on the TiM communication example by SICK AG.
*
*/

#ifdef _MSC_VER
#pragma warning(disable: 4996)
#pragma warning(disable: 4267)
#pragma warning(disable: 4101)   // C4101: "e" : Unreferenzierte lokale Variable
#define _WIN32_WINNT 0x0501

#endif

#include "SickScanAdapter.h"
#include <sick_scan/tcp/colaa.hpp>
#include <sick_scan/tcp/colab.hpp>
#include <sick_scan/sick_generic_radar.h>
#include <sick_scan/helper/angle_compensator.h>
#include <sick_scan/sick_scan_config_internal.h>
#include <sick_scan/sick_generic_imu.h>

#ifdef ROSSIMU
#include <sick_scan/rosconsole_simu.hpp>
#endif

#include <sick_scan/binScanf.hpp>
#include <sick_scan/RadarScan.h>
#include <sick_scan/Encoder.h>

#include <boost/asio.hpp>
#include <boost/lambda/lambda.hpp>
#include <algorithm>
#include <iterator>
#include <boost/lexical_cast.hpp>
#include <vector>

#ifndef rad2deg
#define rad2deg(x) ((x) / M_PI * 180.0)
#endif
#define deg2rad_const (0.017453292519943295769236907684886f)


std::vector<unsigned char> exampleData(65536);
std::vector<unsigned char> receivedData(65536);
//static long receivedDataLen = 0;

static int getDiagnosticErrorCode()
{
#ifdef _MSC_VER
#undef ERROR
    return (2);
#else
    return (diagnostic_msgs::DiagnosticStatus::ERROR);
#endif
}

namespace armarx
{
    bool emulateReply(UINT8* requestData, int requestLen, std::vector<unsigned char>* replyVector)
    {
        std::string request;
        std::string reply;
        std::vector<std::string> keyWordList;
        std::vector<std::string> answerList;

        std::string scannerType = "???";
        ros::NodeHandle nhPriv;

        enum
        {
            SIMU_RADAR = 0, SIMU_MRS_1XXX = 1, SIMU_NUM
        };
        // XXX
        keyWordList.push_back("sMN SetAccessMode");
        answerList.push_back("sAN SetAccessMode 1");

        keyWordList.push_back("sWN EIHstCola");
        answerList.push_back("sWA EIHstCola");

        keyWordList.push_back("sRN FirmwareVersion");
        answerList.push_back("sRA FirmwareVersion 8 1.0.0.0R");

        keyWordList.push_back("sRN OrdNum");
        answerList.push_back("sRA OrdNum 7 1234567");

        keyWordList.push_back("sWN TransmitTargets 1");
        answerList.push_back("sWA TransmitTargets");

        keyWordList.push_back("sWN TransmitObjects 1");
        answerList.push_back("sWA TransmitObjects");

        keyWordList.push_back("sWN TCTrackingMode 0");
        answerList.push_back("sWA TCTrackingMode");

        keyWordList.push_back("sRN SCdevicestate");
        answerList.push_back("sRA SCdevicestate 1");

        keyWordList.push_back("sRN DItype");
        answerList.push_back("sRA DItype D RMS3xx-xxxxxx");

        keyWordList.push_back("sRN ODoprh");
        answerList.push_back("sRA ODoprh 451");

        keyWordList.push_back("sMN mSCloadappdef");
        answerList.push_back("sAN mSCloadappdef");


        keyWordList.push_back("sRN SerialNumber");
        answerList.push_back("sRA SerialNumber 8 18020073");

        keyWordList.push_back("sMN Run");
        answerList.push_back("sAN Run 1s");

        keyWordList.push_back("sRN ODpwrc");
        answerList.push_back("sRA ODpwrc 20");

        keyWordList.push_back("sRN LocationName");
        answerList.push_back("sRA LocationName B not defined");

        keyWordList.push_back("sEN LMDradardata 1");
        answerList.push_back("sEA LMDradardata 1");

        for (int i = 0; i < requestLen; i++)
        {
            request += (char) requestData[i];
        }
        for (size_t i = 0; i < keyWordList.size(); i++)
        {
            if (request.find(keyWordList[i]) != std::string::npos)
            {
                reply = (char) 0x02;
                reply += answerList[i];
                reply += (char) 0x03;
            }
        }

        replyVector->clear();
        for (size_t i = 0; i < reply.length(); i++)
        {
            replyVector->push_back((unsigned char) reply[i]);
        }

        return (true);
    }


    SickScanAdapter::SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit,
                                     sick_scan::SickGenericParser* parser, char cola_dialect_id)
        :
        sick_scan::SickScanCommon(parser),
        socket_(io_service_),
        deadline_(io_service_),
        parser_ptr(parser),
        hostname_(hostname),
        port_(port),
        timelimit_(timelimit)
    {

        setEmulSensor(false);
        if ((cola_dialect_id == 'a') || (cola_dialect_id == 'A'))
        {
            this->setProtocolType(CoLa_A);
        }

        if ((cola_dialect_id == 'b') || (cola_dialect_id == 'B'))
        {
            this->setProtocolType(CoLa_B);
        }

        assert(this->getProtocolType() != CoLa_Unknown);

        m_numberOfBytesInReceiveBuffer = 0;
        m_alreadyReceivedBytes = 0;
        this->setReplyMode(0);
        // io_service_.setReadCallbackFunction(boost::bind(&SopasDevice::readCallbackFunction, this, _1, _2));

        // Set up the deadline actor to implement timeouts.
        // Based on blocking TCP example on:
        // http://www.boost.org/doc/libs/1_46_0/doc/html/boost_asio/example/timeouts/blocking_tcp_client.cpp
        deadline_.expires_at(boost::posix_time::pos_infin);
        checkDeadline();

    }

    SickScanAdapter::~SickScanAdapter()
    {
        // stop_scanner();
        close_device();
    }

    using boost::asio::ip::tcp;
    using boost::lambda::var;
    using boost::lambda::_1;


    /*!
    \brief parsing datagram and publishing ros messages
    \return error code
    */
    int SickScanAdapter::loopOnce(LaserScan& scanData)
    {
        static int cnt = 0;

        unsigned char receiveBuffer[65536];
        int actual_length = 0;
        static unsigned int iteration_count = 0;
        //Always ASCII
        bool useBinaryProtocol = false;

        ros::Time recvTimeStamp = ros::Time::now();  // timestamp incoming package, will be overwritten by get_datagram
        // tcp packet
        ros::Time recvTimeStampPush = ros::Time::now();  // timestamp

        int packetsInLoop = 0;
        const int maxNumAllowedPacketsToProcess = 25; // maximum number of packets, which will be processed in this loop.
        int numPacketsProcessed = 0; // count number of processed datagrams
        do
        {

            int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, useBinaryProtocol, &packetsInLoop);
            numPacketsProcessed++;

            ros::Duration dur = recvTimeStampPush - recvTimeStamp;

            if (result != 0)
            {
                ARMARX_ERROR_S << "Read Error when getting datagram: " << result;
                return sick_scan::ExitError; // return failure to exit node
            }
            if (actual_length <= 0)
            {
                return sick_scan::ExitSuccess;
            } // return success to continue looping

            // ----- if requested, skip frames
            if (iteration_count++ % (config_.skip + 1) != 0)
            {
                return sick_scan::ExitSuccess;
            }
            //msg.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
            //datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
            char* buffer_pos = (char*) receiveBuffer;
            char* dstart, *dend;
            bool dataToProcess = true;
            std::vector<float> vang_vec;
            vang_vec.clear();
            dstart = NULL;
            dend = NULL;

            while (dataToProcess)
            {
                size_t dlength;
                int success = -1;
                int numEchos = 1;
                int echoMask = 0;
                LaserScannerInfo scanInfo;

                // Always Parsing Ascii-Encoding of datagram
                dstart = strchr(buffer_pos, 0x02);
                if (dstart != NULL)
                {
                    dend = strchr(dstart + 1, 0x03);
                }
                if ((dstart != NULL) && (dend != NULL))
                {
                    dataToProcess = true; // continue parsing
                    dlength = dend - dstart;
                    *dend = '\0';
                    dstart++;
                }
                else
                {
                    dataToProcess = false;
                    break;
                }

                // HEADER of data followed by DIST1 ... DIST2 ... DIST3 .... RSSI1 ... RSSI2.... RSSI3...
                // <frameid>_<sign>00500_DIST[1|2|3]
                //success = parser_ptr->parse_datagram(dstart, dlength, config_, msg, numEchos, echoMask);
                success = parseDatagram(dstart, dlength, config_, scanData, scanInfo, numEchos, echoMask, true);

                if (success == sick_scan::ExitSuccess)
                {
                    if (numEchos > 5)
                    {
                        ARMARX_ERROR_S << "Too many echos";
                    }
                    //                            cloud_.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
                    //                            cloud_.header.frame_id = config_.frame_id;
                    //                            cloud_.header.seq = 0;
                    //                            cloud_.height = numTmpLayer * numValidEchos; // due to multi echo multiplied by num. of layers
                    //                            cloud_.width = msg.ranges.size();
                    //                            cloud_.is_bigendian = false;
                    //                            cloud_.is_dense = true;
                    //                            cloud_.point_step = numChannels * sizeof(float);
                    //                            cloud_.row_step = cloud_.point_step * cloud_.width;
                    //                            cloud_.fields.resize(numChannels);
                    //                            for (int i = 0; i < numChannels; i++)
                    //                            {
                    //                                std::string channelId[] = {"x", "y", "z", "intensity"};
                    //                                cloud_.fields[i].name = channelId[i];
                    //                                cloud_.fields[i].offset = i * sizeof(float);
                    //                                cloud_.fields[i].count = 1;
                    //                                cloud_.fields[i].datatype = sensor_msgs::PointField::FLOAT32;
                    //                            }
                    cnt++;
                    if (cnt == 25)
                    {
                        ARMARX_INFO_S << "10th measurement: (" << scanData[10].angle << ", " << scanData[10].distance << ")";
                        cnt = 0;
                    }
                }
                // Start Point
                if (dend != NULL)
                {
                    buffer_pos = dend + 1;
                }
            } // end of while loop
        }
        while ((packetsInLoop > 0) && (numPacketsProcessed < maxNumAllowedPacketsToProcess));
        return sick_scan::ExitSuccess; // return success to continue looping
    }

    //Adapted from sick_generic_parser
    int SickScanAdapter::parseDatagram(char* datagram, size_t datagram_length, sick_scan::SickScanConfig& config,
                                       LaserScan& scanData, LaserScannerInfo& scanInfo, int& numEchos, int& echoMask, bool updateScannerInfo)
    {
        int HEADER_FIELDS = 32;
        char* cur_field;
        size_t count;

        // Reserve sufficient space
        std::vector<char*> fields;
        fields.reserve(datagram_length / 2);

        // ----- only for debug output
        std::vector<char> datagram_copy_vec;
        datagram_copy_vec.resize(datagram_length + 1); // to avoid using malloc. destructor frees allocated mem.
        char* datagram_copy = &(datagram_copy_vec[0]);

        ARMARX_INFO_S << "parse_datagram(): Copying";
        strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
        datagram_copy[datagram_length] = 0;

        count = 0;
        cur_field = strtok(datagram, " ");

        while (cur_field != NULL)
        {
            fields.push_back(cur_field);
            cur_field = strtok(NULL, " ");
        }
        count = fields.size();

        ARMARX_INFO_S << "parse_datagram(): Validate header";
        // Validate header. Total number of tokens is highly unreliable as this may
        // change when you change the scanning range or the device name using SOPAS ET
        // tool. The header remains stable, however.
        if ((int) count < HEADER_FIELDS)
        {
            ARMARX_ERROR_S << "received less fields than minimum fields (actual: " << count
                           << ", minimum: " << HEADER_FIELDS << "), ignoring scan\n"
                           << "are you using the correct node? (124 --> sick_tim310_1130000m01, > 33 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)";
            return sick_scan::ExitError;
        }

        if (strcmp(fields[15], "0"))
        {
            ARMARX_ERROR_S << "Field 15 of received data is not equal to 0. Unexpected data, ignoring scan";
            return sick_scan::ExitError;
        }
        if (strcmp(fields[20], "DIST1"))
        {
            ARMARX_ERROR_S << "Field 20 of received data is not equal to DIST1i. Unexpected data, ignoring scan";
            return sick_scan::ExitError;
        }

        // More in depth checks: check data length and RSSI availability
        // 25: Number of data (<= 10F)
        unsigned short int number_of_data = 0;
        sscanf(fields[25], "%hx", &number_of_data);

        //int numOfExpectedShots = parser_ptr->basicParams[scannerIdx].getNumberOfShots();
        //if (number_of_data < 1 || number_of_data > numOfExpectedShots)
        //{
        //    ARMARX_ERROR_S << "Data length is outside acceptable range 1-%d (%d). Ignoring scan";//, numOfExpectedShots, number_of_data);
        //    return sick_scan::ExitError;
        //}
        if ((int) count < HEADER_FIELDS + number_of_data)
        {
            ARMARX_ERROR_S << "Less fields than expected for %d data points (%zu). Ignoring scan"; //, number_of_data, count);
            return sick_scan::ExitError;
        }

        // Calculate offset of field that contains indicator of whether or not RSSI data is included
        size_t rssi_idx = 26 + number_of_data;
        bool rssi = false;
        if (strcmp(fields[rssi_idx], "RSSI1") == 0)
        {
            rssi = true;
        }
        unsigned short int number_of_rssi_data = 0;
        if (rssi)
        {
            sscanf(fields[rssi_idx + 5], "%hx", &number_of_rssi_data);

            // Number of RSSI data should be equal to number of data
            if (number_of_rssi_data != number_of_data)
            {
                //      ARMARX_ERROR("Number of RSSI data is lower than number of range data (%d vs %d", number_of_data,
                //               number_of_rssi_data);
                return sick_scan::ExitError;
            }

            // Check if the total length is still appropriate.
            // RSSI data size = number of RSSI readings + 6 fields describing the data
            if ((int) count < HEADER_FIELDS + number_of_data + number_of_rssi_data + 6)
            {
                //      ARMARX_ERROR("Less fields than expected for %d data points (%zu). Ignoring scan", number_of_data, count);
                return sick_scan::ExitError;
            }

            if (strcmp(fields[rssi_idx], "RSSI1"))
            {
                //      ARMARX_ERROR("Field %zu of received data is not equal to RSSI1 (%s). Unexpected data, ignoring scan", rssi_idx + 1,
                //               fields[rssi_idx + 1]);
            }
        }

        ARMARX_INFO_S << "parse_datagram(): Checks complete";

        // ----- read fields into msg
        //msg.header.frame_id = config.frame_id;
        // evtl. debug stream benutzen
        // ROS_DEBUG("publishing with frame_id %s", config.frame_id.c_str());

        ros::Time start_time = ros::Time::now(); // will be adjusted in the end

        // <STX> (\x02)
        // 0: Type of command (SN)
        // 1: Command (LMDscandata)
        // 2: Firmware version number (1)
        // 3: Device number (1)
        // 4: Serial number (eg. B96518)
        // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
        // 7: Telegram counter (eg. 99)
        // 8: Scan counter (eg. 9A)
        // 9: Time since startup (eg. 13C8E59)
        // 10: Time of transmission (eg. 13C9CBE)
        // 11 + 12: Input status (0 0)
        // 13 + 14: Output status (8 0)
        // 15: Reserved Byte A (0)
        // 16: Scanning Frequency (5DC)
        unsigned short scanning_freq = -1;
        sscanf(fields[16], "%hx", &scanning_freq);
        //  msg.scan_time = 1.0 / (scanning_freq / 100.0);

        // 17: Measurement Frequency (36)
        unsigned short measurement_freq = -1;
        sscanf(fields[17], "%hx", &measurement_freq);
        //  msg.time_increment = 1.0 / (measurement_freq * 100.0);

        // 18: Number of encoders (0)
        // 19: Number of 16 bit channels (1)
        // 20: Measured data contents (DIST1)
        // 21: Scaling factor (3F800000)
        // ignored for now (is always 1.0):
        // 22: Scaling offset (00000000) -- always 0
        // 23: Starting angle (FFF92230)
        if (updateScannerInfo)
        {
            uint starting_angle = (uint) - 1;
            sscanf(fields[23], "%x", &starting_angle);
            scanInfo.minAngle = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
            // ROS_DEBUG("starting_angle: %d, angle_min: %f", starting_angle, msg.angle_min);

            // 24: Angular step width (2710)
            unsigned short angular_step_width = -1;
            sscanf(fields[24], "%hx", &angular_step_width);
            scanInfo.stepSize = (angular_step_width / 10000.0) / 180.0 * M_PI;
            scanInfo.maxAngle = scanInfo.minAngle + (number_of_data - 1) * scanInfo.stepSize;
        }

        // 25: Number of data (<= 10F)
        // This is already determined above in number_of_data
        //int index_min = 0;

        ARMARX_INFO_S << "parse_datagram(): Parsing";
        //param_ptr->checkForDistAndRSSI(fields, number_of_data, distNum, rssiNum, msg.ranges, msg.intensities, echoMask);

        int distNum = 0;
        int rssiNum = 0;
        int baseOffset = 20;

        echoMask = 0;
        // More in depth checks: check data length and RSSI availability
        // 25: Number of data (<= 10F)
        unsigned short int curr_number_of_data = 0;
        if (strstr(fields[baseOffset], "DIST") != fields[baseOffset]) // First initial check
        {
            //      ARMARX_ERROR("Field 20 of received data does not start with DIST (is: %s). Unexpected data, ignoring scan",
            //           fields[20]);
            return sick_scan::ExitError;
        }

        //Read either intensity or distance data, regarding what type is given in the datagram
        std::vector<float> distVal, intensityVal;
        for (int offset = 20; offset < (int) fields.size();)
        {
            bool distFnd = false;
            bool rssiFnd = false;
            if (strlen(fields[offset]) == 5)
            {
                if (strstr(fields[offset], "DIST") == fields[offset])
                {
                    distFnd = true;
                    distNum++;
                    int distId = -1;
                    if (1 == sscanf(fields[offset], "DIST%d", &distId))
                    {
                        echoMask |= (1 << (distId - 1)); // set bit regarding to id
                    }
                }
                else if (strstr(fields[offset], "RSSI") == fields[offset])
                {
                    rssiNum++;
                    rssiFnd = true;
                }
            }
            if (rssiFnd || distFnd)
            {
                offset += 5;
                if (offset >= (int) fields.size())
                {
                    ARMARX_ERROR_S << "Missing RSSI or DIST data";
                    return sick_scan::ExitError;
                }
                curr_number_of_data = 0;
                sscanf(fields[offset], "%hx", &curr_number_of_data);
                if (curr_number_of_data != number_of_data)
                {
                    ARMARX_ERROR_S << "number of dist or rssi values mismatching.";
                    return sick_scan::ExitError;
                }
                offset++;
                // Here is the first value
                for (int i = 0; i < curr_number_of_data; i++)
                {
                    if (distFnd)
                    {
                        unsigned short iRange;
                        sscanf(fields[offset + i], "%hx", &iRange);
                        float range = iRange / 1000.0;
                        distVal.push_back(range);
                    }
                    else
                    {
                        unsigned short iRSSI;
                        sscanf(fields[offset + i], "%hx", &iRSSI);
                        intensityVal.push_back((float) iRSSI);
                    }
                }
                offset += number_of_data;
            }
            else
            {
                offset++; // necessary????
            }
        }
        numEchos = distNum;

        ARMARX_INFO_S << "numDistanceVals: " << distNum << ", numIntensityVals: " << rssiNum;
        //TODO: Write ScanSteps with intensity
        scanData.reserve(distVal.size());
        for (int i = 0; i < (int) distVal.size(); i++)
        {
            LaserScanStep step;
            step.angle = i * scanInfo.stepSize;
            step.distance = distVal[i];
            scanData.push_back(step);
        }

        // 26 + n: RSSI data included
        // IF RSSI not included:
        //   26 + n + 1 .. 26 + n + 3 = unknown (but seems to be [0, 1, B] always)
        //   26 + n + 4 .. count - 4 = device label
        //   count - 3 .. count - 1 = unknown (but seems to be 0 always)
        //   <ETX> (\x03)

        //  msg.range_min = override_range_min_;
        //  msg.range_max = override_range_max_;

        ARMARX_INFO_S << "parse_datagram(): Time check";
        return sick_scan::ExitSuccess;
    }


    void SickScanAdapter::disconnectFunction()
    {

    }

    void SickScanAdapter::disconnectFunctionS(void* obj)
    {
        if (obj != NULL)
        {
            ((SickScanAdapter*)(obj))->disconnectFunction();
        }
    }

    void SickScanAdapter::readCallbackFunctionS(void* obj, UINT8* buffer, UINT32& numOfBytes)
    {
        ((SickScanAdapter*) obj)->readCallbackFunction(buffer, numOfBytes);
    }


    void SickScanAdapter::setReplyMode(int _mode)
    {
        m_replyMode = _mode;
    }

    int SickScanAdapter::getReplyMode()
    {
        return (m_replyMode);
    }

    /*!
    \brief Set emulation flag (using emulation instead of "real" scanner - currently implemented for radar
    \param _emulFlag: Flag to switch emulation on or off
    \return
    */
    void SickScanAdapter::setEmulSensor(bool _emulFlag)
    {
        m_emulSensor = _emulFlag;
    }

    /*!
    \brief get emulation flag (using emulation instead of "real" scanner - currently implemented for radar
    \param
    \return bool: Flag to switch emulation on or off
    */
    bool SickScanAdapter::getEmulSensor()
    {
        return (m_emulSensor);
    }

    //
    // Look for 23-frame (STX/ETX) in receive buffer.
    // Move frame to start of buffer
    //
    // Return: 0 : No (complete) frame found
    //        >0 : Frame length
    //
    SopasEventMessage SickScanAdapter::findFrameInReceiveBuffer()
    {
        UINT32 frameLen = 0;
        UINT32 i;

        // Depends on protocol...
        if (getProtocolType() == CoLa_A)
        {
            //
            // COLA-A
            //
            // Must start with STX (0x02)
            if (m_receiveBuffer[0] != 0x02)
            {
                // Look for starting STX (0x02)
                for (i = 1; i < m_numberOfBytesInReceiveBuffer; i++)
                {
                    if (m_receiveBuffer[i] == 0x02)
                    {
                        break;
                    }
                }

                // Found beginning of frame?
                if (i >= m_numberOfBytesInReceiveBuffer)
                {
                    // No start found, everything can be discarded
                    m_numberOfBytesInReceiveBuffer = 0; // Invalidate buffer
                    return SopasEventMessage(); // No frame found
                }

                // Move frame start to index 0
                UINT32 newLen = m_numberOfBytesInReceiveBuffer - i;
                memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[i]), newLen);
                m_numberOfBytesInReceiveBuffer = newLen;
            }

            // Look for ending ETX (0x03)
            for (i = 1; i < m_numberOfBytesInReceiveBuffer; i++)
            {
                if (m_receiveBuffer[i] == 0x03)
                {
                    break;
                }
            }

            // Found end?
            if (i >= m_numberOfBytesInReceiveBuffer)
            {
                // No end marker found, so it's not a complete frame (yet)
                return SopasEventMessage(); // No frame found
            }

            // Calculate frame length in byte
            frameLen = i + 1;

            return SopasEventMessage(m_receiveBuffer, CoLa_A, frameLen);
        }
        else if (getProtocolType() == CoLa_B)
        {
            UINT32 magicWord;
            UINT32 payloadlength;

            if (m_numberOfBytesInReceiveBuffer < 4)
            {
                return SopasEventMessage();
            }
            UINT16 pos = 0;
            magicWord = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
            if (magicWord != 0x02020202)
            {
                // Look for starting STX (0x02020202)
                for (i = 1; i <= m_numberOfBytesInReceiveBuffer - 4; i++)
                {
                    pos = i; // this is needed, as the position value is updated by getIntegerFromBuffer
                    magicWord = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
                    if (magicWord == 0x02020202)
                    {
                        // found magic word
                        break;
                    }
                }

                // Found beginning of frame?
                if (i > m_numberOfBytesInReceiveBuffer - 4)
                {
                    // No start found, everything can be discarded
                    m_numberOfBytesInReceiveBuffer = 0; // Invalidate buffer
                    return SopasEventMessage(); // No frame found
                }
                else
                {
                    // Move frame start to index
                    UINT32 bytesToMove = m_numberOfBytesInReceiveBuffer - i;
                    memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[i]), bytesToMove); // payload+magic+length+s+checksum
                    m_numberOfBytesInReceiveBuffer = bytesToMove;
                }
            }

            // Pruefe Laenge des Pufferinhalts
            if (m_numberOfBytesInReceiveBuffer < 9)
            {
                // Es sind nicht genug Daten fuer einen Frame
                printInfoMessage("SickScanCommonNw::findFrameInReceiveBuffer: Frame cannot be decoded yet, only " +
                                 ::toString(m_numberOfBytesInReceiveBuffer) + " bytes in the buffer.", m_beVerbose);
                return SopasEventMessage();
            }

            // Read length of payload
            pos = 4;
            payloadlength = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
            printInfoMessage(
                "SickScanCommonNw::findFrameInReceiveBuffer: Decoded payload length is " + ::toString(payloadlength) +
                " bytes.", m_beVerbose);

            // Ist die Datenlaenge plausibel und wuede in den Puffer passen?
            if (payloadlength > (sizeof(m_receiveBuffer) - 9))
            {
                // magic word + length + checksum = 9
                printWarning(
                    "SickScanCommonNw::findFrameInReceiveBuffer: Frame too big for receive buffer. Frame discarded with length:"
                    + ::toString(payloadlength) + ".");
                m_numberOfBytesInReceiveBuffer = 0;
                return SopasEventMessage();
            }
            if ((payloadlength + 9) > m_numberOfBytesInReceiveBuffer)
            {
                // magic word + length + s + checksum = 10
                printInfoMessage(
                    "SickScanCommonNw::findFrameInReceiveBuffer: Frame not complete yet. Waiting for the rest of it (" +
                    ::toString(payloadlength + 9 - m_numberOfBytesInReceiveBuffer) + " bytes missing).", m_beVerbose);
                return SopasEventMessage(); // frame not complete
            }

            // Calculate the total frame length in bytes: Len = Frame (9 bytes) + Payload
            frameLen = payloadlength + 9;

            //
            // test checksum of payload
            //
            UINT8 temp = 0;
            UINT8 temp_xor = 0;
            UINT8 checkSum;

            // Read original checksum
            pos = frameLen - 1;
            checkSum = colab::getIntegerFromBuffer<UINT8>(m_receiveBuffer, pos);

            // Erzeuge die Pruefsumme zum Vergleich
            for (UINT16 i = 8; i < (frameLen - 1); i++)
            {
                pos = i;
                temp = colab::getIntegerFromBuffer<UINT8>(m_receiveBuffer, pos);
                temp_xor = temp_xor ^ temp;
            }

            // Vergleiche die Pruefsummen
            if (temp_xor != checkSum)
            {
                printWarning("SickScanCommonNw::findFrameInReceiveBuffer: Wrong checksum, Frame discarded.");
                m_numberOfBytesInReceiveBuffer = 0;
                return SopasEventMessage();
            }

            return SopasEventMessage(m_receiveBuffer, CoLa_B, frameLen);
        }

        // Return empty frame
        return SopasEventMessage();
    }


    /**
    * Read callback. Diese Funktion wird aufgerufen, sobald Daten auf der Schnittstelle
    * hereingekommen sind.
    */

    void SickScanAdapter::processFrame(ros::Time timeStamp, SopasEventMessage& frame)
    {

        if (getProtocolType() == CoLa_A)
        {
            printInfoMessage(
                "SickScanCommonNw::processFrame: Calling processFrame_CoLa_A() with " + ::toString(frame.size()) + " bytes.",
                m_beVerbose);
            // processFrame_CoLa_A(frame);
        }
        else if (getProtocolType() == CoLa_B)
        {
            printInfoMessage(
                "SickScanCommonNw::processFrame: Calling processFrame_CoLa_B() with " + ::toString(frame.size()) + " bytes.",
                m_beVerbose);
            // processFrame_CoLa_B(frame);
        }

        // Push frame to recvQueue

        DatagramWithTimeStamp dataGramWidthTimeStamp(timeStamp, std::vector<unsigned char>(frame.getRawData(),
                frame.getRawData() +
                frame.size()));
        // recvQueue.push(std::vector<unsigned char>(frame.getRawData(), frame.getRawData() + frame.size()));
        recvQueue.push(dataGramWidthTimeStamp);
    }

    void SickScanAdapter::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
    {
        ros::Time rcvTimeStamp = ros::Time::now(); // stamp received datagram
        bool beVerboseHere = false;
        printInfoMessage(
            "SickScanCommonNw::readCallbackFunction(): Called with " + toString(numOfBytes) + " available bytes.",
            beVerboseHere);

        ScopedLock lock(&m_receiveDataMutex); // Mutex for access to the input buffer
        UINT32 remainingSpace = sizeof(m_receiveBuffer) - m_numberOfBytesInReceiveBuffer;
        UINT32 bytesToBeTransferred = numOfBytes;
        if (remainingSpace < numOfBytes)
        {
            bytesToBeTransferred = remainingSpace;
            // printWarning("SickScanCommonNw::readCallbackFunction(): Input buffer space is to small, transferring only " +
            //              ::toString(bytesToBeTransferred) + " of " + ::toString(numOfBytes) + " bytes.");
        }
        else
        {
            // printInfoMessage("SickScanCommonNw::readCallbackFunction(): Transferring " + ::toString(bytesToBeTransferred) +
            //                   " bytes from TCP to input buffer.", beVerboseHere);
        }

        if (bytesToBeTransferred > 0)
        {
            // Data can be transferred into our input buffer
            memcpy(&(m_receiveBuffer[m_numberOfBytesInReceiveBuffer]), buffer, bytesToBeTransferred);
            m_numberOfBytesInReceiveBuffer += bytesToBeTransferred;

            UINT32 size = 0;

            while (1)
            {
                // Now work on the input buffer until all received datasets are processed
                SopasEventMessage frame = findFrameInReceiveBuffer();

                size = frame.size();
                if (size == 0)
                {
                    // Framesize = 0: There is no valid frame in the buffer. The buffer is either empty or the frame
                    // is incomplete, so leave the loop
                    printInfoMessage("SickScanCommonNw::readCallbackFunction(): No complete frame in input buffer, we are done.",
                                     beVerboseHere);

                    // Leave the loop
                    break;
                }
                else
                {
                    // A frame was found in the buffer, so process it now.
                    printInfoMessage(
                        "SickScanCommonNw::readCallbackFunction(): Processing a frame of length " + ::toString(frame.size()) +
                        " bytes.", beVerboseHere);
                    processFrame(rcvTimeStamp, frame);
                    UINT32 bytesToMove = m_numberOfBytesInReceiveBuffer - size;
                    memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[size]), bytesToMove); // payload+magic+length+s+checksum
                    m_numberOfBytesInReceiveBuffer = bytesToMove;

                }
            }
        }
        else
        {
            // There was input data from the TCP interface, but our input buffer was unable to hold a single byte.
            // Either we have not read data from our buffer for a long time, or something has gone wrong. To re-sync,
            // we clear the input buffer here.
            m_numberOfBytesInReceiveBuffer = 0;
        }
    }


    int SickScanAdapter::init_device()
    {
        int portInt;
        sscanf(port_.c_str(), "%d", &portInt);
        m_nw.init(hostname_, portInt, disconnectFunctionS, (void*) this);
        m_nw.setReadCallbackFunction(readCallbackFunctionS, (void*) this);
        if (this->getEmulSensor())
        {
            ARMARX_INFO_S << "Sensor emulation is switched on - network traffic is switched off.";
        }
        else
        {
            m_nw.connect();
        }
        return sick_scan::ExitSuccess;
    }

    int SickScanAdapter::close_device()
    {
        ARMARX_ERROR_S << "Disconnecting TCP-Connection.";
        m_nw.disconnect();
        return 0;
    }


    bool SickScanAdapter::stopScanData()
    {
        stop_scanner();
        return (true);
    }

    void SickScanAdapter::handleRead(boost::system::error_code error, size_t bytes_transfered)
    {
        ec_ = error;
        bytes_transfered_ += bytes_transfered;
    }


    void SickScanAdapter::checkDeadline()
    {
        if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now())
        {
            // The reason the function is called is that the deadline expired. Close
            // the socket to return all IO operations and reset the deadline
            socket_.close();
            deadline_.expires_at(boost::posix_time::pos_infin);
        }

        // Nothing bad happened, go back to sleep
        deadline_.async_wait(boost::bind(&SickScanAdapter::checkDeadline, this));
    }


    int SickScanAdapter::numberOfDatagramInInputFifo()
    {
        int numVal = this->recvQueue.getNumberOfEntriesInQueue();
        return (numVal);
    }

    int SickScanAdapter::readWithTimeout(size_t timeout_ms, char* buffer, int buffer_size, int* bytes_read,
                                         bool* exception_occured, bool isBinary)
    {
        // Set up the deadline to the proper timeout, error and delimiters
        deadline_.expires_from_now(boost::posix_time::milliseconds(timeout_ms));

        ec_ = boost::asio::error::would_block;
        bytes_transfered_ = 0;

        // Polling - should be changed to condition variable in the future
        int waitingTimeInMs = 1; // try to lookup for new incoming packages
        size_t i;
        for (i = 0; i < timeout_ms; i += waitingTimeInMs)
        {
            if (false == this->recvQueue.isQueueEmpty())
            {
                break;
            }
            boost::this_thread::sleep(boost::posix_time::milliseconds(waitingTimeInMs));
        }
        if (i >= timeout_ms)
        {
            ROS_ERROR("no answer received after %zu ms. Maybe sopas mode is wrong.\n", timeout_ms);
            return (sick_scan::ExitError);
        }
        DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();

        *bytes_read = datagramWithTimeStamp.datagram.size();
        memcpy(buffer, &(datagramWithTimeStamp.datagram[0]), datagramWithTimeStamp.datagram.size());
        return (sick_scan::ExitSuccess);
    }

    /**
     * Send a SOPAS command to the device and print out the response to the console.
     */
    int SickScanAdapter::sendSOPASCommand(const char* request, std::vector<unsigned char>* reply, int cmdLen)
    {
#if 0
        if (!socket_.is_open())
        {
            ROS_ERROR("sendSOPASCommand: socket not open");
            diagnostics_.broadcast(getDiagnosticErrorCode(), "sendSOPASCommand: socket not open.");
            return sick_scan::ExitError;
        }
#endif
        int sLen = 0;
        int preambelCnt = 0;
        bool cmdIsBinary = false;

        if (request != NULL)
        {
            sLen = cmdLen;
            preambelCnt = 0; // count 0x02 bytes to decide between ascii and binary command
            if (sLen >= 4)
            {
                for (int i = 0; i < 4; i++)
                {
                    if (request[i] == 0x02)
                    {
                        preambelCnt++;
                    }
                }
            }

            if (preambelCnt < 4)
            {
                cmdIsBinary = false;
            }
            else
            {
                cmdIsBinary = true;
            }
            int msgLen = 0;
            if (cmdIsBinary == false)
            {
                msgLen = strlen(request);
            }
            else
            {
                int dataLen = 0;
                for (int i = 4; i < 8; i++)
                {
                    dataLen |= ((unsigned char) request[i] << (7 - i) * 8);
                }
                msgLen = 8 + dataLen + 1; // 8 Msg. Header + Packet +
            }
#if 1
            if (getEmulSensor())
            {
                emulateReply((UINT8*) request, msgLen, reply);
            }
            else
            {
                bool debugBinCmd = false;
                if (debugBinCmd)
                {
                    printf("=== START HEX DUMP ===\n");
                    for (int i = 0; i < msgLen; i++)
                    {
                        unsigned char* ptr = (UINT8*) request;
                        printf("%02x ", ptr[i]);
                    }
                    printf("\n=== END HEX DUMP ===\n");
                }
                m_nw.sendCommandBuffer((UINT8*) request, msgLen);
            }
#else

            /*
             * Write a SOPAS variable read request to the device.
             */
            try
            {
                boost::asio::write(socket_, boost::asio::buffer(request, msgLen));
            }
            catch (boost::system::system_error& e)
            {
                ROS_ERROR("write error for command: %s", request);
                diagnostics_.broadcast(getDiagnosticErrorCode(), "Write error for sendSOPASCommand.");
                return sick_scan::ExitError;
            }
#endif
        }

        // Set timeout in 5 seconds
        const int BUF_SIZE = 65536;
        char buffer[BUF_SIZE];
        int bytes_read;
        // !!!
        if (getEmulSensor())
        {

        }
        else
        {
            if (readWithTimeout(getReadTimeOutInMs(), buffer, BUF_SIZE, &bytes_read, 0, cmdIsBinary) == sick_scan::ExitError)
            {
                ARMARX_INFO_S << "sendSOPASCommand: no full reply available for read after (ms): " << getReadTimeOutInMs();
                return sick_scan::ExitError;
            }


            if (reply)
            {
                reply->resize(bytes_read);

                std::copy(buffer, buffer + bytes_read, &(*reply)[0]);
            }
        }
        return sick_scan::ExitSuccess;
    }


    int SickScanAdapter::get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize,
                                      int* actual_length,
                                      bool isBinaryProtocol, int* numberOfRemainingFifoEntries)
    {
        if (NULL != numberOfRemainingFifoEntries)
        {
            *numberOfRemainingFifoEntries = 0;
        }
        this->setReplyMode(1);

        if (this->getEmulSensor())
        {
#ifndef ROSSIMU
            // boost::this_thread::sleep(boost::posix_time::milliseconds(waitingTimeInMs));
            ros::Time timeStamp = ros::Time::now();
            uint32_t nanoSec = timeStamp.nsec;
            double waitTime10Hz = 10.0 * (double) nanoSec / 1E9;  // 10th of sec. [0..10[

            uint32_t waitTime = (int) waitTime10Hz; // round down

            double waitTimeUntilNextTime10Hz = 1 / 10.0 * (1.0 - (waitTime10Hz - waitTime));

            ros::Duration(waitTimeUntilNextTime10Hz).sleep();
            SickScanRadar radar(this);
            radar.setEmulation(true);
            radar.simulateAsciiDatagram(receiveBuffer, actual_length);
            recvTimeStamp = ros::Time::now();
#endif
        }
        else
        {
            const int maxWaitInMs = getReadTimeOutInMs();
            std::vector<unsigned char> dataBuffer;
#if 1 // prepared for reconnect
            bool retVal = this->recvQueue.waitForIncomingObject(maxWaitInMs);
            if (retVal == false)
            {
                ARMARX_ERROR_S << "Timeout during waiting for new datagram";
                return sick_scan::ExitError;
            }
            else
            {
                // Look into receiving queue for new Datagrams
                //
                //
                DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
                if (NULL != numberOfRemainingFifoEntries)
                {
                    *numberOfRemainingFifoEntries = this->recvQueue.getNumberOfEntriesInQueue();
                }
                recvTimeStamp = datagramWithTimeStamp.timeStamp;
                dataBuffer = datagramWithTimeStamp.datagram;

            }
#endif
            // dataBuffer = this->recvQueue.pop();
            long size = dataBuffer.size();
            memcpy(receiveBuffer, &(dataBuffer[0]), size);
            *actual_length = size;
        }

#if 0
        static int cnt = 0;
        char szFileName[255];
        sprintf(szFileName, "/tmp/dg%06d.bin", cnt++);

        FILE* fout;

        fout = fopen(szFileName, "wb");
        if (fout != NULL)
        {
            fwrite(receiveBuffer, size, 1, fout);
            fclose(fout);
        }
#endif
        return sick_scan::ExitSuccess;

        if (!socket_.is_open())
        {
            ROS_ERROR("get_datagram: socket not open");
            diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: socket not open.");
            return sick_scan::ExitError;
        }

        /*
         * Write a SOPAS variable read request to the device.
         */
        std::vector<unsigned char> reply;

        // Wait at most 5000ms for a new scan
        size_t timeout = 30000;
        bool exception_occured = false;

        char* buffer = reinterpret_cast<char*>(receiveBuffer);

        if (readWithTimeout(timeout, buffer, bufferSize, actual_length, &exception_occured, isBinaryProtocol) !=
            sick_scan::ExitSuccess)
        {
            ROS_ERROR_THROTTLE(1.0, "get_datagram: no data available for read after %zu ms", timeout);
            diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: no data available for read after timeout.");

            // Attempt to reconnect when the connection was terminated
            if (!socket_.is_open())
            {
#ifdef _MSC_VER
                Sleep(1000);
#else
                sleep(1);
#endif
                ARMARX_INFO << "Failure - attempting to reconnect";
                return init();
            }

            return exception_occured ? sick_scan::ExitError : sick_scan::ExitSuccess;    // keep on trying
        }

        return sick_scan::ExitSuccess;
    }

}