sick_generic_radar.cpp

Go to the documentation of this file.

#ifdef _MSC_VER
#define _WIN32_WINNT 0x0501
#pragma warning(disable: 4996)
#pragma warning(disable: 4267)
#endif

#ifndef _MSC_VER


#endif

#include <sick_scan/sick_scan_common_tcp.h>
#include <sick_scan/sick_generic_parser.h>
#include <sick_scan/sick_generic_radar.h>
#include <sick_scan/RadarScan.h> // generated by msg-generator

#ifdef _MSC_VER
#include "sick_scan/rosconsole_simu.hpp"
#endif
#define _USE_MATH_DEFINES

#include <math.h>
#include "string"
#include <stdio.h>
#include <stdlib.h>

namespace sick_scan
{


/* Null, because instance will be initialized on demand. */
  SickScanRadarSingleton *SickScanRadarSingleton::instance = 0;

  SickScanRadarSingleton *SickScanRadarSingleton::getInstance()
  {
    if (instance == 0)
    {
      instance = new SickScanRadarSingleton();
    }

    return instance;
  }

  SickScanRadarSingleton::SickScanRadarSingleton()
  {
    // just for debugging, but very helpful for the start
    cloud_radar_rawtarget_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud_radar_rawtarget", 100);
    cloud_radar_track_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud_radar_track", 100);

    radarScan_pub_ = nh_.advertise<sick_scan::RadarScan>("radar", 100);

  }


  int16_t getShortValue(std::string str)
  {
    int val = 0;
    if (1 == sscanf(str.c_str(), "%x", &val))
    {

    }
    else
    {
      ROS_WARN("Problems parsing %s\n", str.c_str());
    }
    return (val);

  }

  int getHexValue(std::string str)
  {
    int val = 0;
    if (1 == sscanf(str.c_str(), "%x", &val))
    {

    }
    else
    {
      ROS_WARN("Problems parsing %s\n", str.c_str());
    }
    return (val);

  }


  float convertScaledIntValue(int value, float scale, float offset)
  {
    float val = 0;
    val = (float) (value * scale + offset);
    return (val);
  }

  float getFloatValue(std::string str)
  {
    float tmpVal = 0.0;
    unsigned char *ptr;
    ptr = (unsigned char *) (&tmpVal);
    int strLen = str.length();
    if (strLen < 8)
    {
    }
    else
    {
      for (int i = 0; i < 4; i++)
      {
        std::string dummyStr = "";
        dummyStr += str[i * 2];
        dummyStr += str[i * 2 + 1];
        int val = getHexValue(dummyStr);
        unsigned char ch = (0xFF & val);
        ptr[3 - i] = ch;
      }
    }
    return (tmpVal);
  }

  void SickScanRadarSingleton::setEmulation(bool _emul)
  {
    emul = _emul;
  }

  bool SickScanRadarSingleton::getEmulation(void)
  {
    return (emul);
  }

  int SickScanRadarSingleton::parseAsciiDatagram(char *datagram, size_t datagram_length,
                                                 sick_scan::RadarScan *msgPtr,
                                                 std::vector<SickScanRadarObject> &objectList,
                                                 std::vector<SickScanRadarRawTarget> &rawTargetList)
  {
    int exitCode = ExitSuccess;
    ros::NodeHandle tmpParam("~");
    bool dumpData = false;
    int verboseLevel = 0;
    tmpParam.getParam("verboseLevel", verboseLevel);

    // !!!!!
    // verboseLevel = 1;
    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]);

    if (verboseLevel > 0)
    {
      ROS_WARN("Verbose LEVEL activated. Only for DEBUG.");
    }

    if (verboseLevel > 0)
    {
      static int cnt = 0;
      char szDumpFileName[255] = {0};
      char szDir[255] = {0};
#ifdef _MSC_VER
      strcpy(szDir, "C:\\temp\\");
#else
      strcpy(szDir, "/tmp/");
#endif
      sprintf(szDumpFileName, "%stmp%06d.bin", szDir, cnt);
      FILE *ftmp;
      ftmp = fopen(szDumpFileName, "wb");
      if (ftmp != NULL)
      {
        fwrite(datagram, datagram_length, 1, ftmp);
        fclose(ftmp);
      }
    }

    strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
    datagram_copy[datagram_length] = 0;

    // ----- tokenize
    count = 0;
    cur_field = strtok(datagram, " ");

    while (cur_field != NULL)
    {
      fields.push_back(cur_field);
      //std::cout << cur_field << std::endl;
      cur_field = strtok(NULL, " ");
    }

    //std::cout << fields[27] << std::endl;

    count = fields.size();


    if (verboseLevel > 0)
    {
      static int cnt = 0;
      char szDumpFileName[255] = {0};
      char szDir[255] = {0};
#ifdef _MSC_VER
      strcpy(szDir, "C:\\temp\\");
#else
      strcpy(szDir, "/tmp/");
#endif
      sprintf(szDumpFileName, "%stmp%06d.txt", szDir, cnt);
      ROS_WARN("Verbose LEVEL activated. Only for DEBUG.");
      FILE *ftmp;
      ftmp = fopen(szDumpFileName, "w");
      if (ftmp != NULL)
      {
        int i;
        for (i = 0; i < count; i++)
        {
          fprintf(ftmp, "%3d: %s\n", i, fields[i]);
        }
        fclose(ftmp);
      }
      cnt++;
    }


    enum PREHEADER_TOKEN_SEQ
    {
      PREHEADER_TOKEN_SSN,          // 0: sSN
      PREHEADER_TOKEN_LMDRADARDATA, // 1: LMDradardata
      PREHEADER_TOKEN_UI_VERSION_NO,
      PREHEADER_TOKEN_UI_IDENT,
      PREHEADER_TOKEN_UDI_SERIAL_NO,
      PREHEADER_TOKEN_XB_STATE_0,
      PREHEADER_TOKEN_XB_STATE_1,
      PREHEADER_TOKEN_TELEGRAM_COUNT, // 7
      PREHEADER_TOKEN_CYCLE_COUNT,
      PREHEADER_TOKEN_SYSTEM_COUNT_SCAN,
      PREHEADER_TOKEN_SYSTEM_COUNT_TRANSMIT,
      PREHEADER_TOKEN_XB_INPUTS_0,
      PREHEADER_TOKEN_XB_INPUTS_1,
      PREHEADER_TOKEN_XB_OUTPUTS_0, // 13
      PREHEADER_TOKEN_XB_OUTPUTS_1, // 14
      PREHEADER_TOKEN_CYCLE_DURATION, // 15
      PREHEADER_TOKEN_NOISE_LEVEL,    // 16
      PREHEADER_NUM_ENCODER_BLOCKS, // 17
      PREHADERR_TOKEN_FIX_NUM
    };
