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
 
#include <sick_scan/sick_ros_wrapper.h>
 
#include <sick_scan/sick_scan_common_tcp.h>
#include <sick_scan/sick_generic_callback.h>
#include <sick_scan/sick_generic_parser.h>
#include <sick_scan/sick_generic_radar.h>
//#include <sick_scan/RadarScan.h> // generated by msg-generator
 
#define _USE_MATH_DEFINES
 
#include <math.h>
#include "string"
#include <stdio.h>
#include <stdlib.h>
 
namespace sick_scan_xd
{
 
 
/* Null, because instance will be initialized on demand. */
  SickScanRadarSingleton *SickScanRadarSingleton::instance = 0;
 
  SickScanRadarSingleton *SickScanRadarSingleton::getInstance(rosNodePtr nh)
  {
    if (instance == 0)
    {
      instance = new SickScanRadarSingleton(nh);
    }
 
    return instance;
  }
 
  SickScanRadarSingleton::SickScanRadarSingleton(rosNodePtr nh) : node(nh)
  {
    std::string nodename;
    rosDeclareParam(nh, "nodename", nodename);
    rosGetParam(nh, "nodename", nodename);
 
    // publish radar targets and objects
    cloud_radar_rawtarget_pub_ = rosAdvertise<ros_sensor_msgs::PointCloud2>(nh, nodename + "/cloud_radar_rawtarget", 100);
    cloud_radar_track_pub_ = rosAdvertise<ros_sensor_msgs::PointCloud2>(nh, nodename + "/cloud_radar_track", 100);
 
    radarScan_pub_ = rosAdvertise<sick_scan_msg::RadarScan>(nh, nodename + "/radar", 100);
    
    m_add_transform_xyz_rpy = sick_scan_xd::SickCloudTransform(nh, true); // Apply an additional transform to the cartesian pointcloud, default: "0,0,0,0,0,0" (i.e. no transform)
 
    float range_min = 0, range_max = 100;
    int range_filter_handling = 0;
    rosDeclareParam(nh, "range_min", range_min);
    rosGetParam(nh, "range_min", range_min);
    rosDeclareParam(nh, "range_max", range_max);
    rosGetParam(nh, "range_max", range_max);
    rosDeclareParam(nh, "range_filter_handling", range_filter_handling);
    rosGetParam(nh, "range_filter_handling", range_filter_handling);
    m_range_filter = sick_scan_xd::SickRangeFilter(range_min, range_max, (sick_scan_xd::RangeFilterResultHandling)range_filter_handling);
    ROS_INFO_STREAM("Range filter configuration for SickScanRadar: range_min=" << range_min << ", range_max=" << range_max << ", range_filter_handling=" << range_filter_handling);
 
  }
 
 
  int16_t getShortValue(std::string str)
  {
    int val = 0;
    if (1 == sscanf(str.c_str(), "%x", &val))
    {
 
    }
    else
    {
      ROS_WARN_STREAM("Problems parsing " << str << "\n");
    }
    return (val);
 
  }
 
  // Convert a hex string to 32 bit signed integer
  static int getHexValue_32_signed(std::string str)
  {
    int val = 0;
    if (1 == sscanf(str.c_str(), "%x", &val))
    {
 
    }
    else
    {
        ROS_WARN_STREAM("Problems parsing " << str << "\n");
    }
    return (val);
 
  }
 
  union INT_4BYTE_UNION
  {
    unsigned int u32_val;
    int8_t i8_val[4];
    int16_t i16_val[2];
    int32_t i32_val[1];
  };
  
  // Convert a hex string to 32, 16 or 8 bit signed integer (2er compliment)
  static int getHexValue_32_16_8_signed(std::string str)
  {
    INT_4BYTE_UNION conv;
    conv.u32_val = 0;
    if (1 == sscanf(str.c_str(), "%x", &conv.u32_val))
    {
      if (str.size() <= 2)
        return conv.i8_val[0];
      else if (str.size() <= 4)
        return conv.i16_val[0];
      else
        return conv.i32_val[0];
    }
    else
    {
        ROS_WARN_STREAM("getHexValue(): Problems parsing " << str << "\n");
    }
    return 0;
  }
 
