result_port_parser.cpp
Go to the documentation of this file.
1 /*
2  * @brief sim_loc_result_port_parser implements a parser for
3  * result port telegrams for SIM Localization.
4  *
5  * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
6  * Copyright (C) 2019 SICK AG, Waldkirch
7  *
8  * Licensed under the Apache License, Version 2.0 (the "License");
9  * you may not use this file except in compliance with the License.
10  * You may obtain a copy of the License at
11  *
12  * http://www.apache.org/licenses/LICENSE-2.0
13  *
14  * Unless required by applicable law or agreed to in writing, software
15  * distributed under the License is distributed on an "AS IS" BASIS,
16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17  * See the License for the specific language governing permissions and
18  * limitations under the License.
19  *
20  * All rights reserved.
21  *
22  * Redistribution and use in source and binary forms, with or without
23  * modification, are permitted provided that the following conditions are met:
24  *
25  * * Redistributions of source code must retain the above copyright
26  * notice, this list of conditions and the following disclaimer.
27  * * Redistributions in binary form must reproduce the above copyright
28  * notice, this list of conditions and the following disclaimer in the
29  * documentation and/or other materials provided with the distribution.
30  * * Neither the name of SICK AG nor the names of its
31  * contributors may be used to endorse or promote products derived from
32  * this software without specific prior written permission
33  * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
34  * contributors may be used to endorse or promote products derived from
35  * this software without specific prior written permission
36  *
37  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
38  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
39  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
40  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
41  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
42  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
43  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
44  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
45  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
46  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
47  * POSSIBILITY OF SUCH DAMAGE.
48  *
49  * Authors:
50  * Michael Lehning <michael.lehning@lehning.de>
51  *
52  * Copyright 2019 SICK AG
53  * Copyright 2019 Ing.-Buero Dr. Michael Lehning
54  *
55  */
56 #include <stdexcept>
57 #include "sick_scan/ros_wrapper.h"
58 
59 #include "crc16ccitt_false.h"
61 
63 #define PARSE_ASSERT(assertion,info) sick_scan::ResultPortParser::parseAssert((assertion),(#assertion),info,__FILE__,__LINE__)
64 
65 /*
66  * Constructor of class ResultPortParser, which implements a parser for
67  * result port telegrams for SIM Localization.
68  * @param[in] frame_id frame_id of published ros messages (type SickLocResultPortTelegramMsg)
69  */
70 sick_scan::ResultPortParser::ResultPortParser(const std::string & frame_id) : m_publish_frame_id(frame_id), m_result_port_telegram(), m_little_endian_payload(false)
71 {
72 }
73 
74 /*
75  * Shortcut to print error messages and to throw an std::invalid_argument exception in case of assertion failures
76  */
77 void sick_scan::ResultPortParser::parseAssert(bool assertion, const std::string & assertion_msg, const std::string & info, const std::string & file, int line)
78 {
79  if(!assertion)
80  {
81  std::stringstream exc_info;
82  exc_info << "sick_scan::ResultPortParser: assertion \"" << assertion_msg << "\" failed, " << info << " (" << file << ":" << line;
83  throw std::invalid_argument(exc_info.str());
84  }
85 }
86 
87 /*
88  * Copies the next N=sizeof(value) bytes from binary_data to a value (number type), starting at binary_data[start_byte].
89  * @param[in] binary_data binary data, at least start_byte+sizeof(value) byte binary data
90  * @param[in] start_byte first byte to start copying from binary_data
91  * @param[out] value destination
92  * @param[in] info descriptional message, used in exception in case of errors
93  * @param[in] little_endian true: binary_data encoded in little endian format, false (default): binary_data encoded in big endian format
94  * @return number of bytes decoded
95  * @throws std::invalid_argument in case of parse errors
96  */
97 template<typename T> size_t sick_scan::ResultPortParser::copyBytesToValue(const std::vector<uint8_t> & binary_data, size_t start_byte, T & value, const std::string & info, bool little_endian)
98 {
99  PARSE_ASSERT(binary_data.size() >= start_byte + sizeof(value), std::string("ResultPortParser::copyBytesToValue(): invalid size (") + info + ")");
100  value = 0;
101  if(little_endian) // Little endian: LSB first, MSB last
102  {
103  for (int n = (int)sizeof(value) - 1; n >= 0; n--)
104  {
105  value = ((value << 8) | (binary_data[start_byte + n]));
106  }
107  }
108  else // Big endian: MSB first, LSB last
109  {
110  for (size_t n = 0; n < sizeof(value); n++)
111  {
112  value = ((value << 8) | (binary_data[start_byte + n]));
113  }
114  }
115  return sizeof(value);
116 }
117 
118 /*
119  * Copies the next N=dst_array.size() bytes from binary_data to dst_array, starting at binary_data[start_byte].
120  * @param[in] binary_data binary data, at least start_byte+sizeof(value) byte binary data
121  * @param[in] start_byte first byte to start copying from binary_data
122  * @param[out] dst_array destination
123  * @param[in] info descriptional message, used in exception in case of errors
124  * @return number of bytes decoded := dst_array.size()
125  * @throws std::invalid_argument in case of parse errors
126  */
127 template<typename T> size_t sick_scan::ResultPortParser::copyBytesToArray(const std::vector<uint8_t> & binary_data, size_t start_byte, std::vector<T> & dst_array, const std::string & info)
128 {
129  PARSE_ASSERT(binary_data.size() >= start_byte + dst_array.size(), std::string("ResultPortParser::copyBytesToArray(): invalid size (") + info + ")");
130  for (size_t n = 0; n < dst_array.size(); n++)
131  {
132  dst_array[n] = binary_data[start_byte + n];
133  }
134  return dst_array.size();
135 }
136 
137 /*
138  * Computes and returns the checksum of a result port telegram.
139  *
140  * Checksum := CRC16-CCITT over length of header (52 bytes) and payload (52 bytes) without 2 bytes of this trailer. Size: UInt16 = 2 byte
141  * Checksum details (See chapter 5.9 "About result port telegrams" of the operation manual for further details):
142  * Width: 16 bits
143  * Truncated polynomial: 0x1021 CRC polynomials with orders of x16 + x12 + x5 + 1 (counted without the leading '1' bit)
144  * Initial value = 0xFFFF
145  *
146  * Additional note: According to http://srecord.sourceforge.net/crc16-ccitt.html, CRC16-CCITT is specified by
147  * Width = 16 bits, Truncated polynomial = 0x1021, Initial value = 0xFFFF, Input data is NOT reflected, Output CRC is NOT reflected.
148  * This is often referred as "CRC-16/CCITT-FALSE" (because of "reflected_in/out=FALSE", width=16 poly=0x1021 init=0xffff refin=false refout=false),
149  * in contrast to other flavors like "CRC-16/MCRF4XX" (width=16 poly=0x1021 init=0xffff refin=true refout=true).
150  *
151  * This CRC checksum uses the implementation by https://github.com/madler/crcany (sources under the zlib license,
152  * permitting free commercial use) with algorithm "CRC-16/CCITT-FALSE" (crc16ccitt_false.c and crc16ccitt_false.h).
153  * Other crc checksum algorithms may be used if required.
154  *
155  * @param[in] binary_data binary data of result port telegram
156  * @param[in] binary_data_with_trailer true (default): binary_data (input) contains 2 byte trailer
157  * @return CRC16 checksum
158  */
159 uint16_t sick_scan::ResultPortParser::computeChecksum(const std::vector<uint8_t> & binary_data, bool binary_data_with_trailer)
160 {
161  PARSE_ASSERT(binary_data_with_trailer == false || binary_data.size() >= 2, std::string("ResultPortParser::computeChecksum(): invalid input, binary_data.size() = ") + std::to_string(binary_data.size()));
162  size_t len = binary_data_with_trailer ? (binary_data.size() - 2) : (binary_data.size());
163  unsigned checksum1 = ::crc16ccitt_false_bit(0xFFFF, binary_data.data(), len);
164  unsigned checksum2 = ::crc16ccitt_false_byte(0xFFFF, binary_data.data(), len);
165  unsigned checksum3 = ::crc16ccitt_false_word(0xFFFF, binary_data.data(), len);
166  PARSE_ASSERT(checksum1 == checksum2, std::string("ResultPortParser::computeChecksum(): ambigous checksums ") + std::to_string(checksum1) + "," + std::to_string(checksum2));
167  PARSE_ASSERT(checksum1 == checksum3, std::string("ResultPortParser::computeChecksum(): ambigous checksums ") + std::to_string(checksum1) + "," + std::to_string(checksum3));
168  return (uint16_t)(checksum1 & 0xFFFF);
169 }
170 
171 /*
172  * Returns true, if the PayloadType of a telegram_header indicates a little endian payload, or false otherwise.
173  * @param[in] payload_type the PayloadType of a telegram_header
174  * @return true for little endian payloads, false otherwise
175  */
177 {
178  return (payload_type == 0x06c2);
179 }
180 
181 /*
182  * Decodes the header of a result port telegram from binary data.
183  * @param[in] binary_data binary data, at least start_byte+52 byte binary result port telegram header
184  * @param[in] start_byte first byte to start decoding in binary_data
185  * @param[out] telegram_header decoded result port telegram header
186  * @return number of bytes decoded
187  * @throws std::invalid_argument in case of parse errors
188  */
189 size_t sick_scan::ResultPortParser::decodeResultPortHeader(const std::vector<uint8_t> & binary_data, size_t start_byte, sick_scan::SickLocResultPortHeaderMsg & telegram_header)
190 {
191  size_t bytes_decoded = 0;
192 
193  // Decode MagicWord: Magic word SICK (0x53 0x49 0x43 0x4B). Size: 4 × UInt8 = 4 byte
194  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.magicword, "Header.MagicWord");
195  PARSE_ASSERT(telegram_header.magicword == 0x5349434B, std::string("ResultPortParser::decodeResultPortHeader(): invalid Header.MagicWord ") + std::to_string(telegram_header.magicword));
196 
197  // Decode Length: Length of telegram incl. header, payload, and trailer. Size: UInt32 = 4 byte
198  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.length, "Header.Length");
199  PARSE_ASSERT(telegram_header.length == 106, std::string("ResultPortParser::decodeResultPortHeader(): invalid Header.Length ") + std::to_string(telegram_header.length));
200 
201  // Decode PayloadType: Payload type 0x06c2 = Little Endian, 0x0642 = Big Endian. Size: UInt16 = 2 byte
202  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.payloadtype, "Header.PayloadType");
203  PARSE_ASSERT(telegram_header.payloadtype == 0x06c2 || telegram_header.payloadtype == 0x0642, std::string("ResultPortParser::decodeResultPortHeader(): invalid PayloadType ") + std::to_string(telegram_header.payloadtype));
204  m_little_endian_payload = isLittleEndianPayload(telegram_header.payloadtype);
205 
206 
207  // Decode PayloadVersion: Version of PayloadType structure. Size: UInt16 = 2 byte
208  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.payloadversion, "Header.PayloadVersion");
209 
210  // Decode OrderNumber: Order number of the localization controller. Size: UInt32 = 4 byte
211  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.ordernumber, "Header.OrderNumber");
212 
213  // Decode SerialNumber: Serial number of the localization controller. Size: UInt32 = 4 byte
214  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.serialnumber, "Header.SerialNumber");
215 
216  // Decode FW_Version: Software version of the localization controller. Size: 20 × UInt8 = 20 byte
217  telegram_header.fw_version = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
218  bytes_decoded += copyBytesToArray(binary_data, start_byte + bytes_decoded, telegram_header.fw_version, "Header.FW_Version");
219 
220  // Decode TelegramCounter: Telegram counter since last start-up. Size: UInt32 = 4 byte
221  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.telegramcounter, "Header.TelegramCounter");
222 
223  // Decode SystemTime: Not used. Size: NTP = 8 byte
224  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.systemtime, "Header.SystemTime");
225 
226  PARSE_ASSERT(bytes_decoded == 52, std::string("ResultPortParser::decodeResultPortHeader(): ") + std::to_string(bytes_decoded) + " bytes decoded, expected 52 byte");
227  return bytes_decoded;
228 }
229 
230 /*
231  * Decodes the payload of a result port telegram from binary data.
232  * @param[in] binary_data binary data, at least start_byte+52 byte binary result port telegram payload
233  * @param[in] start_byte first byte to start decoding in binary_data
234  * @param[out] telegram_payload decoded result port telegram payload
235  * @return number of bytes decoded
236  * @throws std::invalid_argument in case of parse errors
237  */
238 size_t sick_scan::ResultPortParser::decodeResultPortPayload(const std::vector<uint8_t> & binary_data, size_t start_byte, sick_scan::SickLocResultPortPayloadMsg & telegram_payload)
239 {
240  size_t bytes_decoded = 0;
241 
242  // Decode ErrorCode: ErrorCode 0: OK, ErrorCode 1: UNKNOWNERROR. Size: UInt16 = 2 byte
243  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.errorcode, "Payload.ErrorCode", m_little_endian_payload);
244 
245  // Decode ScanCounter: Counter of related scan data. Size: UInt32 = 4 byte
246  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.scancounter, "Payload.ScanCounter", m_little_endian_payload);
247 
248  // Decode Timestamp: Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte
249  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.timestamp, "Payload.Timestamp", m_little_endian_payload);
250 
251  // Decode PoseX: Position X of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte
252  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.posex, "Payload.PoseX", m_little_endian_payload);
253 
254  // Decode PoseY: Position Y of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte
255  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.posey, "Payload.PoseY", m_little_endian_payload);
256 
257  // Decode PoseYaw: Orientation (yaw) of the vehicle on the map [mdeg] Size: Int32 = 4 byte
258  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.poseyaw, "Payload.PoseYaw", m_little_endian_payload);
259 
260  // Decode Reserved1: Reserved. Size: UInt32 = 4 byte
261  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.reserved1, "Payload.Reserved1", m_little_endian_payload);
262 
263  // Decode Reserved2: Reserved. Size: Int32 = 4 byte
264  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.reserved2, "Payload.Reserved2", m_little_endian_payload);
265 
266  // Decode Quality: Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality. Size: UInt8 = 1 byte
267  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.quality, "Payload.Quality", m_little_endian_payload);
268  PARSE_ASSERT(telegram_payload.quality >= 0 && telegram_payload.quality <= 100, std::string("ResultPortParser::decodeResultPortPayload(): invalid Payload.Quality ") + std::to_string(telegram_payload.quality));
269 
270  // Decode OutliersRatio: Ratio of beams that cannot be assigned to the current reference map [%]. Size: UInt8 = 1 byte
271  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.outliersratio, "Payload.OutliersRatio", m_little_endian_payload);
272  PARSE_ASSERT(telegram_payload.outliersratio >= 0 && telegram_payload.outliersratio <= 100, std::string("ResultPortParser::decodeResultPortPayload(): invalid Payload.OutliersRatio ") + std::to_string(telegram_payload.outliersratio));
273 
274  // Decode CovarianceX: Covariance c1 of the pose X [mm^2]. Size: Int32 = 4 byte
275  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.covariancex, "Payload.CovarianceX", m_little_endian_payload);
276 
277  // Decode CovarianceY: Covariance c5 of the pose Y [mm^2]. Size: Int32 = 4 byte
278  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.covariancey, "Payload.", m_little_endian_payload);
279 
280  // Decode CovarianceYaw: Covariance c9 of the pose Yaw [mdeg^2]. Size: Int32 = 4 byte
281  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.covarianceyaw, "Payload.CovarianceYaw", m_little_endian_payload);
282 
283  // Decode Reserved3: Reserved. Size: UInt64 = 8 byte
284  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.reserved3, "Payload.Reserved3", m_little_endian_payload);
285 
286  PARSE_ASSERT(bytes_decoded == 52, std::string("ResultPortParser::decodeResultPortPayload(): ") + std::to_string(bytes_decoded) + " bytes decoded, expected 52 byte");
287  return bytes_decoded;
288 }
289 
290 /*
291  * Decodes the trailer of a result port telegram from binary data.
292  * @param[in] binary_data binary data, at least start_byte+2 byte binary result port telegram trailer
293  * @param[in] start_byte first byte to start decoding in binary_data
294  * @param[out] telegram_trailer decoded result port telegram trailer
295  * @return number of bytes decoded
296  * @throws std::invalid_argument in case of parse errors
297  */
298 size_t sick_scan::ResultPortParser::decodeResultPortTrailer(const std::vector<uint8_t> & binary_data, size_t start_byte, sick_scan::SickLocResultPortCrcMsg & telegram_trailer)
299 {
300  size_t bytes_decoded = 0;
301 
302  // Decode Checksum: CRC16-CCITT over length of header (52 bytes) and payload (52 bytes) without 2 bytes of this trailer. Size: UInt16 = 2 byte
303  bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_trailer.checksum, "Payload.Checksum");
304 
305  PARSE_ASSERT(bytes_decoded == 2, std::string("ResultPortParser::decodeResultPortTrailer(): ") + std::to_string(bytes_decoded) + " bytes decoded, expected 2 byte");
306  return bytes_decoded;
307 }
308 
309 /*
310  * Decodes a result port telegram from binary data.
311  * @param[in] binary_data binary data (106 byte binary result port telegram), f.e. { 0x53, 0x49, 0x43, 0x4B, 0x00, ... }
312  * @return true if binary_data successfully decode, false otherwise.
313  */
314 bool sick_scan::ResultPortParser::decode(const std::vector<uint8_t> & binary_data)
315 {
316  try
317  {
318  size_t bytes_decoded = 0;
319  PARSE_ASSERT(binary_data.size() >= 106, std::string("ResultPortParser::decode(): ") + std::to_string(binary_data.size()) + " byte binary data, expected 106 byte result port telegram");
320  m_result_port_telegram.header.stamp = ROS::now();
321  m_result_port_telegram.header.frame_id = m_publish_frame_id;
322 
323  // Decode result port header
324  bytes_decoded += decodeResultPortHeader(binary_data, bytes_decoded, m_result_port_telegram.telegram_header);
325 
326  // Decode result port payload
327  bytes_decoded += decodeResultPortPayload(binary_data, bytes_decoded, m_result_port_telegram.telegram_payload);
328 
329  // Decode result port crc
330  bytes_decoded += decodeResultPortTrailer(binary_data, bytes_decoded, m_result_port_telegram.telegram_trailer);
331  PARSE_ASSERT(bytes_decoded == m_result_port_telegram.telegram_header.length, std::string("ResultPortParser::decode(): ") + std::to_string(bytes_decoded) + " bytes decoded, expected " + std::to_string(m_result_port_telegram.telegram_header.length) + " byte (telegram_header.Length))");
332 
333  // Verify Checksum := CRC16-CCITT over length of header (52 bytes) and payload (52 bytes) without 2 bytes of this trailer. Size: UInt16 = 2 byte
334  // Checksum details (See chapter 5.9 "About result port telegrams" of the operation manual for further details):
335  // Width: 16 bits, Initial value = 0xFFFF, Truncated polynomial: 0x1021 CRC polynomials with orders of x16 + x12 + x5 + 1 (counted without the leading '1' bit)
336  uint16_t checksum = computeChecksum(binary_data);
337  PARSE_ASSERT(checksum == m_result_port_telegram.telegram_trailer.checksum, std::string("ResultPortParser::decode(): invalid checksum ") + std::to_string(m_result_port_telegram.telegram_trailer.checksum) + " decoded, expected checksum " + std::to_string(checksum));
338 
339  return true;
340  }
341  catch(const std::invalid_argument & exc)
342  {
343  ROS_ERROR_STREAM("## ERROR in sick_scan::ResultPortParser::decode(): exception " << exc.what());
344  }
345  return false;
346 }
347 
348 /*
349  * Encodes a value to binary data.
350  * @param[in] value source
351  * @param[out] binary_data binary data (destination buffer)
352  * @param[in] little_endian true: binary_data encoded in little endian format, false (default): binary_data encoded in big endian format
353  */
354 template<typename T> void sick_scan::ResultPortParser::encodePushValue(T value, std::vector<uint8_t> & binary_data, bool little_endian)
355 {
356  if(little_endian) // Little endian: LSB first, MSB last
357  {
358  for (size_t n = 0; n < sizeof(value); n++)
359  {
360  binary_data.push_back(value & 0xFF);
361  value = (value >> 8);
362  }
363  }
364  else // Big endian: MSB first, LSB last
365  {
366  for (int n = sizeof(value) - 1; n >= 0; n--)
367  {
368  binary_data.push_back((value >> (8*n)) & 0xFF);
369  }
370  }
371 }
372 
373 /*
374  * Encodes the header of the result port telegram and append its binary data to binary_data (destination).
375  * @param[in] telegram_header header of result port telegram
376  * @param[out] binary_data destination buffer
377  */
378 void sick_scan::ResultPortParser::encodeResultPortHeader(const sick_scan::SickLocResultPortHeaderMsg & telegram_header, std::vector<uint8_t> & binary_data)
379 {
380  encodePushValue(telegram_header.magicword, binary_data);
381  encodePushValue(telegram_header.length, binary_data);
382  encodePushValue(telegram_header.payloadtype, binary_data);
383  encodePushValue(telegram_header.payloadversion, binary_data);
384  encodePushValue(telegram_header.ordernumber, binary_data);
385  encodePushValue(telegram_header.serialnumber, binary_data);
386  for (size_t n = 0; n < telegram_header.fw_version.size(); n++)
387  binary_data.push_back(telegram_header.fw_version[n]);
388  encodePushValue(telegram_header.telegramcounter, binary_data);
389  encodePushValue(telegram_header.systemtime, binary_data);
390 }
391 
392 /*
393  * Encodes the payload of the result port telegram and append its binary data to binary_data (destination).
394  * @param[in] telegram_payload payload of result port telegram
395  * @param[out] binary_data destination buffer
396  */
397 void sick_scan::ResultPortParser::encodeResultPortPayload(const sick_scan::SickLocResultPortPayloadMsg & telegram_payload, std::vector<uint8_t> & binary_data)
398 {
399  encodePushValue(telegram_payload.errorcode, binary_data, m_little_endian_payload);
400  encodePushValue(telegram_payload.scancounter, binary_data, m_little_endian_payload);
401  encodePushValue(telegram_payload.timestamp, binary_data, m_little_endian_payload);
402  encodePushValue(telegram_payload.posex, binary_data, m_little_endian_payload);
403  encodePushValue(telegram_payload.posey, binary_data, m_little_endian_payload);
404  encodePushValue(telegram_payload.poseyaw, binary_data, m_little_endian_payload);
405  encodePushValue(telegram_payload.reserved1, binary_data, m_little_endian_payload);
406  encodePushValue(telegram_payload.reserved2, binary_data, m_little_endian_payload);
407  encodePushValue(telegram_payload.quality, binary_data, m_little_endian_payload);
408  encodePushValue(telegram_payload.outliersratio, binary_data, m_little_endian_payload);
409  encodePushValue(telegram_payload.covariancex, binary_data, m_little_endian_payload);
410  encodePushValue(telegram_payload.covariancey, binary_data, m_little_endian_payload);
411  encodePushValue(telegram_payload.covarianceyaw, binary_data, m_little_endian_payload);
412  encodePushValue(telegram_payload.reserved3, binary_data, m_little_endian_payload);
413 }
414 
415 /*
416  * Encodes the checksum (trailer) of the result port telegram and append its binary data to binary_data (destination).
417  * @param[in] checksum checksum (trailer) of result port telegram
418  * @param[out] binary_data destination buffer
419  */
420 void sick_scan::ResultPortParser::encodeResultPortTrailer(uint16_t checksum, std::vector<uint8_t> & binary_data)
421 {
422  encodePushValue(checksum, binary_data);
423 }
424 
425 /*
426  * Encodes the result port telegram and returns its binary data.
427  * @return binary data (106 byte binary result port telegram), f.e. { 0x53, 0x49, 0x43, 0x4B, 0x00, ... }
428  */
429 std::vector<uint8_t> sick_scan::ResultPortParser::encode(void)
430 {
431  std::vector<uint8_t> binary_data;
432  binary_data.reserve(106);
433  m_little_endian_payload = isLittleEndianPayload(m_result_port_telegram.telegram_header.payloadtype);
434  encodeResultPortHeader(m_result_port_telegram.telegram_header, binary_data);
435  encodeResultPortPayload(m_result_port_telegram.telegram_payload, binary_data);
436  uint16_t checksum = computeChecksum(binary_data, false);
437  m_result_port_telegram.telegram_trailer.checksum = checksum;
438  encodeResultPortTrailer(checksum, binary_data);
439  return binary_data;
440 }
sick_scan::ResultPortParser::copyBytesToArray
size_t copyBytesToArray(const std::vector< uint8_t > &binary_data, size_t start_byte, std::vector< T > &dst_array, const std::string &info="")
Definition: result_port_parser.cpp:127
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
sick_scan::ResultPortParser::encode
virtual std::vector< uint8_t > encode(void)
Definition: result_port_parser.cpp:429
sick_scan::ResultPortParser::isLittleEndianPayload
virtual bool isLittleEndianPayload(uint16_t payload_type)
Definition: result_port_parser.cpp:176
sick_scan::ResultPortParser::decodeResultPortPayload
virtual size_t decodeResultPortPayload(const std::vector< uint8_t > &binary_data, size_t start_byte, sick_scan::SickLocResultPortPayloadMsg &telegram_payload)
Definition: result_port_parser.cpp:238
crc16ccitt_false_bit
unsigned crc16ccitt_false_bit(unsigned crc, void const *mem, size_t len)
Definition: crc16ccitt_false.cpp:16
sick_scan::ResultPortParser::encodePushValue
void encodePushValue(T value, std::vector< uint8_t > &binary_data, bool little_endian=false)
Definition: result_port_parser.cpp:354
sick_scan::ResultPortParser::computeChecksum
virtual uint16_t computeChecksum(const std::vector< uint8_t > &binary_data, bool binary_data_with_trailer=true)
Definition: result_port_parser.cpp:159
ROS::now
ROS::Time now(void)
Definition: ros_wrapper.cpp:116
sick_scan::ResultPortParser::decodeResultPortTrailer
virtual size_t decodeResultPortTrailer(const std::vector< uint8_t > &binary_data, size_t start_byte, sick_scan::SickLocResultPortCrcMsg &telegram_trailer)
Definition: result_port_parser.cpp:298
sick_scan::ResultPortParser::decodeResultPortHeader
virtual size_t decodeResultPortHeader(const std::vector< uint8_t > &binary_data, size_t start_byte, sick_scan::SickLocResultPortHeaderMsg &telegram_header)
Definition: result_port_parser.cpp:189
sick_scan::ResultPortParser::decode
virtual bool decode(const std::vector< uint8_t > &binary_data)
Definition: result_port_parser.cpp:314
crc16ccitt_false_byte
unsigned crc16ccitt_false_byte(unsigned crc, void const *mem, size_t len)
Definition: crc16ccitt_false.cpp:278
result_port_parser.h
sick_scan::ResultPortParser::encodeResultPortHeader
virtual void encodeResultPortHeader(const sick_scan::SickLocResultPortHeaderMsg &telegram_header, std::vector< uint8_t > &binary_data)
Definition: result_port_parser.cpp:378
sick_scan::ResultPortParser::parseAssert
static void parseAssert(bool assertion, const std::string &assertion_msg, const std::string &info, const std::string &file, int line)
Definition: result_port_parser.cpp:77
crc16ccitt_false_word
unsigned crc16ccitt_false_word(unsigned crc, void const *mem, size_t len)
Definition: crc16ccitt_false.cpp:297
sick_scan::ResultPortParser::ResultPortParser
ResultPortParser(const std::string &frame_id="")
Definition: result_port_parser.cpp:70
sick_scan::ResultPortParser::encodeResultPortTrailer
virtual void encodeResultPortTrailer(uint16_t checksum, std::vector< uint8_t > &binary_data)
Definition: result_port_parser.cpp:420
sick_scan::ResultPortParser::copyBytesToValue
size_t copyBytesToValue(const std::vector< uint8_t > &binary_data, size_t start_byte, T &value, const std::string &info="", bool little_endian=false)
Definition: result_port_parser.cpp:97
PARSE_ASSERT
#define PARSE_ASSERT(assertion, info)
Definition: result_port_parser.cpp:63
ros_wrapper.h
sick_scan::ResultPortParser::encodeResultPortPayload
virtual void encodeResultPortPayload(const sick_scan::SickLocResultPortPayloadMsg &telegram_payload, std::vector< uint8_t > &binary_data)
Definition: result_port_parser.cpp:397


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Thu Sep 8 2022 02:30:19