00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- 00002 00003 // -- BEGIN LICENSE BLOCK ---------------------------------------------- 00004 00024 // -- END LICENSE BLOCK ------------------------------------------------ 00025 00026 //---------------------------------------------------------------------- 00033 //---------------------------------------------------------------------- 00034 00035 #include <sick_safetyscanners/data_processing/ParseFieldGeometryData.h> 00036 00037 #include <sick_safetyscanners/cola2/Command.h> 00038 00039 namespace sick { 00040 namespace data_processing { 00041 00042 ParseFieldGeometryData::ParseFieldGeometryData() 00043 { 00044 m_reader_ptr = std::make_shared<sick::data_processing::ReadWriteHelper>(); 00045 } 00046 00047 00048 bool ParseFieldGeometryData::parseTCPSequence(const datastructure::PacketBuffer& buffer, 00049 sick::datastructure::FieldData& field_data) const 00050 { 00051 const uint8_t* data_ptr(buffer.getBuffer().data()); 00052 uint32_t array_length = readArrayLength(data_ptr); 00053 std::vector<uint16_t> geometry_distance_mm; 00054 for (uint32_t i = 0; i < array_length; i++) 00055 { 00056 geometry_distance_mm.push_back(readArrayElement(data_ptr, i)); 00057 } 00058 field_data.setBeamDistances(geometry_distance_mm); 00059 00060 return true; 00061 } 00062 00063 uint32_t ParseFieldGeometryData::readArrayLength(const uint8_t*& data_ptr) const 00064 { 00065 return m_reader_ptr->readuint32_tLittleEndian(data_ptr, 4); 00066 } 00067 00068 uint16_t ParseFieldGeometryData::readArrayElement(const uint8_t*& data_ptr, 00069 uint32_t elem_number) const 00070 { 00071 return m_reader_ptr->readuint16_tLittleEndian(data_ptr, 8 + elem_number * 2); 00072 } 00073 00074 00075 } // namespace data_processing 00076 } // namespace sick