  // Convert a hex string to 32 or 16 bit signed integer (2er compliment)
  static int getHexValue_32_16_signed(std::string str)
  {
    INT_4BYTE_UNION conv;
    conv.u32_val = 0;
    if (1 == sscanf(str.c_str(), "%x", &conv.u32_val))
    {
      if (str.size() <= 4)
        return conv.i16_val[0];
      else
        return conv.i32_val[0];
    }
    else
    {
        ROS_WARN_STREAM("getHexValue(): Problems parsing " << str << "\n");
    }
    return 0;
  }
 
  int getHexValue(std::string str)
  {
    // Hexvalues < 0x8000 are always positiv (unsigned), but hexvalues >= 0x8000 are interpreted as signed values (2er compliment)
    return getHexValue_32_16_signed(str);
    // return getHexValue_32_16_8_signed(str);
    // return getHexValue_32_signed(str);
  }
 
#if 0 // Basic unittests, disabled by default
  // Basic unittest for hex string to signed integer conversion (8, 16 or 32 bit signed values in 2er compliment).
  static void unittestHex2Int_32_16_8_signed(const char* hexstr, int value)
  {
    int val = getHexValue_32_16_8_signed(hexstr);
    if (val == value)
      std::cout << hexstr << " = " << val << " OK" << std::endl;
    else
      std::cerr << "## ERROR: " << hexstr << " = " << val << ", expected " << value << std::endl;
  }
  // Basic unittest for hex string to signed integer conversion (16 or 32 bit signed values in 2er compliment).
  static void unittestHex2Int_32_16_signed(const char* hexstr, int value)
  {
    int val = getHexValue_32_16_signed(hexstr);
    if (val == value)
      std::cout << hexstr << " = " << val << " OK" << std::endl;
    else
      std::cerr << "## ERROR: " << hexstr << " = " << val << ", expected " << value << std::endl;
  }
  // Some basic unittests for hex string to signed integer conversion (8, 16 or 32 bit signed values in 2er compliment). 
  // See online converter e.g. https://www.rapidtables.com/convert/number/hex-to-decimal.html
  static void unittestsHex2Int()
  {
    unittestHex2Int_32_16_8_signed("B", 11);
    unittestHex2Int_32_16_8_signed("0B", 11);
    unittestHex2Int_32_16_8_signed("7B", 123);
    unittestHex2Int_32_16_8_signed("A1", -95);
    unittestHex2Int_32_16_8_signed("123", 291);
    unittestHex2Int_32_16_8_signed("0123", 291);
    unittestHex2Int_32_16_8_signed("7123", 28963);
    unittestHex2Int_32_16_8_signed("9123", -28381);
    unittestHex2Int_32_16_8_signed("71235", 463413);
    unittestHex2Int_32_16_8_signed("A1235", 660021);
    unittestHex2Int_32_16_8_signed("AB1235", 11211317);
    unittestHex2Int_32_16_8_signed("7AB1235", 128651829);
    unittestHex2Int_32_16_8_signed("AAB1235", 178983477);
    unittestHex2Int_32_16_8_signed("1AAB1235", 447418933);
    unittestHex2Int_32_16_8_signed("7AAB1235", 2058031669);
    unittestHex2Int_32_16_8_signed("8AAB1235", -1968500171);
    unittestHex2Int_32_16_8_signed("AAAB1235", -1431629259);
    unittestHex2Int_32_16_signed("B", 11);
    unittestHex2Int_32_16_signed("0B", 11);
    unittestHex2Int_32_16_signed("7B", 123);
    unittestHex2Int_32_16_signed("A1", 161);
    unittestHex2Int_32_16_signed("123", 291);
    unittestHex2Int_32_16_signed("0123", 291);
    unittestHex2Int_32_16_signed("7123", 28963);
    unittestHex2Int_32_16_signed("7FFF", 32767);
    unittestHex2Int_32_16_signed("8000", -32768);
    unittestHex2Int_32_16_signed("9123", -28381);
    unittestHex2Int_32_16_signed("71235", 463413);
    unittestHex2Int_32_16_signed("A1235", 660021);
    unittestHex2Int_32_16_signed("AB1235", 11211317);
    unittestHex2Int_32_16_signed("7AB1235", 128651829);
    unittestHex2Int_32_16_signed("AAB1235", 178983477);
    unittestHex2Int_32_16_signed("1AAB1235", 447418933);
    unittestHex2Int_32_16_signed("7AAB1235", 2058031669);
    unittestHex2Int_32_16_signed("8AAB1235", -1968500171);
    unittestHex2Int_32_16_signed("AAAB1235", -1431629259);
  }
#endif // Basic unittests, disabled by default
 
  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);
  }
 
  class RadarDatagramField
  {
  public:
    RadarDatagramField(char* _data = 0, size_t _len = 0) : data(_data), len(_len) {}
    char* data;
    size_t len;
    template<typename T> bool toInteger(T &value) const
    {
      if(len == sizeof(value))
      {
        memcpy(&value, data, len);
        swap_endian((unsigned char *)&value, len);
        return true;
      }
      return false;
    }
  };
 
  static uint32_t radarFieldToUint32(const RadarDatagramField& field, bool useBinaryProtocol)
  {
    uint32_t u32_value = 0;
    if(useBinaryProtocol)
    {
      bool success = false;
      uint8_t u8_value = 0;
      uint16_t u16_value = 0;
      switch (field.len)
      {
      case 1:
        success = field.toInteger(u8_value);
        u32_value = u8_value;
        break;
      case 2:
        success = field.toInteger(u16_value);
        u32_value = u16_value;
        break;
      case 4:
        success = field.toInteger(u32_value);
        break;
      default:
        break;
      }
      if(!success)
        ROS_WARN_STREAM("radarFieldToUint32() failed (field.len=" << field.len << ")");
    }
    else
    {
      u32_value = strtoul(field.data, NULL, 16);
    }
    return u32_value;
  }
 
  static int32_t radarFieldToInt32(const RadarDatagramField& field, bool useBinaryProtocol)
  {
    // unittestsHex2Int(); // Basic unittests, disabled by default
    int32_t i32_value = 0;
    if(useBinaryProtocol)
    {
      bool success = false;
      int8_t i8_value = 0;
      int16_t i16_value = 0;
      switch (field.len)
      {
      case 1:
        success = field.toInteger(i8_value);
        i32_value = i8_value;
        break;
      case 2:
        success = field.toInteger(i16_value);
        i32_value = i16_value;
        break;
      case 4:
        success = field.toInteger(i32_value);
        break;
      default:
        break;
      }
      if(!success)
        ROS_WARN_STREAM("radarFieldToInt32() failed");
    }
    else
    {
      i32_value = getHexValue(field.data);
    }
    return i32_value;
  }
 
  static float radarFieldToFloat(const RadarDatagramField& field, bool useBinaryProtocol)
  {
    float value = 0;
    if(useBinaryProtocol)
    {
      if(field.len == sizeof(value))
      {
        memcpy(&value, field.data, field.len);
        swap_endian((unsigned char *)&value, sizeof(value));
      }
      else
      {
        ROS_WARN_STREAM("radarFieldToFloat(): field.len=" << field.len << ", expected 4 byte");
      }
    }
    else
    {
      std::string token(field.data);
      value = getFloatValue(token);
    }
    return value;
  }
 
 
  static std::string radarFieldToString(const RadarDatagramField& field, bool useBinaryProtocol)
  {
    return std::string(field.data, field.len);
  }
 
  typedef char* char_ptr;
 
  static bool appendRadarDatagramField(char_ptr & datagram, size_t & datagram_length, size_t field_length, std::vector<RadarDatagramField> & fields)
  {
    if(datagram_length >= field_length)
    {
      fields.push_back(RadarDatagramField(datagram, field_length));
      datagram += field_length;
      datagram_length -= field_length;
      return true;
    }
    return false;
  }
 
 
  static std::vector<RadarDatagramField> splitBinaryRadarDatagramToFields(char* datagram, size_t datagram_length, int verboseLevel)
  {
    bool success = true;
    std::vector<RadarDatagramField> fields;
    fields.reserve(datagram_length); // max. <datagram_length> fields in binary mode
    // Command type, 3 byte + space
    if(datagram_length >= 4 && memcmp(datagram, "sSN ", 4) == 0)
    {
      fields.push_back(RadarDatagramField(datagram, 3));
      datagram += 4;
      datagram_length -= 4;
    }
    else
    {
      return std::vector<RadarDatagramField>(); // unrecoverable parse error
    }
    // Command, 12 byte + space
    if(datagram_length >= 4 && memcmp(datagram, "LMDradardata ", 13) == 0)
    {
      fields.push_back(RadarDatagramField(datagram, 12));
      datagram += 13;
      datagram_length -= 13;
    }
    else
    {
      return std::vector<RadarDatagramField>(); // unrecoverable parse error
    }
    // Append radar datagram fields
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte Version number
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte Device number
    success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte Serial number
    success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields); // 2 x 1 byte Device status
    success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields); // 2 x 1 byte Device status
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte Telegram counter
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte Scan counter
    success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte Time since start up in microsec
    success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte Time of transmission in microsec
    success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields); // 2 x 1 byte Status of digital inputs
    success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields); // 2 x 1 byte Status of digital inputs
    success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields); // 2 x 1 byte Status of digital outputs
    success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields); // 2 x 1 byte Status of digital outputs
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte CycleDuration
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte reserved
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte Amount of encoder
    success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte Encoder position
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte Encoder speed
    // Append 16 bit channels
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte Amount of 16 bit channels
    uint16_t num_16_bit_channels = 0;
    fields.back().toInteger(num_16_bit_channels);
    // if(num_16_bit_channels > 4) // max 4 16-bit channel in case of RMSxxxx
    // {
    //   return std::vector<RadarDatagramField>(); // unrecoverable parse error
    // }
    for(int channel_idx = 0; channel_idx < num_16_bit_channels; channel_idx++)
    {
      success = success && appendRadarDatagramField(datagram, datagram_length, 5, fields); // 5 byte Content string (identifier)
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte scale factor (float)
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte scale factor offset (float)
      success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte Amount of data
      uint16_t amount_of_data = 0;
      fields.back().toInteger(amount_of_data);
      for(int data_field_idx = 0; data_field_idx < amount_of_data; data_field_idx++)
      {
        success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte output data
      }
    }
    // Append 8 bit channels
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte Amount of 8 bit channels
    uint16_t num_8_bit_channels = 0;
    fields.back().toInteger(num_8_bit_channels);
    if(num_8_bit_channels > 4) // max 4 8-bit channel
    {
      return std::vector<RadarDatagramField>(); // unrecoverable parse error
    }
    for(int channel_idx = 0; channel_idx < num_8_bit_channels; channel_idx++)
    {
      success = success && appendRadarDatagramField(datagram, datagram_length, 5, fields); // 5 byte Content string (identifier)
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte scale factor (float)
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte scale factor offset (float)
      success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte Amount of data
      uint16_t amount_of_data = 0;
      fields.back().toInteger(amount_of_data);
      for(int data_field_idx = 0; data_field_idx < amount_of_data; data_field_idx++)
      {
        success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields); // 1 byte output data
      }
    }
    // Append position
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte Output of position data
    uint16_t position_data_available = 0;
    fields.back().toInteger(position_data_available);
    if(position_data_available)
    {
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte X-coordinate (float)
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte Y-coordinate (float)
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte Z-coordinate (float)
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte X-rotation (float)
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte Y-rotation (float)
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields); // 4 byte Z-rotation (float)
      success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields); // 1 byte rotation type
      success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields); // 1 byte device name
    }
    // Append device name
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte name is transmitted
    uint16_t device_name_available = 0;
    fields.back().toInteger(device_name_available);
    if(device_name_available)
    {
      success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields);  // 1 byte length of name
      success = success && appendRadarDatagramField(datagram, datagram_length, 16, fields); // 16 byte device name
    }
    // Append comment
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte comment is transmitted
    uint16_t comment_available = 0;
    fields.back().toInteger(comment_available);
    if(comment_available)
    {
      success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields);  // 1 byte length of comment
      success = success && appendRadarDatagramField(datagram, datagram_length, 16, fields); // 16 byte comment
    }
    // Append timestamp
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte timestamp is transmitted
    uint16_t timestamp_available = 0;
    fields.back().toInteger(timestamp_available);
    if(timestamp_available)
    {
      success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields);  // 2 byte Year
      success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields);  // 1 byte Month
      success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields);  // 1 byte Day
      success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields);  // 1 byte Hour
      success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields);  // 1 byte Minutes
      success = success && appendRadarDatagramField(datagram, datagram_length, 1, fields);  // 1 byte Seconds
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields);  // 4 byte Microseconds
    }
    // Append eventinfo
    success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte eventinfo is transmitted
    uint16_t eventinfo_available = 0;
    fields.back().toInteger(eventinfo_available);
    if(eventinfo_available)
    {
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields);  // 4 byte type (string)
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields);  // 4 byte encoder position
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields);  // 4 byte time of event in microseconds
      success = success && appendRadarDatagramField(datagram, datagram_length, 4, fields);  // 4 byte angle of event
    }
    // Return radar fields
    if(verboseLevel > 0)
    {
      ROS_INFO_STREAM("splitBinaryRadarDatagramToFields(): " << fields.size() << " fields, " << num_16_bit_channels << " 16-bit channels, " << num_8_bit_channels << " 8-bit channels, success=" << success);
    }
    return fields;
  }
 
 
  int SickScanRadarSingleton::parseRadarDatagram(char* datagram, size_t datagram_length, bool useBinaryProtocol,
                                                 sick_scan_msg::RadarScan *msgPtr,
                                                 std::vector<SickScanRadarObject> &objectList,
                                                 std::vector<SickScanRadarRawTarget> &rawTargetList,
                                                 int verboseLevel)
  {
    int exitCode = ExitSuccess;
    bool dumpData = false;
 
    // !!!!!
    // verboseLevel = 1;
    int HEADER_FIELDS = 32;
 
    // Reserve sufficient space
    std::vector<RadarDatagramField> fields;
 
    // ----- 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)
    {
      sick_scan_xd::SickScanCommon::dumpDatagramForDebugging((unsigned char *)datagram, datagram_length, useBinaryProtocol);
    }
 
    strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
    datagram_copy[datagram_length] = 0;
 
    // ----- tokenize
    if(useBinaryProtocol)
    {
      fields = splitBinaryRadarDatagramToFields(datagram, datagram_length, verboseLevel);
    }
    else
    {
      fields.reserve(datagram_length / 2); // max. datagram_length/2 in ascii mode
      char* cur_field = strtok(datagram, " ");
      while (cur_field != NULL)
      {
        fields.push_back(RadarDatagramField(cur_field, strlen(cur_field)));
        //std::cout << cur_field << std::endl;
        cur_field = strtok(NULL, " ");
      }
    }
 
    //std::cout << fields[27] << std::endl;
    size_t count = fields.size();
 
    if (verboseLevel > 0 && !useBinaryProtocol)
    {
      std::vector<unsigned char> raw_fields;
      for (int i = 0; i < count; i++)
      {
        for(int j = 0; j < fields[i].len; j++)
          raw_fields.push_back(fields[i].data[j]);
        raw_fields.push_back(' ');
      }
      sick_scan_xd::SickScanCommon::dumpDatagramForDebugging(raw_fields.data(), raw_fields.size(), useBinaryProtocol);
    }
 
 
    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
        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.
     *
     */
    if (count < PREHADERR_TOKEN_FIX_NUM)
    {
      ROS_WARN_STREAM("parseRadarDatagram: " << count << " fields in datagram, at least " << PREHADERR_TOKEN_FIX_NUM << " fields required");
      return ExitError;
    }
    for (int i = 0; i < PREHADERR_TOKEN_FIX_NUM; i++)
    {
      if(i == PREHEADER_TOKEN_SSN || i == PREHEADER_TOKEN_LMDRADARDATA) // skip "sSN LMDradardata"
        continue;
      UINT16 uiValue = 0x00;
      UINT32 udiValue = 0x00;
      unsigned long int uliDummy;
      uliDummy = radarFieldToUint32(fields[i], useBinaryProtocol); // strtoul(fields[i].data, 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:
          uiValue = radarFieldToUint32(fields[i], useBinaryProtocol); // sscanf(fields[i].data, "%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 = radarFieldToUint32(fields[rowIndex], useBinaryProtocol); // strtoul(fields[rowIndex].data, NULL, 16);
              msgPtr->radarpreheader.radarpreheaderarrayencoderblock[j].udiencoderpos = udiValue;
              udiValue = radarFieldToUint32(fields[rowIndex + 1], useBinaryProtocol); // strtoul(fields[rowIndex + 1].data, 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;
 
    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:
          switch(this->radarType)
          {
            case RADAR_1D:
              keyWordList.push_back(P3DX1_KEYWORD);
              keyWordList.push_back(V3DX1_KEYWORD);
              keyWordList.push_back(OBJLEN_KEYWORD);
              keyWordList.push_back(OBJID_KEYWORD);
              break;
            case RADAR_3D:
              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;
            default: // unsupported
            assert(0);
            break;
          }
 
          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 (fields[i].len == keyWordList[j].length() && strncmp(fields[i].data, keyWordList[j].c_str(), fields[i].len) == 0)
          {
            keyWordPos[j] = i;
          }
        }
      }
 
      bool entriesNumOk = true;
      bool allow_missing_keywords = false;
      int entriesNum = 0;
      if (keyWordPos[0] == -1)
      {
        entriesNumOk = false;
      }
      else
      {
 
        entriesNum = (int)radarFieldToInt32(fields[keyWordPos[0] + 3], useBinaryProtocol); // getHexValue(fields[keyWordPos[0] + 3].data);
        for (int i = 0; i < numKeyWords; i++)
        {
          if (keyWordPos[i] == -1)
          {
            entriesNumOk = false;
            ROS_WARN_STREAM("parseRadarDatagram(): " << (i + 1) << ". keyword " << keyWordList[i] << " missing, but first keyword " << keyWordList[0] << " found, value set to 0.0");
            entriesNumOk = false;
            allow_missing_keywords = true; // P3DY1 and V3DY1 set to 0 if missing
          }
          else
          {
            int entriesNumTmp = (int)radarFieldToInt32(fields[keyWordPos[i] + 3], useBinaryProtocol); // getHexValue(fields[keyWordPos[i] + 3].data);
            if (entriesNumTmp != entriesNum)
            {
              ROS_WARN_STREAM("parseRadarDatagram(): Number of items for keyword " << keyWordList[i] << " differs from number of items for " << keyWordList[0]);
              entriesNumOk = false;
            }
          }
        }
      }
 
      if (true == entriesNumOk || true == allow_missing_keywords)
      {
 
 
        for (int i = 0; i < numKeyWords; i++)
        {
          if (true == allow_missing_keywords && keyWordPos[i] == -1)
          {
            continue; // keyword is missing
          }
          int scaleLineIdx = keyWordPos[i] + 1;
          int scaleOffsetLineIdx = keyWordPos[i] + 2;
          // std::string token = radarFieldToString(fields[scaleLineIdx], useBinaryProtocol); // fields[scaleLineIdx].data;
          keyWordScale[i] = radarFieldToFloat(fields[scaleLineIdx], useBinaryProtocol); // getFloatValue(token);
          // token = fields[scaleOffsetLineIdx].data;
          keyWordScaleOffset[i] = radarFieldToFloat(fields[scaleOffsetLineIdx], useBinaryProtocol); // 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++)
              {
                if (true == allow_missing_keywords && keyWordPos[j] == -1)
                {
                  continue; // keyword is missing
                }
                int dataRowIdx = keyWordPos[j] + 4 + i;
                std::string token = keyWordList[j];
                if (token.compare(DIST1_KEYWORD) == 0)
                {
                  int distRaw = (int)radarFieldToInt32(fields[dataRowIdx], useBinaryProtocol); // getHexValue(fields[dataRowIdx].data);
                  dist = convertScaledIntValue(distRaw, keyWordScale[j], keyWordScaleOffset[j]) * 0.001f;
                }
                if (token.compare(AZMT1_KEYWORD) == 0)
                {
                  int azimuthRaw = (int)radarFieldToInt32(fields[dataRowIdx], useBinaryProtocol); // getShortValue(fields[dataRowIdx].data);
                  azimuth = (float) convertScaledIntValue(azimuthRaw, keyWordScale[j], keyWordScaleOffset[j]);
                }
                if (token.compare(VRAD1_KEYWORD) == 0)
                {
                  int vradRaw = (int)radarFieldToInt32(fields[dataRowIdx], useBinaryProtocol); // getShortValue(fields[dataRowIdx].data);
                  vrad = (float) convertScaledIntValue(vradRaw, keyWordScale[j], keyWordScaleOffset[j]);
                }
                if (token.compare(MODE1_KEYWORD) == 0)
                {
                  int modeRaw = (int)radarFieldToInt32(fields[dataRowIdx], useBinaryProtocol); // getHexValue(fields[dataRowIdx].data);
                  mode = (int) (convertScaledIntValue(modeRaw, keyWordScale[j], keyWordScaleOffset[j]) + 0.5);
                }
                if (token.compare(AMPL1_KEYWORD) == 0)
                {
                  int amplRaw = (int)radarFieldToInt32(fields[dataRowIdx], useBinaryProtocol); // getShortValue(fields[dataRowIdx].data);
                  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++)
              {
                if (true == allow_missing_keywords && keyWordPos[j] == -1)
                {
                  continue; // keyword is missing
                }
                int dataRowIdx = keyWordPos[j] + 4 + i;
                std::string token = keyWordList[j];
                int intVal = (int)radarFieldToInt32(fields[dataRowIdx], useBinaryProtocol); // getShortValue(fields[dataRowIdx].data);
                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 = (int)radarFieldToInt32(fields[dataRowIdx], useBinaryProtocol); // getHexValue(fields[dataRowIdx].data);
                  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.8f;
        y = iY * 2.0;
        float speed = y * 10.0f; // [m/s]
        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_STREAM("Can not find simulation file corresponding to pattern " << filePattern);
      }
    }
 
    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(rosTime 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");
 
    }
 
    ros_sensor_msgs::PointCloud2 cloud_;
    sick_scan_msg::RadarScan radarMsg_;
 
    std::vector<SickScanRadarObject> objectList;
    std::vector<SickScanRadarRawTarget> rawTargetList;
 
    if (useBinaryProtocol && getNameOfRadar() == SICK_SCANNER_RMS_XXXX_NAME)
    {
      throw std::logic_error("Binary protocol for RMSxxxx currently not supported. Please use <param name=\"use_binary_protocol\" type=\"bool\" value=\"false\"/> in your launchfile.");
    }
    else
    {
      bool dataToProcess = false;
      char *buffer_pos = (char *) receiveBuffer;
      char *dstart = NULL;
      char *dend = NULL;
      int32_t dlength = 0;
 
      if (useBinaryProtocol)
      {
        // Decode binary: {4 byte STX} + {4 byte payload length} + {binary payload} + {1 byte CRC}
        uint32_t binary_stx = 0x02020202;
        dstart = strchr(buffer_pos, 0x02);
        if(dstart != 0 && memcmp(dstart, &binary_stx, sizeof(binary_stx)) == 0)
        {
          dstart += 4; // {4 byte STX}
          memcpy(&dlength, dstart, 4); // read length indicator
          swap_endian((unsigned char *) &dlength, 4);
          dstart += 4; // {4 byte payload length}
          dataToProcess = true; // continue parsing
        }
      }
      else
      {
        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++;
        }
      }
      if(dataToProcess && dstart != 0 && dlength > 0 && dstart + dlength <= (char*)receiveBuffer + actual_length)
      {
        if (parseRadarDatagram(dstart, dlength, useBinaryProtocol, &radarMsg_, objectList, rawTargetList) != ExitSuccess)
        {
          dataToProcess = false;
        }
      }
      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;
          ROS_HEADER_SEQ(cloud_.header, 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 = ros_sensor_msgs::PointField::FLOAT32;
          }
 
          cloud_.data.resize(cloud_.row_step * cloud_.height, 0);
          float *valPtr = (float *) (&(cloud_.data[0]));
          int off = 0;
          size_t numFilteredTargets = 0;
          for (int i = 0; i < numTargets; i++)
          {
            bool tgt_valid = true, range_modified = false;
            switch (iLoop)
            {
              case 0:
                {
                  float angle = deg2rad * rawTargetList[i].Azimuth();
                  float range = rawTargetList[i].Dist();
                  tgt_valid = m_range_filter.apply(range, range_modified);
                  if (tgt_valid)
                  {
                    valSingle[0] = range * cos(angle);
                    valSingle[1] = range * 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;
            }
            if (!tgt_valid)
              continue;
            numFilteredTargets++;
            m_add_transform_xyz_rpy.applyTransform(valSingle[0], valSingle[1], valSingle[2]); // apply optional transform to (x, y, z)
 
            for (int j = 0; j < numChannels; j++)
            {
              valPtr[off] = valSingle[j];
              off++;
            }
            if (iLoop == RADAR_PROC_RAW_TARGET)
            {
              // is this a deep copy ???
              radarMsg_.targets = cloud_;
            }
          }
          if (numFilteredTargets < numTargets)
            m_range_filter.resizePointCloud(numFilteredTargets, cloud_); // targets dropped by range filter, resize pointcloud
#ifndef ROSSIMU
            sick_scan_xd::PointCloud2withEcho sick_cloud_msg(&cloud_, 1, 0, "radar");
            switch (iLoop)
            {
              case RADAR_PROC_RAW_TARGET:
                  notifyCartesianPointcloudListener(node, &sick_cloud_msg);
                  rosPublish(cloud_radar_rawtarget_pub_, cloud_);
                break;
              case RADAR_PROC_TRACK:
                  notifyCartesianPointcloudListener(node, &sick_cloud_msg);
                  rosPublish(cloud_radar_track_pub_, cloud_);
                break;
            }
#endif
 
        }
      }
      // Publishing radar messages
      // ...
      radarMsg_.header.stamp = timeStamp;
      radarMsg_.header.frame_id = "radar";
      ROS_HEADER_SEQ(radarMsg_.header, 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].id = objectList[i].ObjId();
        radarMsg_.objects[i].tracking_time = timeStamp;
 
        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;
 
 
      }
 
      notifyRadarScanListener(node, &radarMsg_);
      rosPublish(radarScan_pub_, radarMsg_);
 
 
    }
    return (exitCode);
  }
 
}