/*

        StatusBlock
        ===========
        7: BCC            uiTelegramCount
        8: DC0C           uiCycleCount (or uiScanCount???)
        9: 730E9D16       udiSystemCountScan
        10: 730EA06D       udiSystemCountTransmit
        11: 0              xbInputs[0]
        12: 0              xbInputs[1]
        13: 0              xbOutputs[0]
        14: 0              xbOutputs[1]

        MeasurementParam1Block
        ======================
        15: 0              MeasurementParam1Block.uiCycleDuration
        16: 0              MeasurementParam1Block.uiNoiseLevel

        aEncoderBlock
        =============
        17: 1              Number of aEncoderBlocks


        18: 0              aEncoderBlock[0].udiEncoderPos
        19: 0              aEncoderBlock[0].iEncoderSpeed


        PREHADERR_TOKEN_FIX_NUM};  // Number of fix token (excluding aEncoderBlock)
*/
    /*
     * To read a single unsigned byte, use the %hhx modifier.
     * %hx is for an unsigned short,
     * %x is for an unsigned int,
     * %lx is for an unsigned long,
     * and %llx is for an `unsigned long long.
     *
     */
    for (int i = 0; i < PREHADERR_TOKEN_FIX_NUM; i++)
    {
      UINT16 uiValue = 0x00;
      UINT32 udiValue = 0x00;
      unsigned long int uliDummy;
      uliDummy = strtoul(fields[i], NULL, 16);
      switch (i)
      {
        case PREHEADER_TOKEN_UI_VERSION_NO:
          msgPtr->radarPreHeader.uiVersionNo = (UINT16) (uliDummy & 0xFFFF);
          break;
        case PREHEADER_TOKEN_UI_IDENT:
          msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.uiIdent = (UINT16) (uliDummy & 0xFFFF);
          break;
        case PREHEADER_TOKEN_UDI_SERIAL_NO:
          msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.udiSerialNo = (UINT32) (uliDummy & 0xFFFFFFFF);
          break;
        case PREHEADER_TOKEN_XB_STATE_0:
          for (int j = 0; j < 3; j++)
          {
            bool flag = false;
            if (0 != (uliDummy & (1 << j)))
            {
              flag = true;
            }
            switch (j)
            {
              case 0:
                msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.bDeviceError = flag;
                break;
              case 1:
                msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.bContaminationWarning = flag;
                break;
              case 2:
                msgPtr->radarPreHeader.radarPreHeaderDeviceBlock.bContaminationError = flag;
                break;
              default:
                ROS_WARN("Flag parsing for this PREHEADER-Flag not implemented");
            }
          }
          break;
        case PREHEADER_TOKEN_XB_STATE_1:
          // reserved - do nothing
          break;
        case PREHEADER_TOKEN_TELEGRAM_COUNT: // 7
          msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiTelegramCount = (UINT16) (uliDummy & 0xFFFF);
          break;
        case PREHEADER_TOKEN_CYCLE_COUNT:
          sscanf(fields[i], "%hu", &uiValue);
          msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiCycleCount = (UINT16) (uliDummy & 0xFFFF);
          break;
        case PREHEADER_TOKEN_SYSTEM_COUNT_SCAN:
          msgPtr->radarPreHeader.radarPreHeaderStatusBlock.udiSystemCountScan = (UINT32) (uliDummy & 0xFFFFFFFF);
          break;
        case PREHEADER_TOKEN_SYSTEM_COUNT_TRANSMIT:
          msgPtr->radarPreHeader.radarPreHeaderStatusBlock.udiSystemCountTransmit = (UINT32) (uliDummy & 0xFFFFFFFF);
          break;
        case PREHEADER_TOKEN_XB_INPUTS_0:
          msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiInputs = (UINT8) (uliDummy & 0xFF);;
          msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiInputs <<= 8;
          break;
        case PREHEADER_TOKEN_XB_INPUTS_1:
          msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiInputs |= (UINT8) (uliDummy & 0xFF);;
          break;
        case PREHEADER_TOKEN_XB_OUTPUTS_0:
          msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiOutputs = (UINT8) (uliDummy & 0xFF);;
          msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiOutputs <<= 8;
          break;
        case PREHEADER_TOKEN_XB_OUTPUTS_1:
          msgPtr->radarPreHeader.radarPreHeaderStatusBlock.uiOutputs |= (UINT8) (uliDummy & 0xFF);;
          break;
        case PREHEADER_TOKEN_CYCLE_DURATION:
          msgPtr->radarPreHeader.radarPreHeaderMeasurementParam1Block.uiCycleDuration = (UINT16) (uliDummy & 0xFFFF);
          break;
        case PREHEADER_TOKEN_NOISE_LEVEL:
          msgPtr->radarPreHeader.radarPreHeaderMeasurementParam1Block.uiNoiseLevel = (UINT16) (uliDummy & 0xFFFF);
          break;
        case PREHEADER_NUM_ENCODER_BLOCKS:
        {
          UINT16 u16NumberOfBlock = (UINT16) (uliDummy & 0xFFFF);

          if (u16NumberOfBlock > 0)
          {
            msgPtr->radarPreHeader.radarPreHeaderArrayEncoderBlock.resize(u16NumberOfBlock);

            for (int j = 0; j < u16NumberOfBlock; j++)
            {
              INT16 iEncoderSpeed;
              int rowIndex = PREHEADER_NUM_ENCODER_BLOCKS + j * 2 + 1;
              udiValue = strtoul(fields[rowIndex], NULL, 16);
              msgPtr->radarPreHeader.radarPreHeaderArrayEncoderBlock[j].udiEncoderPos = udiValue;
              udiValue = strtoul(fields[rowIndex + 1], NULL, 16);
              iEncoderSpeed = (int) udiValue;
              msgPtr->radarPreHeader.radarPreHeaderArrayEncoderBlock[j].iEncoderSpeed = iEncoderSpeed;

            }
          }
        }
          break;
      }
    }
