sick_scan_messages.cpp
Go to the documentation of this file.
1 /*
2  * @brief Implementation of ROS messages for sick_scan
3  *
4  * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim
5  * Copyright (C) 2021, SICK AG, Waldkirch
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above copyright
14  * notice, this list of conditions and the following disclaimer in the
15  * documentation and/or other materials provided with the distribution.
16  * * Neither the name of Osnabrück University nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
24  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
25  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
26  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
27  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
28  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  * Created on: 12.01.2021
33  *
34  * Authors:
35  * Michael Lehning <michael.lehning@lehning.de>
36  *
37  * Based on the TiM communication example by SICK AG.
38  *
39  */
40 
42 
43 typedef uint8_t* byte_ptr;
44 template<typename T> static bool readBinaryBuffer(byte_ptr& buffer, int & bufferlen, T& value)
45 {
46  if(sizeof(value) > bufferlen)
47  {
48  ROS_ERROR_STREAM("## ERROR SickScanMessages::readBinaryBuffer(): bufferlen=" << bufferlen << " byte, " << sizeof(value) << " byte required.");
49  return false;
50  }
51  memcpy(&value, buffer, sizeof(value));
52  swap_endian((unsigned char *) &value, sizeof(value));
53  buffer += sizeof(value);
54  bufferlen -= (int)sizeof(value);
55  return true;
56 }
57 
59 {
60 }
61 
63 {
64 }
65 
66 /*
67  * @brief parses and converts a lidar LIDoutputstate message to a ros LIDoutputstate message
68  *
69  * Example LIDoutputstate message from 20210111_sick_tim781s_mon_elephant.pcapng.json:
70  * "tcp.description": ".......4sSN LIDoutputstate .................................E",
71  * "tcp.payload": "02:02:02:02:00:00:00:34:73:53:4e:20:4c:49:44:6f:75:74:70:75:74:73:74:61:74:65:20:00:00:00:00:00:00:01:00:00:01:26:00:00:00:01:22:00:00:00:00:00:01:00:00:00:04:02:00:00:00:00:00:00:45"
72  * 33 byte LIDoutputstate payload after "sSN LIDoutputstate ": "00:00:00:00:00:00:01:00:00:01:26:00:00:00:01:22:00:00:00:00:00:01:00:00:00:04:02:00:00:00:00:00:00"
73  *
74  * @param[in] timeStamp timestamp on receiving the lidar message
75  * @param[in] receiveBuffer byte array of lidar message
76  * @param[in] receiveLength size of lidar message in byte
77  * @param[in] useBinaryProtocol binary lidar message (true, Cola-B) or ascii lidar message (false, Cola-A)
78  * @param[in] frame_id frame id of output message
79  * @param[out] output_msg converted output message
80  *
81  * @return true on success, false on error
82  */
83 bool sick_scan::SickScanMessages::parseLIDoutputstateMsg(const ros::Time& timeStamp, uint8_t* receiveBuffer, int receiveLength, bool useBinaryProtocol, const std::string& frame_id, sick_scan::LIDoutputstateMsg& output_msg)
84 {
85  if(useBinaryProtocol)
86  {
87  // parse and convert LIDoutputstate message
88  int dbg_telegram_length = receiveLength;
89  std::string dbg_telegram_hex_copy = DataDumper::binDataToAsciiString(receiveBuffer, receiveLength);
90  int msg_start_idx = 27; // 4 byte STX + 4 byte payload_length + 19 byte "sSN LIDoutputstate " = 27 byte
91  int msg_parameter_length = receiveLength - msg_start_idx - 1; // start bytes + 1 byte CRC
92  if(msg_parameter_length < 8) // at least 6 byte (version+system) + 2 byte (timestamp status)
93  {
94  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): received " << receiveLength << " byte, expected at least 8 byte (" << __FILE__ << ":" << __LINE__ << ")");
95  return false;
96  }
97  // The LIDoutputstate parameter may have 11 byte timestamp or not, i.e. the parameter format is either:
98  // a) msg_parameter_length := (6 byte version+system) + number_of_output_states * (5 byte state+counter per output_state) + (2 byte 0x0000 no timestamp), or
99  // b) msg_parameter_length := (6 byte version+system) + number_of_output_states * (5 byte state+counter per output_state) + (2 byte 0x0001 timestamp available) + (11 byte timestamp)
100  bool parameter_format_ok = false;
101  // Check parameter format a (default, TiM781S: no timestamp)
102  uint16_t timestamp_status = (receiveBuffer[receiveLength - 3] << 8) | (receiveBuffer[receiveLength - 2]); // 2 byte before receiveBuffer[receiveLength-1] = CRC
103  int number_of_output_states = (msg_parameter_length - 6 - 2) / 5; // each output state has 1 byte state enum + 4 byte counter = 5 byte
104  if(msg_parameter_length == 6 + number_of_output_states * 5 + 2 && timestamp_status == 0)
105  {
106  parameter_format_ok = true;
107  ROS_DEBUG_STREAM("SickScanMessages::parseLIDoutputstateMsg(): " << number_of_output_states << " output states, no timestamp" );
108  }
109  else // Check parameter format b with timestamp
110  {
111  timestamp_status = (receiveBuffer[receiveLength - 14] << 8) | (receiveBuffer[receiveLength - 13]); // 2 byte before receiveBuffer[receiveLength-12] = (11 byte timestamp) + (1 byte CRC)
112  number_of_output_states = (msg_parameter_length - 6 - 2 - 11) / 5; // each output state has 1 byte state enum + 4 byte counter = 5 byte
113  if(msg_parameter_length == 6 + number_of_output_states * 5 + 2 + 11 && timestamp_status > 0)
114  {
115  parameter_format_ok = true;
116  ROS_DEBUG_STREAM("SickScanMessages::parseLIDoutputstateMsg(): " << number_of_output_states << " output states and timestamp" );
117  }
118  }
119  if(!parameter_format_ok)
120  {
121  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): received " << receiveLength << " byte with invalid format (" << __FILE__ << ":" << __LINE__ << "): "
122  << DataDumper::binDataToAsciiString(receiveBuffer, receiveLength));
123  return false;
124  }
125  // Read 2 byte version + 2 byte system status
126  receiveBuffer += msg_start_idx;
127  receiveLength -= msg_start_idx;
128  if( !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.version_number)
129  || !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.system_counter))
130  {
131  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing version_number and system_counter (" << __FILE__ << ":" << __LINE__ << ")");
132  return false;
133  }
134  // Read N output states
135  output_msg.output_state.reserve(number_of_output_states);
136  output_msg.output_count.reserve(number_of_output_states);
137  for(int state_cnt = 0; state_cnt < number_of_output_states; state_cnt++)
138  {
139  uint8_t output_state;
140  uint32_t output_count;
141  if( !readBinaryBuffer(receiveBuffer, receiveLength, output_state)
142  || !readBinaryBuffer(receiveBuffer, receiveLength, output_count))
143  {
144  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing version_number and system_counter (" << __FILE__ << ":" << __LINE__ << ")");
145  return false;
146  }
147  if(output_state == 0 || output_state == 1) // 0: not active, 1: active, 2: not used
148  {
149  output_msg.output_state.push_back(output_state);
150  output_msg.output_count.push_back(output_count);
151  }
152  }
153  // Read timestamp state
154  if( !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.time_state))
155  {
156  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing time_state (" << __FILE__ << ":" << __LINE__ << ")");
157  return false;
158  }
159  if(output_msg.time_state != timestamp_status) // time_state and previously parsed timestamp_status must be identical
160  {
161  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): time_state mismatch, received " << (int)output_msg.time_state << ", expected " << (int)timestamp_status << " (" << __FILE__ << ":" << __LINE__ << ")");
162  return false;
163 
164  }
165  // Read optional timestamp
166  if(timestamp_status > 0)
167  {
168  if(!readBinaryBuffer(receiveBuffer, receiveLength, output_msg.year)
169  || !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.month)
170  || !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.day)
171  || !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.hour)
172  || !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.minute)
173  || !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.second)
174  || !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.microsecond))
175  {
176  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): error parsing timestamp (" << __FILE__ << ":" << __LINE__ << ")");
177  return false;
178  }
179  }
180  output_msg.header.stamp = timeStamp;
181  output_msg.header.seq = 0;
182  output_msg.header.frame_id = frame_id;
183 
184  // Debug messages
185  std::stringstream state_str;
186  assert(output_msg.output_state.size() == output_msg.output_state.size());
187  state_str << number_of_output_states << " output states received" << (timestamp_status?" with":", no") << " timestamp\n";
188  for(int state_cnt = 0; state_cnt < output_msg.output_count.size(); state_cnt++)
189  state_str << "state[" << state_cnt << "]: " << (uint32_t)output_msg.output_state[state_cnt] << ", count=" << (uint32_t)output_msg.output_count[state_cnt] << "\n";
190  ROS_DEBUG_STREAM("SickScanMessages::parseLIDoutputstateMsg():\n"
191  << dbg_telegram_length << " byte telegram: " << dbg_telegram_hex_copy << "\n"
192  << "version_number: " << (uint32_t)output_msg.version_number << ", system_counter: " << (uint32_t)output_msg.system_counter << "\n"
193  << state_str.str()
194  << "time state: " << (uint32_t)output_msg.time_state
195  << ", date: " << std::setfill('0') << std::setw(4) << (uint32_t)output_msg.year << "-" << std::setfill('0') << std::setw(2) << (uint32_t)output_msg.month << "-" << std::setfill('0') << std::setw(2) << (uint32_t)output_msg.day
196  << ", time: " << std::setfill('0') << std::setw(2) << (uint32_t)output_msg.hour << ":" << std::setfill('0') << std::setw(2) << (uint32_t)output_msg.minute << ":" << std::setfill('0') << std::setw(2) << (uint32_t)output_msg.second << "." << std::setfill('0') << std::setw(6) << (uint32_t)output_msg.microsecond);
197 
198  return true;
199  }
200  else
201  {
202  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg not implemented for Cola-A messages(" << __FILE__ << ":" << __LINE__ << ")");
203  }
204  return false;
205 }
206 
207 /*
208  * @brief parses and converts a lidar LFErec message to a ros LFErec message
209  *
210  * Example LIDoutputstate message from 20210111_sick_tim781s_lferec_elephant.pcapng.json:
211  * "tcp.description": "........sSN LFErec ........................0..............3...........................0..............4....P......................0..............35.....",
212  * "tcp.payload": "02:02:02:02:00:00:00:8e:73:53:4e:20:4c:46:45:72:65:63:20:00:03:00:01:01:00:00:00:00:3f:80:00:00:00:00:00:00:00:00:0d:05:ff:f9:22:30:00:00:00:00:00:00:00:00:01:07:b2:01:01:08:33:2e:00:09:ac:90:00:01:02:00:00:00:00:3f:80:00:00:00:00:00:00:00:00:0d:05:ff:f9:22:30:01:00:00:00:00:00:00:00:01:07:b2:01:01:08:34:02:00:06:9f:50:00:01:03:00:00:00:00:3f:80:00:00:00:00:00:00:00:00:0d:05:ff:f9:22:30:01:00:00:00:00:00:00:00:01:07:b2:01:01:08:33:35:00:09:ac:90:ac"
213  * 131 byte LFErec payload after "sSN LFErec ": "00:03:00:01:01:00:00:00:00:3f:80:00:00:00:00:00:00:00:00:0d:05:ff:f9:22:30:00:00:00:00:00:00:00:00:01:07:b2:01:01:08:33:2e:00:09:ac:90:00:01:02:00:00:00:00:3f:80:00:00:00:00:00:00:00:00:0d:05:ff:f9:22:30:01:00:00:00:00:00:00:00:01:07:b2:01:01:08:34:02:00:06:9f:50:00:01:03:00:00:00:00:3f:80:00:00:00:00:00:00:00:00:0d:05:ff:f9:22:30:01:00:00:00:00:00:00:00:01:07:b2:01:01:08:33:35:00:09:ac:90"
214  * LFErec payload starts with 16 byte fields_number "00:03" -> 3 fields, each field has (131-2)/3 = 43 byte
215  * Each field contains a field_index (1,2,3) and field_result (0: invalid/incorrect, 1: free/clear, 2: infringed)
216  * Note: field indices are sorted in reverse order, i.e. with 2 fields configured, we have typically
217  * output_msg.fields[0].field_index = 1, output_msg.fields[0].field_result_mrs = 0 (invalid)
218  * output_msg.fields[1].field_index = 2, output_msg.fields[1].field_result_mrs = 1 or 2 (clear=1 or infringed=2)
219  * output_msg.fields[2].field_index = 3, output_msg.fields[2].field_result_mrs = 1 or 2 (clear=1 or infringed=2)
220  *
221  * @param[in] timeStamp timestamp on receiving the lidar message
222  * @param[in] receiveBuffer byte array of lidar message
223  * @param[in] receiveLength size of lidar message in byte
224  * @param[in] useBinaryProtocol binary lidar message (true, Cola-B) or ascii lidar message (false, Cola-A)
225  * @param[in] eval_field_logic USE_EVAL_FIELD_LMS5XX_LOGIC or USE_EVAL_FIELD_TIM7XX_LOGIC
226  * @param[in] frame_id frame id of output message
227  * @param[out] output_msg converted output message
228  *
229  * @return true on success, false on error
230  */
231 bool sick_scan::SickScanMessages::parseLFErecMsg(const ros::Time& timeStamp, uint8_t* receiveBuffer, int receiveLength, bool useBinaryProtocol, EVAL_FIELD_SUPPORT eval_field_logic, const std::string& frame_id, sick_scan::LFErecMsg& output_msg)
232 {
233  if(useBinaryProtocol)
234  {
235  uint8_t* msg_receiveBuffer = receiveBuffer;
236  int msg_receiveLength = receiveLength;
237 
238  // parse and convert LFErec messages, see https://github.com/SICKAG/libsick_ldmrs/blob/master/src/sopas/LdmrsSopasLayer.cpp lines 1414 ff.
239  int msg_start_idx = 19; // 4 byte STX + 4 byte payload_length + 11 byte "sSN LFErec " = 19 byte
240  receiveBuffer += msg_start_idx;
241  receiveLength -= msg_start_idx;
242 
243  output_msg.fields_number = 0;
244  if(!readBinaryBuffer(receiveBuffer, receiveLength, output_msg.fields_number) || output_msg.fields_number <= 0)
245  {
246  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): parse error, fields number = " << output_msg.fields_number<< " should be greater 0 (" << __FILE__ << ":" << __LINE__ << ")");
247  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): " << msg_receiveLength << " byte received: " << DataDumper::binDataToAsciiString(msg_receiveBuffer, msg_receiveLength));
248  return false;
249  }
250  output_msg.fields.reserve(output_msg.fields_number);
251  for(int field_idx = 0; field_idx < output_msg.fields_number; field_idx++)
252  {
253  sick_scan::LFErecFieldMsg field_msg;
254  if( !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.version_number)
255  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.field_index)
256  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.sys_count)
257  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.dist_scale_factor)
258  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.dist_scale_offset)
259  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.angle_scale_factor)
260  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.angle_scale_offset)
261  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.field_result_mrs))
262  {
263  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): parse error field " << field_idx << " (" << __FILE__ << ":" << __LINE__ << ")");
264  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): " << msg_receiveLength << " byte received: " << DataDumper::binDataToAsciiString(msg_receiveBuffer, msg_receiveLength));
265  return false;
266  }
267  uint16_t dummy_byte[3] = {0};
268  if(!readBinaryBuffer(receiveBuffer, receiveLength, dummy_byte[0])
269  || !readBinaryBuffer(receiveBuffer, receiveLength, dummy_byte[1])
270  || !readBinaryBuffer(receiveBuffer, receiveLength, dummy_byte[2]))
271  {
272  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): parse error field " << field_idx << " (" << __FILE__ << ":" << __LINE__ << ")");
273  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): " << msg_receiveLength << " byte received: " << DataDumper::binDataToAsciiString(msg_receiveBuffer, msg_receiveLength));
274  return false;
275  }
276  int additional_dummy_byte_length = 0;
277  // if(eval_field_logic == USE_EVAL_FIELD_LMS5XX_LOGIC && field_msg.field_result_mrs == 2)
278  // additional_dummy_byte_length = (21 - 6);
279  if(eval_field_logic == USE_EVAL_FIELD_LMS5XX_LOGIC && (dummy_byte[0] != 0 || dummy_byte[1] != 0 || dummy_byte[2] != 0))
280  additional_dummy_byte_length = (21 - 6);
281  receiveBuffer += additional_dummy_byte_length;
282  receiveLength -= additional_dummy_byte_length;
283  if(receiveLength < 0)
284  {
285  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): parse error field " << field_idx << " (" << __FILE__ << ":" << __LINE__ << ")");
286  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): " << msg_receiveLength << " byte received: " << DataDumper::binDataToAsciiString(msg_receiveBuffer, msg_receiveLength));
287  return false;
288  }
289  if(!readBinaryBuffer(receiveBuffer, receiveLength, field_msg.time_state))
290  {
291  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): parse error field " << field_idx << " (" << __FILE__ << ":" << __LINE__ << ")");
292  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): " << msg_receiveLength << " byte received: " << DataDumper::binDataToAsciiString(msg_receiveBuffer, msg_receiveLength));
293  return false;
294  }
295  if(field_msg.time_state)
296  {
297  if( !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.year)
298  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.month)
299  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.day)
300  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.hour)
301  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.minute)
302  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.second)
303  || !readBinaryBuffer(receiveBuffer, receiveLength, field_msg.microsecond))
304  {
305  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): parse error field " << field_idx << " (" << __FILE__ << ":" << __LINE__ << ")");
306  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): " << msg_receiveLength << " byte received: " << DataDumper::binDataToAsciiString(msg_receiveBuffer, msg_receiveLength));
307  return false;
308  }
309  }
310  else
311  {
312  field_msg.year = 0;
313  field_msg.month = 0;
314  field_msg.day = 0;
315  field_msg.hour = 0;
316  field_msg.minute = 0;
317  field_msg.second = 0;
318  field_msg.microsecond = 0;
319  }
320  output_msg.fields.push_back(field_msg);
321  }
322  if(output_msg.fields.size() != output_msg.fields_number)
323  {
324  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): parse error, fields number = " << output_msg.fields_number << ", but " << output_msg.fields.size() << " fields parsed (" << __FILE__ << ":" << __LINE__ << ")");
325  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): " << msg_receiveLength << " byte received: " << DataDumper::binDataToAsciiString(msg_receiveBuffer, msg_receiveLength));
326  return false;
327  }
328  if(receiveLength != 1) // 1 byte CRC still in receiveBuffer
329  {
330  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): parse error, " << receiveLength << " bytes in buffer after decoding all fields, should be 0 (" << __FILE__ << ":" << __LINE__ << ")");
331  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg(): " << msg_receiveLength << " byte received: " << DataDumper::binDataToAsciiString(msg_receiveBuffer, msg_receiveLength));
332  return false;
333  }
334 
335  output_msg.header.stamp = timeStamp;
336  output_msg.header.seq = 0;
337  output_msg.header.frame_id = frame_id;
338 
339  std::stringstream fields_str;
340  for(int field_idx = 0; field_idx < output_msg.fields.size(); field_idx++)
341  {
342  sick_scan::LFErecFieldMsg& field_msg = output_msg.fields[field_idx];
343  if(field_idx > 0)
344  fields_str << "\n";
345  fields_str << "field[" << field_idx << "]: idx=" << (uint32_t)field_msg.field_index
346  << ", dist_scale_factor=" << field_msg.dist_scale_factor
347  << ", dist_scale_offset=" << field_msg.dist_scale_offset
348  << ", angle_scale_factor=" << field_msg.angle_scale_factor
349  << ", angle_scale_offset=" << field_msg.angle_scale_offset
350  << ", field_result_mrs=" << (uint32_t)field_msg.field_result_mrs
351  << ", time state: " << (uint32_t)field_msg.time_state
352  << ", date: " << std::setfill('0') << std::setw(4) << (uint32_t)field_msg.year << "-" << std::setfill('0') << std::setw(2) << (uint32_t)field_msg.month << "-" << std::setfill('0') << std::setw(2) << (uint32_t)field_msg.day
353  << ", time: " << std::setfill('0') << std::setw(2) << (uint32_t)field_msg.hour << ":" << std::setfill('0') << std::setw(2) << (uint32_t)field_msg.minute << ":" << std::setfill('0') << std::setw(2) << (uint32_t)field_msg.second
354  << "." << std::setfill('0') << std::setw(6) << (uint32_t)field_msg.microsecond;
355  }
356  ROS_DEBUG_STREAM("SickScanMessages::parseLFErecMsg(): LFErec with " << output_msg.fields.size() << " fields:\n" << fields_str.str());
357 
358  return true;
359  }
360  else
361  {
362  ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLFErecMsg not implemented for Cola-A messages (" << __FILE__ << ":" << __LINE__ << ")");
363  }
364  return false;
365 }
sick_scan_messages.h
sick_scan::SickScanMessages::parseLIDoutputstateMsg
static bool parseLIDoutputstateMsg(const ros::Time &timeStamp, uint8_t *receiveBuffer, int receiveLength, bool useBinaryProtocol, const std::string &frame_id, sick_scan::LIDoutputstateMsg &output_msg)
Definition: sick_scan_messages.cpp:83
sick_scan::USE_EVAL_FIELD_LMS5XX_LOGIC
@ USE_EVAL_FIELD_LMS5XX_LOGIC
Definition: sick_generic_parser.h:65
sick_scan::EVAL_FIELD_SUPPORT
EVAL_FIELD_SUPPORT
Definition: sick_generic_parser.h:61
swap_endian
void swap_endian(unsigned char *ptr, int numBytes)
Universal swapping function.
Definition: sick_scan_common.cpp:102
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
sick_scan::SickScanMessages::SickScanMessages
SickScanMessages(ros::NodeHandle *nh=0)
Definition: sick_scan_messages.cpp:58
readBinaryBuffer
static bool readBinaryBuffer(byte_ptr &buffer, int &bufferlen, T &value)
Definition: sick_scan_messages.cpp:44
byte_ptr
uint8_t * byte_ptr
Definition: sick_scan_messages.cpp:43
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
DataDumper::binDataToAsciiString
static std::string binDataToAsciiString(const uint8_t *binary_data, int length)
Definition: dataDumper.cpp:108
sick_scan::SickScanMessages::parseLFErecMsg
static bool parseLFErecMsg(const ros::Time &timeStamp, uint8_t *receiveBuffer, int receiveLength, bool useBinaryProtocol, EVAL_FIELD_SUPPORT eval_field_logic, const std::string &frame_id, sick_scan::LFErecMsg &output_msg)
Definition: sick_scan_messages.cpp:231
ros::Time
timeStamp
ros::Time const * timeStamp(const M &m)
sick_scan::SickScanMessages::~SickScanMessages
virtual ~SickScanMessages()
Definition: sick_scan_messages.cpp:62
ros::NodeHandle


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