/*
                                MeasurementData
                                ===============
                                2: 1             MeasurementData.uiVersionNo  : Version Information for this while structureValue
                                Value   Range: 0 ... 65535
                                };

*/
    std::vector<std::string> keyWordList;
#define DIST1_KEYWORD "DIST1"
#define AZMT1_KEYWORD "AZMT1"
#define VRAD1_KEYWORD "VRAD1"
#define AMPL1_KEYWORD "AMPL1"
#define MODE1_KEYWORD "MODE1"

#define P3DX1_KEYWORD "P3DX1"
#define P3DY1_KEYWORD "P3DY1"
#define V3DX1_KEYWORD "V3DX1"
#define V3DY1_KEYWORD "V3DY1"
#define OBJLEN_KEYWORD "OBLE1"
#define OBJID_KEYWORD "OBID1"

    const int RAWTARGET_LOOP = 0;
    const int OBJECT_LOOP = 1;

    // std::vector<SickScanRadarRawTarget> rawTargets;
    // std::vector<SickScanRadarObject> objectList;

    for (int iLoop = 0; iLoop < 2; iLoop++)
    {
      keyWordList.clear();
      switch (iLoop)
      {
        case RAWTARGET_LOOP:
          keyWordList.push_back(DIST1_KEYWORD);
          keyWordList.push_back(AZMT1_KEYWORD);
          keyWordList.push_back(VRAD1_KEYWORD);
          keyWordList.push_back(AMPL1_KEYWORD);
          keyWordList.push_back(MODE1_KEYWORD);
          break;
        case OBJECT_LOOP:
          keyWordList.push_back(P3DX1_KEYWORD);
          keyWordList.push_back(P3DY1_KEYWORD);
          keyWordList.push_back(V3DX1_KEYWORD);
          keyWordList.push_back(V3DY1_KEYWORD);
          keyWordList.push_back(OBJLEN_KEYWORD);
          keyWordList.push_back(OBJID_KEYWORD);
          break;
      }
      std::vector<int> keyWordPos;
      std::vector<float> keyWordScale;
      std::vector<float> keyWordScaleOffset;
      keyWordPos.resize(keyWordList.size());
      keyWordScale.resize(keyWordList.size());
      keyWordScaleOffset.resize(keyWordList.size());
      for (int i = 0; i < keyWordPos.size(); i++)
      {
        keyWordPos[i] = -1;
      }
      int numKeyWords = keyWordPos.size();
      for (int i = 0; i < fields.size(); i++)
      {
        for (int j = 0; j < keyWordList.size(); j++)
        {
          if (strcmp(fields[i], keyWordList[j].c_str()) == 0)
          {
            keyWordPos[j] = i;
          }
        }
      }

      bool entriesNumOk = true;
      int entriesNum = 0;
      if (keyWordPos[0] == -1)
      {
        entriesNumOk = false;
      }
      else
      {

        entriesNum = getHexValue(fields[keyWordPos[0] + 3]);
        for (int i = 0; i < numKeyWords; i++)
        {
          if (keyWordPos[i] == -1)
          {
            entriesNumOk = false;
            ROS_WARN("Missing keyword %s but first keyword found.\n", keyWordList[i].c_str());
            entriesNumOk = false;
          }
          else
          {
            int entriesNumTmp = getHexValue(fields[keyWordPos[i] + 3]);
            if (entriesNumTmp != entriesNum)
            {
              ROS_WARN("Number of items for keyword %s differs from number of items for %s\n.",
                       keyWordList[i].c_str(), keyWordList[0].c_str());
              entriesNumOk = false;
            }
          }
        }
      }

      if (true == entriesNumOk)
      {


        for (int i = 0; i < numKeyWords; i++)
        {
          int scaleLineIdx = keyWordPos[i] + 1;
          int scaleOffsetLineIdx = keyWordPos[i] + 2;
          std::string token = fields[scaleLineIdx];
          keyWordScale[i] = getFloatValue(token);
          token = fields[scaleOffsetLineIdx];
          keyWordScaleOffset[i] = getFloatValue(token);
          // printf("Keyword: %-6s %8.3lf %8.3lf\n", keyWordList[i].c_str(), keyWordScale[i], keyWordScaleOffset[i]);
        }

        switch (iLoop)
        {
          case RAWTARGET_LOOP:
          {
            rawTargetList.resize(entriesNum);
            break;
          }
          case OBJECT_LOOP:
          {
            objectList.resize(entriesNum);
            break;
          }
        }
        for (int i = 0; i < entriesNum; i++)
        {
          switch (iLoop)
          {
            case RAWTARGET_LOOP:
            {
              float dist = 0.0;
              float azimuth = 0.0;
              float ampl = 0.0;
              float vrad = 0.0;
              int mode = 0;
              for (int j = 0; j < numKeyWords; j++)
              {
                int dataRowIdx = keyWordPos[j] + 4 + i;
                std::string token = keyWordList[j];
                if (token.compare(DIST1_KEYWORD) == 0)
                {
                  int distRaw = getHexValue(fields[dataRowIdx]);
                  dist = convertScaledIntValue(distRaw, keyWordScale[j], keyWordScaleOffset[j]) * 0.001;
                }
                if (token.compare(AZMT1_KEYWORD) == 0)
                {
                  int azimuthRaw = getShortValue(fields[dataRowIdx]);
                  azimuth = (float) convertScaledIntValue(azimuthRaw, keyWordScale[j], keyWordScaleOffset[j]);
                }
                if (token.compare(VRAD1_KEYWORD) == 0)
                {
                  int vradRaw = getShortValue(fields[dataRowIdx]);
                  vrad = (float) convertScaledIntValue(vradRaw, keyWordScale[j], keyWordScaleOffset[j]);
                }
                if (token.compare(MODE1_KEYWORD) == 0)
                {
                  int modeRaw = getHexValue(fields[dataRowIdx]);
                  mode = (int) (convertScaledIntValue(modeRaw, keyWordScale[j], keyWordScaleOffset[j]) + 0.5);
                }
                if (token.compare(AMPL1_KEYWORD) == 0)
                {
                  int amplRaw = getShortValue(fields[dataRowIdx]);
                  ampl = (int) (convertScaledIntValue(amplRaw, keyWordScale[j], keyWordScaleOffset[j]) + 0.5);
                }
              }
              rawTargetList[i].Dist(dist);
              rawTargetList[i].Ampl(ampl);
              rawTargetList[i].Azimuth(azimuth);
              rawTargetList[i].Mode(mode);
              rawTargetList[i].Vrad(vrad);

            }
              break;
            case OBJECT_LOOP:
            {
              float px = 0.0;
              float py = 0.0;
              float vx = 0.0;
              float vy = 0.0;
              float objLen = 0.0;
              int objId = 0;

              for (int j = 0; j < numKeyWords; j++)
              {
                int dataRowIdx = keyWordPos[j] + 4 + i;
                std::string token = keyWordList[j];
                int intVal = getShortValue(fields[dataRowIdx]);
                float val = convertScaledIntValue(intVal, keyWordScale[j], keyWordScaleOffset[j]);

                if (token.compare(P3DX1_KEYWORD) == 0)
                {
                  px = val * 0.001f;
                }
                if (token.compare(P3DY1_KEYWORD) == 0)
                {
                  py = val * 0.001f;
                }
                if (token.compare(V3DX1_KEYWORD) == 0)
                {
                  vx = val;
                }
                if (token.compare(V3DY1_KEYWORD) == 0)
                {
                  vy = val;

                }
                if (token.compare(OBJLEN_KEYWORD) == 0)
                {
                  objLen = val;
                }
                if (token.compare(OBJID_KEYWORD) == 0)
                {
                  int objIdRaw = getHexValue(fields[dataRowIdx]);
                  objId = (int) (objIdRaw * keyWordScale[j] + 0.5);
                }
              }

              objectList[i].ObjId(objId);
              objectList[i].ObjLength(objLen);
              objectList[i].P3Dx(px);
              objectList[i].P3Dy(py);
              objectList[i].V3Dx(vx);
              objectList[i].V3Dy(vy);
            }
              break;
          }
        }
      }
      // Now parsing the entries
    }

    return (exitCode);
  }

  void SickScanRadarSingleton::simulateAsciiDatagram(unsigned char *receiveBuffer, int *actual_length)
  {
    static int callCnt = 0;

    callCnt++;

    // end
    std::string header = "\x2sSN LMDradardata 1 1 112F6E9 0 0 DFB6 B055 6E596002 6E5AE0E5 0 0 0 0 0 0 1 0 0 ";
    // std::string header =   "\x2sSN LMDradardata 10 20 30 5 6 40 50 60 70 90 A0 B0 C0 D0 E0 1 A000 FFFFFF00 ";
    int channel16BitCnt = 4;
    // Faktoren fuer Rohziele: 40.0 0.16 0.04 1.00 1.00
    float rawTargetFactorList[] = {40.0f, 0.16f, 0.04f, 1.00f, 1.00f};
    float objectFactorList[] = {64.0f, 64.0f, 0.1f, 0.1f, 0.75f, 1.0f};

    std::string dist1_intro = "DIST1 42200000 00000000";
    std::string azmt1_intro = "AZMT1 3E23D70A 00000000";
    std::string vrad1_intro = "VRAD1 3D23D70A 00000000";
    std::string ampl1_intro = "AMPL1 3F800000 00000000";

    std::string pdx1_intro = "P3DX1 42800000 00000000";
    std::string pdy1_intro = "P3DY1 42800000 00000000";
    std::string v3dx_intro = "V3DX1 3DCCCCCD 00000000";
    std::string v3dy_intro = "V3DY1 3DCCCCCD 00000000";
    std::string oblen_intro = "OBLE1 3F400000 00000000";

    int rawTargetChannel16BitCnt = 4;
    int rawTargetChannel8BitCnt = 1;
    std::string mode1_intro = "MODE1 3F800000 00000000";

    std::string obid_intro = "OBID1 3F800000 00000000";


    std::string trailer = "0 0 0 0 0 0\x3";

    std::vector<std::string> channel16BitID;
    std::vector<std::string> channel8BitID;
    channel16BitID.push_back(dist1_intro);
    channel16BitID.push_back(azmt1_intro);
    channel16BitID.push_back(vrad1_intro);
    channel16BitID.push_back(ampl1_intro);

    channel16BitID.push_back(pdx1_intro);
    channel16BitID.push_back(pdy1_intro);
    channel16BitID.push_back(v3dx_intro);
    channel16BitID.push_back(v3dy_intro);
    channel16BitID.push_back(oblen_intro);


    channel8BitID.push_back(mode1_intro);
    channel8BitID.push_back(obid_intro);

    int channel8BitCnt = channel8BitID.size();
    int objectChannel16BitCnt = 5;
    channel16BitCnt = channel16BitID.size();
    float x = 20.0;
    float speed = 50.0; // [m/s]
    std::vector<SickScanRadarRawTarget> rawTargetList;

#if 0 // simulate railway crossing
    for (float y = -20; y <= 20.0; y += 2.0)
    {
      SickScanRadarRawTarget rawTarget;
      float azimuth = atan2(y, x);
      float dist = sqrt(x*x + y*y);
      float vrad = speed * sin(azimuth);  // speed in y direction
      float mode = 0;
      float ampl = 50.0 + y;  // between 30 and 70
      rawTarget.Ampl(ampl);
      rawTarget.Mode(mode);
      rawTarget.Dist(dist);
      rawTarget.Azimuth(azimuth);
      rawTarget.Vrad(vrad);
      rawTargetList.push_back(rawTarget);
    }
#endif

    std::vector<SickScanRadarObject> objectList;

    int objId = 0;
    for (float x = 20; x <= 100.0; x += 50.0)
    {
      SickScanRadarObject vehicle;
      float y = 0.0;
      for (int iY = -1; iY <= 1; iY += 2)
      {
        float xp[2] = {0};  // for raw target
        float yp[2] = {0};
        float vehicleWidth = 1.8;
        y = iY * 2.0;
        float speed = y * 10.0f;
        vehicle.V3Dx(speed); // +/- 20 m/s
        vehicle.V3Dy(0.1f); // just for testing

        float xOff = 20.0;
        if (speed < 0.0)
        {
          xOff = 100.0;
        }
        vehicle.P3Dx((xOff + 0.1 * speed * (callCnt % 20)) * 1000.0);
        vehicle.P3Dy(y * 1000.0);
        float objLen = 6.0f + y;
        vehicle.ObjLength(objLen);

        vehicle.ObjId(objId++);
        objectList.push_back(vehicle);

        for (int i = 0; i < 2; i++)
        {
          SickScanRadarRawTarget rawTarget;

          xp[i] = vehicle.P3Dx() * 0.001;
          yp[i] = vehicle.P3Dy() * 0.001;

          if (i == 0)
          {
            yp[i] -= vehicleWidth / 2.0;
          }
          else
          {
            yp[i] += vehicleWidth / 2.0;
          }
          if (speed < 0.0)  // oncoming
          {
            xp[i] -= objLen * 0.5;
          }
          else
          {
            xp[i] += objLen * 0.5;
          }

          float azimuth = atan2(yp[i], xp[i]);
          float dist = sqrt(xp[i] * xp[i] + yp[i] * yp[i]);
          float vrad = speed * cos(azimuth);  // speed in y direction
          float mode = 0;
          float ampl = 50.0;  // between 30 and 70

          rawTarget.Ampl(ampl);
          rawTarget.Mode(mode);
          rawTarget.Dist(dist);
          rawTarget.Azimuth(azimuth);
          rawTarget.Vrad(vrad);
          rawTargetList.push_back(rawTarget);

        }

      }


    }

    char szDummy[255] = {0};
    std::string resultStr;
    resultStr += header;
    sprintf(szDummy, "%x ", channel16BitCnt);
    resultStr += szDummy;
    for (int i = 0; i < channel16BitCnt; i++)
    {
      resultStr += channel16BitID[i];
      int valNum = rawTargetList.size();
      bool processRawTarget = true;
      if (i < rawTargetChannel16BitCnt)
      {
        valNum = rawTargetList.size();
      }
      else
      {
        processRawTarget = false;
        valNum = objectList.size();
      }

      sprintf(szDummy, " %x ", valNum);
      resultStr += szDummy;
      float val = 0.0;
      for (int j = 0; j < valNum; j++)
      {
        switch (i)
        {
          case 0:
            val = 1000.0 * rawTargetList[j].Dist();
            break;
          case 1:
            val = 1.0 / deg2rad * rawTargetList[j].Azimuth();
            break;
          case 2:
            val = rawTargetList[j].Vrad();
            break;
          case 3:
            val = rawTargetList[j].Ampl();
            break;
          case 4:
            val = objectList[j].P3Dx();
            break;
          case 5:
            val = objectList[j].P3Dy();
            break;
          case 6:
            val = objectList[j].V3Dx();
            break;
          case 7:
            val = objectList[j].V3Dy();
            break;
          case 8:
            val = objectList[j].ObjLength();
            break;
        }


        if (processRawTarget)
        {
          val /= rawTargetFactorList[i];
        }
        else
        {
          int idx = i - rawTargetChannel16BitCnt;
          val /= objectFactorList[idx];
        }

        if (val > 0.0)
        {
          val += 0.5;
        }
        else
        {
          val -= 0.5;
        }
        int16_t shortVal = (int16_t) (val);

        sprintf(szDummy, "%08x", shortVal);
        strcpy(szDummy, szDummy + 4);  // remove first 4 digits due to integer / short
        resultStr += szDummy;
        resultStr += " ";
      }
    }

    sprintf(szDummy, "%x ", channel8BitCnt);
    resultStr += szDummy;
    int valNum = rawTargetList.size();
    for (int i = 0; i < channel8BitCnt; i++)
    {
      resultStr += channel8BitID[i];
      float val = 0.0;
      bool processRawTarget = true;
      if (i < rawTargetChannel8BitCnt)
      {
        valNum = rawTargetList.size();
      }
      else
      {
        processRawTarget = false;
        valNum = objectList.size();
      }
      sprintf(szDummy, " %x ", valNum);
      resultStr += szDummy;
      for (int j = 0; j < valNum; j++)
      {
        switch (i)
        {
          case 0:
            val = rawTargetList[j].Mode();
            break;
          case 1:
            val = objectList[j].ObjId();
            break;
        }

        int offset = 0;
        if (processRawTarget)
        {
          offset = rawTargetChannel16BitCnt;
          val /= rawTargetFactorList[i + offset];
        }
        else
        {
          offset = objectChannel16BitCnt;
          int idx = i - rawTargetChannel8BitCnt;
          val /= objectFactorList[idx + offset];
        }
        if (val > 0.0)
        {
          val += 0.5;
        }
        else
        {
          val -= 0.5;
        }
        int8_t shortVal = (int16_t) (val);

        sprintf(szDummy, "%08x", shortVal);
        strcpy(szDummy, szDummy + 6);  // remove first 6 digits due to integer / short
        resultStr += szDummy;
        resultStr += " ";
      }
    }

    resultStr += trailer;

    *actual_length = resultStr.length();
    strcpy((char *) receiveBuffer, resultStr.c_str());
  }

  void SickScanRadarSingleton::simulateAsciiDatagramFromFile(unsigned char *receiveBuffer, int *pActual_length,
                                                             std::string filePattern)
  {
    static int callCnt = 0;
    FILE *fin;
    char szLine[1024] = {0};
    char szDummyWord[1024] = {0};
    int actual_length = 0;
    int cnt = 0;
    char szFileName[1024] = {0};

    receiveBuffer[0] = '\x02';
    actual_length++;

    for (int iLoop = 0; iLoop < 2; iLoop++)
    {
      sprintf(szFileName, filePattern.c_str(), callCnt);
      callCnt++;
      fin = fopen(szFileName, "r");

      if ((fin == NULL) && (iLoop == 0))
      {
        callCnt = 0;  // reset to 0. This enables a loop over recorded raw data
      }
      else
      {
        break;
      }

      if ((iLoop == 1) && (fin == NULL))
      {
        ROS_ERROR("Can not find simulation file corresponding to pattern %s", filePattern.c_str());
      }
    }

    while (fgets(szLine, 1024, fin) != NULL)
    {
      char *ptr = strchr(szLine, '\n');;
      if (ptr != NULL)
      {
        *ptr = '\0';
      }

      ptr = strchr(szLine, ':');
      if (ptr != NULL)
      {
        if (1 == sscanf(ptr + 2, "%s", szDummyWord))
        {
          if (cnt > 0)
          {
            receiveBuffer[actual_length++] = ' ';
          }
          strcpy((char *) receiveBuffer + actual_length, szDummyWord);
          actual_length += strlen(szDummyWord);
        }
        cnt++;
      }
    }
    receiveBuffer[actual_length] = (char) '\x03';
    actual_length++;
    receiveBuffer[actual_length] = (char) '\x00';
    actual_length++;
    *pActual_length = actual_length;
    fclose(fin);
  }

  int SickScanRadarSingleton::parseDatagram(ros::Time timeStamp, unsigned char *receiveBuffer, int actual_length,
                                            bool useBinaryProtocol)
  {
    int exitCode = ExitSuccess;

    enum enumSimulationMode
    {
      EMULATE_OFF, EMULATE_SYN, EMULATE_FROM_FILE_TRAIN, EMULATE_FROM_FILE_CAR
    };

    int simulationMode = EMULATE_OFF;

    if (this->getEmulation())
    {
      simulationMode = EMULATE_SYN;

    }
    switch (simulationMode)
    {
      case EMULATE_OFF: // do nothing - use real data
        break;
      case EMULATE_SYN:
        simulateAsciiDatagram(receiveBuffer, &actual_length);
        break;
      case EMULATE_FROM_FILE_TRAIN:
        simulateAsciiDatagramFromFile(receiveBuffer, &actual_length,
                                      "/mnt/hgfs/development/ros/bags/raw/trainSeq/tmp%06d.txt");
        break;
      case EMULATE_FROM_FILE_CAR:
        simulateAsciiDatagramFromFile(receiveBuffer, &actual_length,
                                      "/mnt/hgfs/development/ros/bags/raw/carSeq/tmp%06d.txt");
        break;

      default:
        printf("Simulation Mode unknown\n");

    }

    sensor_msgs::PointCloud2 cloud_;
    sick_scan::RadarScan radarMsg_;

    std::vector<SickScanRadarObject> objectList;
    std::vector<SickScanRadarRawTarget> rawTargetList;

    if (useBinaryProtocol)
    {
      throw std::logic_error("Binary protocol currently not supported.");
    }
    else
    {
      bool dataToProcess = false;
      char *buffer_pos = (char *) receiveBuffer;
      char *dstart = NULL;
      char *dend = NULL;
      int dlength = 0;
      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++;
        parseAsciiDatagram(dstart, dlength, &radarMsg_, objectList, rawTargetList);
      }
      else
      {
        dataToProcess = false;
      }


      enum RADAR_PROC_LIST
      {
        RADAR_PROC_RAW_TARGET, RADAR_PROC_TRACK, RADAR_PROC_NUM
      };
      //
      // First loop: looking for raw targets
      // Second loop: looking for tracking objects
      for (int iLoop = 0; iLoop < RADAR_PROC_NUM; iLoop++)
      {
        int numTargets = 0;
        if (dataToProcess)
        {
          std::string channelRawTargetId[] = {"x", "y", "z", "vrad", "amplitude"};
          std::string channelObjectId[] = {"x", "y", "z", "vx", "vy", "vz", "objLen", "objId"};
          std::vector<std::string> channelList;
          std::string frameId = "radar"; //this->commonPtr->getConfigPtr()->frame_id;;
          switch (iLoop)
          {
            case RADAR_PROC_RAW_TARGET:
              numTargets = rawTargetList.size();
              for (int i = 0; i < sizeof(channelRawTargetId) / sizeof(channelRawTargetId[0]); i++)
              {
                channelList.push_back(channelRawTargetId[i]);
              }
              frameId = "radar"; // TODO - move to param list
              break;
            case RADAR_PROC_TRACK:
              numTargets = objectList.size();
              for (int i = 0; i < sizeof(channelObjectId) / sizeof(channelObjectId[0]); i++)
              {
                channelList.push_back(channelObjectId[i]);
              }
              frameId = "radar"; // TODO - move to param list
              break;
          }
          if (numTargets == 0)
          {
            continue;
          }
          int numChannels = channelList.size();

          std::vector<float> valSingle;
          valSingle.resize(numChannels);
          cloud_.header.stamp = timeStamp;
          cloud_.header.frame_id = frameId;
          cloud_.header.seq = 0;
          cloud_.height = 1; // due to multi echo multiplied by num. of layers
          cloud_.width = numTargets;
          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++)
          {
            cloud_.fields[i].name = channelList[i];
            cloud_.fields[i].offset = i * sizeof(float);
            cloud_.fields[i].count = 1;
            cloud_.fields[i].datatype = sensor_msgs::PointField::FLOAT32;
          }

          cloud_.data.resize(cloud_.row_step * cloud_.height);
          float *valPtr = (float *) (&(cloud_.data[0]));
          int off = 0;
          for (int i = 0; i < numTargets; i++)
          {
            switch (iLoop)
            {
              case 0:
              {
                float angle = deg2rad * rawTargetList[i].Azimuth();
                valSingle[0] = rawTargetList[i].Dist() * cos(angle);
                valSingle[1] = rawTargetList[i].Dist() * sin(angle);
                valSingle[2] = 0.0;
                valSingle[3] = rawTargetList[i].Vrad();
                valSingle[4] = rawTargetList[i].Ampl();
              }
                break;

              case 1:
                valSingle[0] = objectList[i].P3Dx();
                valSingle[1] = objectList[i].P3Dy();
                valSingle[2] = 0.0;
                valSingle[3] = objectList[i].V3Dx();
                valSingle[4] = objectList[i].V3Dy();
                valSingle[5] = 0.0;
                valSingle[6] = objectList[i].ObjLength();
                valSingle[7] = objectList[i].ObjId();
                break;
            }

            for (int j = 0; j < numChannels; j++)
            {
              valPtr[off] = valSingle[j];
              off++;
            }
#ifndef _MSC_VER
#if 1 // just for debugging
            switch (iLoop)
            {
              case RADAR_PROC_RAW_TARGET:
                cloud_radar_rawtarget_pub_.publish(cloud_);
                break;
              case RADAR_PROC_TRACK:
                cloud_radar_track_pub_.publish(cloud_);
                break;
            }
#endif
#else
            printf("PUBLISH:\n");
#endif
            if (iLoop == RADAR_PROC_RAW_TARGET)
            {
              // is this a deep copy ???
              radarMsg_.targets = cloud_;
            }
          }
        }
      }
      // Publishing radar messages
      // ...
      radarMsg_.header.stamp = timeStamp;
      radarMsg_.header.frame_id = "radar";
      radarMsg_.header.seq = 0;

      radarMsg_.objects.resize(objectList.size());
      for (int i = 0; i < radarMsg_.objects.size(); i++)
      {
        float vx = objectList[i].V3Dx();
        float vy = objectList[i].V3Dy();
        float v = sqrt(vx * vx + vy * vy);
        float heading = atan2(objectList[i].V3Dy(), objectList[i].V3Dx());

        radarMsg_.objects[i].velocity.twist.linear.x = objectList[i].V3Dx();
        radarMsg_.objects[i].velocity.twist.linear.y = objectList[i].V3Dy();
        radarMsg_.objects[i].velocity.twist.linear.z = 0.0;

        radarMsg_.objects[i].bounding_box_center.position.x = objectList[i].P3Dx();
        radarMsg_.objects[i].bounding_box_center.position.y = objectList[i].P3Dy();
        radarMsg_.objects[i].bounding_box_center.position.z = 0.0;

        float heading2 = heading / 2.0;
        // (n_x, n_y, n_z) = (0, 0, 1), so (x, y, z, w) = (0, 0, sin(theta/2), cos(theta/2))
        // see also this beautiful website: https://quaternions.online/
        radarMsg_.objects[i].bounding_box_center.orientation.x = 0.0;
        radarMsg_.objects[i].bounding_box_center.orientation.y = 0.0;
        radarMsg_.objects[i].bounding_box_center.orientation.z = sin(heading2);
        radarMsg_.objects[i].bounding_box_center.orientation.w = cos(heading2);


        radarMsg_.objects[i].bounding_box_size.x = objectList[i].ObjLength();
        radarMsg_.objects[i].bounding_box_size.y = 1.7;
        radarMsg_.objects[i].bounding_box_size.z = 1.7;
        for (int ii = 0; ii < 6; ii++)
        {
          int mainDiagOffset = ii * 6 + ii;  // build eye-matrix
          radarMsg_.objects[i].object_box_center.covariance[mainDiagOffset] = 1.0;  // it is a little bit hacky ...
          radarMsg_.objects[i].velocity.covariance[mainDiagOffset] = 1.0;
        }
        radarMsg_.objects[i].object_box_center.pose = radarMsg_.objects[i].bounding_box_center;
        radarMsg_.objects[i].object_box_size = radarMsg_.objects[i].bounding_box_size;


      }

      radarScan_pub_.publish(radarMsg_);


    }
    return (exitCode);
  }

}