sick_nav_scandata_parser.cpp
Go to the documentation of this file.
1 /*
2  * sick_nav_scandata_parser parses data telegrams from NAV-350.
3  *
4  * Copyright (C) 2023, Ing.-Buero Dr. Michael Lehning, Hildesheim
5  * Copyright (C) 2023, SICK AG, Waldkirch
6  * All rights reserved.
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 *
21 * All rights reserved.
22 *
23 * Redistribution and use in source and binary forms, with or without
24 * modification, are permitted provided that the following conditions are met:
25 *
26 * * Redistributions of source code must retain the above copyright
27 * notice, this list of conditions and the following disclaimer.
28 * * Redistributions in binary form must reproduce the above copyright
29 * notice, this list of conditions and the following disclaimer in the
30 * documentation and/or other materials provided with the distribution.
31 * * Neither the name of Osnabrueck University nor the names of its
32 * contributors may be used to endorse or promote products derived from
33 * this software without specific prior written permission.
34 * * Neither the name of SICK AG nor the names of its
35 * contributors may be used to endorse or promote products derived from
36 * this software without specific prior written permission
37 * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
38 * contributors may be used to endorse or promote products derived from
39 * this software without specific prior written permission
40 *
41 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
42 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
43 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
44 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
45 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
46 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
47 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
48 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
49 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
50 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
51 * POSSIBILITY OF SUCH DAMAGE.
52  *
53  * Authors:
54  * Michael Lehning <michael.lehning@lehning.de>
55  *
56  */
57 #include <fstream>
58 #include <string>
59 #include <sstream>
60 
61 #include "softwarePLL.h"
62 #include <sick_scan/sick_scan_common.h>
63 #include <sick_scan/sick_lmd_scandata_parser.h>
64 #include <sick_scan/sick_nav_scandata_parser.h>
65 
66 namespace sick_scan_xd
67 {
68  template<typename T> static void appendToBuffer(std::vector<uint8_t>& data_buffer, const T& value)
69  {
70  T dst_value = value;
71  swap_endian((unsigned char *) &dst_value, sizeof(dst_value));
72  size_t pos = data_buffer.size();
73  for (size_t n = 0; n < sizeof(dst_value); n++)
74  data_buffer.push_back(0);
75  memcpy(&data_buffer[pos], &dst_value, sizeof(dst_value));
76  }
77 
78  template<typename T> static bool readFromBuffer(const uint8_t* receiveBuffer, int& pos, int receiveBufferLength, T& value, const char* file, int line)
79  {
80  if(pos + sizeof(value) <= receiveBufferLength)
81  {
82  memcpy(&value, receiveBuffer + pos, sizeof(value));
83  swap_endian((unsigned char *) &value, sizeof(value));
84  pos += sizeof(value);
85  return true;
86  }
87  else
88  {
89  ROS_WARN_STREAM("readFromBuffer(): read pos = " << pos << " + sizeof(value) = " << sizeof(value) << " exceeds receiveBufferLength = " << receiveBufferLength << " (" << file << ":" << line << ")");
90  return false;
91  }
92  }
93 
95  static void writeNAV350BinaryPositionData(const NAV350mNPOSData& navdata, std::vector<uint8_t>& data_buffer)
96  {
97  data_buffer.clear();
98  uint32_t stx = 0x02020202;
99  uint32_t payload_size = 0;
100  appendToBuffer(data_buffer, stx);
101  appendToBuffer(data_buffer, payload_size);
102  std::string sopas_cmd = "sAN mNPOSGetData ";
103  std::copy(sopas_cmd.begin(), sopas_cmd.end(), std::back_inserter(data_buffer));
104  appendToBuffer(data_buffer, navdata.version);
105  appendToBuffer(data_buffer, navdata.errorCode);
106  appendToBuffer(data_buffer, navdata.wait);
107  appendToBuffer(data_buffer, navdata.mask);
108  appendToBuffer(data_buffer, navdata.poseDataValid);
109  if (navdata.poseDataValid > 0)
110  {
111  appendToBuffer(data_buffer, navdata.poseData.x);
112  appendToBuffer(data_buffer, navdata.poseData.y);
113  appendToBuffer(data_buffer, navdata.poseData.phi);
114  appendToBuffer(data_buffer, navdata.poseData.optPoseDataValid);
115  if (navdata.poseData.optPoseDataValid > 0)
116  {
117  appendToBuffer(data_buffer, navdata.poseData.optPoseData.outputMode);
118  appendToBuffer(data_buffer, navdata.poseData.optPoseData.timestamp);
119  appendToBuffer(data_buffer, navdata.poseData.optPoseData.meanDev);
120  appendToBuffer(data_buffer, navdata.poseData.optPoseData.navMode);
121  appendToBuffer(data_buffer, navdata.poseData.optPoseData.infoState);
122  appendToBuffer(data_buffer, navdata.poseData.optPoseData.quantUsedReflectors);
123  }
124  }
125  appendToBuffer(data_buffer, navdata.landmarkDataValid);
126  if (navdata.landmarkDataValid > 0)
127  {
128  appendToBuffer(data_buffer, navdata.landmarkData.landmarkFilter);
129  appendToBuffer(data_buffer, navdata.landmarkData.numReflectors);
130  for(int reflectorCnt = 0; reflectorCnt < navdata.landmarkData.numReflectors; reflectorCnt++)
131  {
132  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].cartesianDataValid);
133  if (navdata.landmarkData.reflectors[reflectorCnt].cartesianDataValid > 0)
134  {
135  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].cartesianData.x);
136  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].cartesianData.y);
137  }
138  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].polarDataValid);
139  if (navdata.landmarkData.reflectors[reflectorCnt].polarDataValid > 0)
140  {
141  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].polarData.dist);
142  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].polarData.phi);
143  }
144  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorDataValid);
145  if (navdata.landmarkData.reflectors[reflectorCnt].optReflectorDataValid > 0)
146  {
147  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.localID);
148  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.globalID);
149  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.type);
150  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.subType);
151  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.quality);
152  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.timestamp);
153  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.size);
154  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.hitCount);
155  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.meanEcho);
156  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.startIndex);
157  appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.endIndex);
158  }
159  }
160  }
161  appendToBuffer(data_buffer, navdata.scanDataValid);
162  for (int channel = 0; channel < navdata.scanDataValid; channel++)
163  {
164  std::copy(navdata.scanData[channel].contentType.begin(), navdata.scanData[channel].contentType.end(), std::back_inserter(data_buffer));
165  appendToBuffer(data_buffer, navdata.scanData[channel].scaleFactor);
166  appendToBuffer(data_buffer, navdata.scanData[channel].scaleOffset);
167  appendToBuffer(data_buffer, navdata.scanData[channel].startAngle);
168  appendToBuffer(data_buffer, navdata.scanData[channel].angleRes);
169  appendToBuffer(data_buffer, navdata.scanData[channel].timestampStart);
170  appendToBuffer(data_buffer, navdata.scanData[channel].numData);
171  for(int data_cnt = 0; data_cnt < navdata.scanData[channel].numData; data_cnt++)
172  {
173  appendToBuffer(data_buffer, navdata.scanData[channel].data[data_cnt]);
174  }
175  }
176  appendToBuffer(data_buffer, navdata.remissionDataValid);
177  if (navdata.remissionDataValid > 0)
178  {
179  std::copy(navdata.remissionData.contentType.begin(), navdata.remissionData.contentType.end(), std::back_inserter(data_buffer));
180  appendToBuffer(data_buffer, navdata.remissionData.scaleFactor);
181  appendToBuffer(data_buffer, navdata.remissionData.scaleOffset);
182  appendToBuffer(data_buffer, navdata.remissionData.startAngle);
183  appendToBuffer(data_buffer, navdata.remissionData.angleRes);
184  appendToBuffer(data_buffer, navdata.remissionData.timestampStart);
185  appendToBuffer(data_buffer, navdata.remissionData.numData);
186  for(int data_cnt = 0; data_cnt < navdata.remissionData.numData; data_cnt++)
187  {
188  appendToBuffer(data_buffer, navdata.remissionData.data[data_cnt]);
189  }
190  }
191  // write payload size
192  payload_size = data_buffer.size() - 8;
193  std::vector<uint8_t> payload_buffer;
194  appendToBuffer(payload_buffer, payload_size);
195  memcpy(&data_buffer[4], &payload_buffer[0], payload_buffer.size());
196  data_buffer.push_back(0); // dummy crc
197  }
198 
200  bool parseNAV350BinaryUnittest()
201  {
202  NAV350mNPOSData navdata_src, navdata_dst;
203  std::vector<uint8_t> data_buffer_src, data_buffer_dst;
204  data_buffer_src.reserve(32*1024);
205  data_buffer_dst.reserve(32*1024);
206  // Create dummy NAV350 position data
207  navdata_src.version = 1;
208  navdata_src.errorCode = 0;
209  navdata_src.wait = 1;
210  navdata_src.mask = 2;
211  navdata_src.poseDataValid = 1;
212  navdata_src.poseData.x = +1234;
213  navdata_src.poseData.y = -1234;
214  navdata_src.poseData.phi = M_PI / 2;
215  navdata_src.poseData.optPoseDataValid = 1;
216  navdata_src.poseData.optPoseData. outputMode = 1;
217  navdata_src.poseData.optPoseData. timestamp = 123456789;
218  navdata_src.poseData.optPoseData. meanDev = 789;
219  navdata_src.poseData.optPoseData. navMode = 3;
220  navdata_src.poseData.optPoseData. infoState = 1234;
221  navdata_src.poseData.optPoseData. quantUsedReflectors = 5;
222  navdata_src.landmarkDataValid = 1;
223  navdata_src.landmarkData.landmarkFilter = 1;
224  navdata_src.landmarkData.numReflectors = 25;
225  navdata_src.landmarkData.reflectors.resize(navdata_src.landmarkData.numReflectors);
226  for(int reflector_cnt = 0; reflector_cnt < navdata_src.landmarkData.numReflectors; reflector_cnt++)
227  {
228  navdata_src.landmarkData.reflectors[reflector_cnt].cartesianDataValid = 1;
229  navdata_src.landmarkData.reflectors[reflector_cnt].cartesianData.x = 2 * reflector_cnt + 0;
230  navdata_src.landmarkData.reflectors[reflector_cnt].cartesianData.y = 2 * reflector_cnt + 1;
231  navdata_src.landmarkData.reflectors[reflector_cnt].polarDataValid = 1;
232  navdata_src.landmarkData.reflectors[reflector_cnt].polarData.dist = 1.1415 * 2 * reflector_cnt;
233  navdata_src.landmarkData.reflectors[reflector_cnt].polarData.phi = M_PI * reflector_cnt / navdata_src.landmarkData.numReflectors;
234  navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorDataValid = 1;
235  navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.localID = 10 * reflector_cnt + 0;
236  navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.globalID = 10 * reflector_cnt + 1;
237  navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.type = 1;
238  navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.subType = 2;
239  navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.quality = 10 * reflector_cnt + 2;
240  navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.timestamp = 10 * reflector_cnt + 3;
241  navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.size = 10 * reflector_cnt + 4;
242  navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.hitCount = 10 * reflector_cnt + 5;
243  navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.meanEcho = 10 * reflector_cnt + 6;
244  navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.startIndex = 10 * reflector_cnt + 7;
245  navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.endIndex = 10 * reflector_cnt + 8;
246  }
247  navdata_src.scanDataValid = 2;
248  navdata_src.scanData.resize(navdata_src.scanDataValid);
249  navdata_src.scanData[0].contentType = "DIST1";
250  navdata_src.scanData[0].scaleFactor = 1;
251  navdata_src.scanData[0].scaleOffset = 0;
252  navdata_src.scanData[0].startAngle = 0;
253  navdata_src.scanData[0].angleRes = 250;
254  navdata_src.scanData[0].timestampStart = 123456789;
255  navdata_src.scanData[0].numData = 1024;
256  navdata_src.scanData[0].data.resize(navdata_src.scanData[0].numData);
257  for(int data_cnt = 0; data_cnt < navdata_src.scanData[0].numData; data_cnt++)
258  navdata_src.scanData[0].data[data_cnt] = data_cnt;
259  navdata_src.scanData[1].contentType = "ANGL1";
260  navdata_src.scanData[1].scaleFactor = 1;
261  navdata_src.scanData[1].scaleOffset = 0;
262  navdata_src.scanData[1].startAngle = 0;
263  navdata_src.scanData[1].angleRes = 250;
264  navdata_src.scanData[1].timestampStart = 123456789;
265  navdata_src.scanData[1].numData = 1024;
266  navdata_src.scanData[1].data.resize(navdata_src.scanData[1].numData);
267  for(int data_cnt = 0; data_cnt < navdata_src.scanData[1].numData; data_cnt++)
268  navdata_src.scanData[1].data[data_cnt] = data_cnt * navdata_src.scanData[1].angleRes;
269  navdata_src.remissionDataValid = 1;
270  navdata_src.remissionData.contentType = "RSSI1";
271  navdata_src.remissionData.scaleFactor = 1;
272  navdata_src.remissionData.scaleOffset = 0;
273  navdata_src.remissionData.startAngle = 0;
274  navdata_src.remissionData.angleRes = 250;
275  navdata_src.remissionData.timestampStart = 123456789;
276  navdata_src.remissionData.numData = 1024;
277  navdata_src.remissionData.data.resize(navdata_src.remissionData.numData);
278  for(int data_cnt = 0; data_cnt < navdata_src.remissionData.numData; data_cnt++)
279  navdata_src.remissionData.data[data_cnt] = data_cnt;
280  // Serialize to NAV350 position data to binary buffer
281  writeNAV350BinaryPositionData(navdata_src, data_buffer_src);
282  // Parse serialized to NAV350 position data
283  if (!parseNAV350BinaryPositionData(data_buffer_src.data(), data_buffer_src.size(), navdata_dst))
284  {
285  ROS_ERROR_STREAM("## ERROR parseNAV350BinaryUnittest(): parseNAV350BinaryPositionData failed");
286  return false;
287  }
288  // Re-serialize to NAV350 position data to binary buffer
289  writeNAV350BinaryPositionData(navdata_dst, data_buffer_dst);
290  // Result of serialization and deserialization must be identical
291  if (data_buffer_src.size() != data_buffer_dst.size() || memcmp(data_buffer_src.data(), data_buffer_dst.data(), data_buffer_src.size()) != 0)
292  {
293  ROS_ERROR_STREAM("## ERROR parseNAV350BinaryUnittest(): parseNAV350BinaryPositionData failed");
294  return false;
295  }
296  ROS_DEBUG_STREAM("parseNAV350BinaryUnittest() passed (" << data_buffer_src.size() << " byte datagram)");
297  return true;
298  }
299 
301  bool parseNAV350BinaryLandmarkData(const uint8_t* receiveBuffer, int& receivePos, int receiveBufferLength, NAV350LandmarkData& landmarkData)
302  {
303  bool success = true;
304  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.landmarkFilter, __FILE__, __LINE__);
305  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.numReflectors, __FILE__, __LINE__);
306  landmarkData.reflectors = std::vector<NAV350ReflectorData>(landmarkData.numReflectors);
307  for(int reflectorCnt = 0; reflectorCnt < landmarkData.numReflectors; reflectorCnt++)
308  {
309  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].cartesianDataValid, __FILE__, __LINE__);
310  if (landmarkData.reflectors[reflectorCnt].cartesianDataValid > 0)
311  {
312  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].cartesianData.x, __FILE__, __LINE__);
313  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].cartesianData.y, __FILE__, __LINE__);
314  }
315  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].polarDataValid, __FILE__, __LINE__);
316  if (landmarkData.reflectors[reflectorCnt].polarDataValid > 0)
317  {
318  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].polarData.dist, __FILE__, __LINE__);
319  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].polarData.phi, __FILE__, __LINE__);
320  }
321  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorDataValid, __FILE__, __LINE__);
322  if (landmarkData.reflectors[reflectorCnt].optReflectorDataValid > 0)
323  {
324  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.localID, __FILE__, __LINE__);
325  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.globalID, __FILE__, __LINE__);
326  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.type, __FILE__, __LINE__);
327  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.subType, __FILE__, __LINE__);
328  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.quality, __FILE__, __LINE__);
329  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.timestamp, __FILE__, __LINE__);
330  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.size, __FILE__, __LINE__);
331  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.hitCount, __FILE__, __LINE__);
332  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.meanEcho, __FILE__, __LINE__);
333  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.startIndex, __FILE__, __LINE__);
334  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.endIndex, __FILE__, __LINE__);
335  }
336  }
337  return success;
338  }
339 
340 
344  bool parseNAV350BinaryPositionData(const uint8_t* receiveBuffer, int receiveBufferLength, NAV350mNPOSData& navdata)
345  {
346  // Init return value
347  navdata = NAV350mNPOSData();
348 
349  // Parse header
350  if (receiveBuffer == 0 || receiveBufferLength < 17 + 9 || memcmp(receiveBuffer, "\x02\x02\x02\x02", 4) != 0)
351  {
352  ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): invalid telegram (" << __FILE__ << ":" << __LINE__ << ")");
353  return false;
354  }
355  bool success = true;
356  int receivePos = 4;
357  uint32_t payload_size = 0;
358  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, payload_size, __FILE__, __LINE__);
359  if (!success || (int)(payload_size) + 9 > receiveBufferLength)
360  {
361  ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): invalid telegram size (" << __FILE__ << ":" << __LINE__ << ")");
362  return false;
363  }
364 
365  // Parse "sAN mNPOSGetData "
366  if (strncmp((const char*)receiveBuffer + receivePos, "sAN mNPOSGetData ", 17) != 0)
367  {
368  ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): telegram does not start with \"sAN mNPOSGetData \"");
369  return false;
370  }
371  receivePos += 17;
372 
373  // Parse version errorCode wait mask poseData
374  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.version, __FILE__, __LINE__);
375  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.errorCode, __FILE__, __LINE__);
376  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.wait, __FILE__, __LINE__);
377  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.mask, __FILE__, __LINE__);
378  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseDataValid, __FILE__, __LINE__);
379  if (navdata.errorCode)
380  {
381  std::map<uint8_t, std::string> errorCodeStr = { {0, "no error"}, {1, "wrong operating mode"}, {2, "asynchrony Method terminated"}, {3, "invalid data"}, {4, "no position available"}, {5, "timeout"}, {6, "method already active"}, {7, "general error"} };
382  ROS_WARN_STREAM("## WARNING parseNAV350BinaryPositionData(): NAV350 mNPOSGetData errorCode = " << (int)navdata.errorCode << " (\"" << errorCodeStr[navdata.errorCode] << "\")");
383  }
384 
385  // Parse poseData x y phi optPoseData
386  if (navdata.poseDataValid > 0)
387  {
388  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.x, __FILE__, __LINE__);
389  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.y, __FILE__, __LINE__);
390  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.phi, __FILE__, __LINE__);
391  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseDataValid, __FILE__, __LINE__);
392  if (navdata.poseData.optPoseDataValid > 0)
393  {
394  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseData.outputMode, __FILE__, __LINE__);
395  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseData.timestamp, __FILE__, __LINE__);
396  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseData.meanDev, __FILE__, __LINE__);
397  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseData.navMode, __FILE__, __LINE__);
398  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseData.infoState, __FILE__, __LINE__);
399  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseData.quantUsedReflectors, __FILE__, __LINE__);
400  }
401  }
402 
403  // Parse landmark data
404  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.landmarkDataValid, __FILE__, __LINE__);
405  if (navdata.landmarkDataValid > 0)
406  {
407  success &= parseNAV350BinaryLandmarkData(receiveBuffer, receivePos, receiveBufferLength, navdata.landmarkData);
408  }
409 
410  // Parse NAVScan data
411  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanDataValid, __FILE__, __LINE__);
412  if (navdata.scanDataValid > 2)
413  ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): \"ScanData\" = " << (int)navdata.scanDataValid << ", max. 2 channel DIST1 and ANGL1 expected");
414  navdata.scanData.resize(navdata.scanDataValid);
415  for (int channel = 0; channel < navdata.scanDataValid; channel++)
416  {
417  if (strncmp((const char*)receiveBuffer + receivePos, "DIST1", 5) != 0 && strncmp((const char*)receiveBuffer + receivePos, "ANGL1", 5) != 0)
418  ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): \"DIST1\" resp. \"ANGL1\" not found");
419  navdata.scanData[channel].contentType = std::string((const char*)receiveBuffer + receivePos, 5);
420  receivePos += 5;
421  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].scaleFactor, __FILE__, __LINE__);
422  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].scaleOffset, __FILE__, __LINE__);
423  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].startAngle, __FILE__, __LINE__);
424  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].angleRes, __FILE__, __LINE__);
425  if (std::abs(navdata.scanData[channel].scaleFactor - 1.0f) > FLT_EPSILON || std::abs(navdata.scanData[channel].scaleOffset - 0.0f) > FLT_EPSILON || navdata.scanData[channel].startAngle != 0 || navdata.scanData[channel].angleRes != 250)
426  {
427  ROS_WARN_STREAM("## WARNING parseNAV350BinaryPositionData(): unexpected scan data parameter: scaleFactor=" << navdata.scanData[channel].scaleFactor << ", scaleOffset=" << navdata.scanData[channel].scaleOffset
428  << ", startAngle=" << navdata.scanData[channel].startAngle << ", angleRes=" << navdata.scanData[channel].angleRes);
429  }
430  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].timestampStart, __FILE__, __LINE__);
431  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].numData, __FILE__, __LINE__);
432  navdata.scanData[channel].data = std::vector<uint32_t>(navdata.scanData[channel].numData);
433  for(int data_cnt = 0; data_cnt < navdata.scanData[channel].numData; data_cnt++)
434  {
435  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].data[data_cnt], __FILE__, __LINE__);
436  }
437  }
438 
439  // Parse remission data
440  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionDataValid, __FILE__, __LINE__);
441  if (navdata.remissionDataValid > 0)
442  {
443  if (strncmp((const char*)receiveBuffer + receivePos, "RSSI1", 5) != 0)
444  ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): \"RSSI1\" not found");
445  navdata.remissionData.contentType = std::string((const char*)receiveBuffer + receivePos, 5);
446  receivePos += 5;
447  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.scaleFactor, __FILE__, __LINE__);
448  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.scaleOffset, __FILE__, __LINE__);
449  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.startAngle, __FILE__, __LINE__);
450  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.angleRes, __FILE__, __LINE__);
451  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.timestampStart, __FILE__, __LINE__);
452  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.numData, __FILE__, __LINE__);
453  navdata.remissionData.data = std::vector<uint16_t>(navdata.remissionData.numData);
454  for(int data_cnt = 0; data_cnt < navdata.remissionData.numData; data_cnt++)
455  {
456  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.data[data_cnt], __FILE__, __LINE__);
457  }
458  }
459 
460  if (!success)
461  ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): parsing error");
462  return success;
463  }
464 
466  bool parseNAV350BinaryLandmarkDataDoMappingResponse(const uint8_t* receiveBuffer, int receiveBufferLength, NAV350LandmarkDataDoMappingResponse& landmarkData)
467  {
468  // Init return value
469  landmarkData = NAV350LandmarkDataDoMappingResponse();
470  // Parse header
471  if (receiveBuffer == 0 || receiveBufferLength < 19 + 9 || memcmp(receiveBuffer, "\x02\x02\x02\x02", 4) != 0)
472  {
473  ROS_ERROR_STREAM("## ERROR parseNAV350LandmarkDataDoMappingResponse(): invalid telegram (" << __FILE__ << ":" << __LINE__ << ")");
474  return false;
475  }
476  bool success = true;
477  int receivePos = 4;
478  uint32_t payload_size = 0;
479  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, payload_size, __FILE__, __LINE__);
480  if (!success || (int)(payload_size) + 9 > receiveBufferLength)
481  {
482  ROS_ERROR_STREAM("## ERROR parseNAV350LandmarkDataDoMappingResponse(): invalid telegram size (" << __FILE__ << ":" << __LINE__ << ")");
483  return false;
484  }
485  // Parse "sAN mNMAPDoMapping "
486  if (strncmp((const char*)receiveBuffer + receivePos, "sAN mNMAPDoMapping ", 19) != 0)
487  {
488  ROS_ERROR_STREAM("## ERROR parseNAV350LandmarkDataDoMappingResponse(): telegram does not start with \"sAN mNMAPDoMapping \"");
489  return false;
490  }
491  receivePos += 19;
492  // Parse errorCode
493  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.errorCode, __FILE__, __LINE__);
494  success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.landmarkDataValid, __FILE__, __LINE__);
495  if (landmarkData.errorCode)
496  {
497  std::map<uint8_t, std::string> errorCodeStr = { {0, "no error"}, {1, "wrong operating mode"}, {2, "asynchrony Method terminated"}, {5, "timeout"}, {6, "method already active"}, {7, "general error"} };
498  ROS_WARN_STREAM("## WARNING parseNAV350LandmarkDataDoMappingResponse(): NAV350 landmarkData.errorCode = " << (int)landmarkData.errorCode << " (\"" << errorCodeStr[landmarkData.errorCode] << "\")");
499  }
500  // Parse landmarkData
501  if (landmarkData.landmarkDataValid > 0)
502  {
503  success &= parseNAV350BinaryLandmarkData(receiveBuffer, receivePos, receiveBufferLength, landmarkData.landmarkData);
504  }
505  return success;
506  }
507 
509  std::vector<uint8_t> createNAV350BinaryAddLandmarkRequest(const NAV350LandmarkData& landmarkData, int nav_curr_layer)
510  {
511  std::string sopas_start = "sMN mNLAYAddLandmark ";
512  std::vector<uint8_t> request(sopas_start.begin(), sopas_start.end());
513  appendToBuffer(request, (uint16_t)landmarkData.reflectors.size());
514  for(int reflector_cnt = 0; reflector_cnt < landmarkData.reflectors.size(); reflector_cnt++)
515  {
516  if (landmarkData.reflectors[reflector_cnt].cartesianDataValid == 0)
517  ROS_ERROR_STREAM("## ERROR createNAV350BinaryAddLandmarkRequest(): " << (reflector_cnt + 1) << ". reflector has no valid cartesian data");
518  if (landmarkData.reflectors[reflector_cnt].optReflectorDataValid == 0)
519  ROS_ERROR_STREAM("## ERROR createNAV350BinaryAddLandmarkRequest(): " << (reflector_cnt + 1) << ". reflector has no valid type and subtype");
520  appendToBuffer(request, (int32_t)landmarkData.reflectors[reflector_cnt].cartesianData.x);
521  appendToBuffer(request, (int32_t)landmarkData.reflectors[reflector_cnt].cartesianData.y);
522  request.push_back((uint8_t)landmarkData.reflectors[reflector_cnt].optReflectorData.type);
523  request.push_back((uint8_t)landmarkData.reflectors[reflector_cnt].optReflectorData.subType);
524  appendToBuffer(request, (uint16_t)landmarkData.reflectors[reflector_cnt].optReflectorData.size);
525  appendToBuffer(request, (uint16_t)1);
526  appendToBuffer(request, (uint16_t)nav_curr_layer);
527  }
528  return request;
529  }
530 
532  std::vector<uint8_t> createNAV350BinaryAddLandmarkRequest(const std::vector<sick_scan_xd::NAV350ImkLandmark> landmarks)
533  {
534  std::string sopas_start = "sMN mNLAYAddLandmark ";
535  std::vector<uint8_t> request(sopas_start.begin(), sopas_start.end());
536  appendToBuffer(request, (uint16_t)landmarks.size());
537  for(int reflector_cnt = 0; reflector_cnt < landmarks.size(); reflector_cnt++)
538  {
539  appendToBuffer(request, (int32_t)landmarks[reflector_cnt].x_mm);
540  appendToBuffer(request, (int32_t)landmarks[reflector_cnt].y_mm);
541  request.push_back((uint8_t)landmarks[reflector_cnt].type);
542  request.push_back((uint8_t)landmarks[reflector_cnt].subType);
543  appendToBuffer(request, (uint16_t)landmarks[reflector_cnt].size_mm);
544  appendToBuffer(request, (uint16_t)landmarks[reflector_cnt].layerID.size());
545  for(int layer_cnt = 0; layer_cnt < landmarks[reflector_cnt].layerID.size(); layer_cnt++)
546  appendToBuffer(request, (uint16_t)landmarks[reflector_cnt].layerID[layer_cnt]);
547  }
548  return request;
549  }
550 
552  std::vector<uint8_t> createNAV350BinarySetSpeedRequest(const sick_scan_msg::NAVOdomVelocity& msg)
553  {
554  std::string sopas_start = "sMN mNPOSSetSpeed ";
555  std::vector<uint8_t> request(sopas_start.begin(), sopas_start.end());
556  appendToBuffer(request, (int16_t)(1000.0 * msg.vel_x));
557  appendToBuffer(request, (int16_t)(1000.0 * msg.vel_y));
558  appendToBuffer(request, (int32_t)(1000.0 * msg.omega * 180.0 / M_PI));
559  appendToBuffer(request, (uint32_t)(msg.timestamp));
560  appendToBuffer(request, (uint8_t)(msg.coordbase));
561  return request;
562  }
563 
565  bool parseNAV350BinaryPositionData(const uint8_t* receiveBuffer, int receiveBufferLength, short& elevAngleX200, double& elevationAngleInRad, rosTime& recvTimeStamp,
566  bool config_sw_pll_only_publish, double config_time_offset, SickGenericParser* parser_, int& numEchos, ros_sensor_msgs::LaserScan & msg,
567  sick_scan_msg::NAVPoseData& nav_pose_msg, sick_scan_msg::NAVLandmarkData& nav_landmark_msg, NAV350mNPOSData& navdata)
568  {
569  // Init return values
570  elevAngleX200 = 0;
571  elevationAngleInRad = 0;
572  numEchos = 1;
573  navdata = NAV350mNPOSData();
574  double nav_angle_offset = parser_->getCurrentParamPtr()->getScanAngleShift();
575 
576  // NAV-350 operation manual: scan frequency 8 Hz, angular resolution 0.25 deg, scanning 0 to 360 deg, 1440 points per scan
577  ROS_HEADER_SEQ(msg.header, elevAngleX200);
578  msg.range_min = parser_->get_range_min();
579  msg.range_max = parser_->get_range_max();
580  msg.angle_min = (float)(0.0 + nav_angle_offset);
581  msg.angle_max = (float)(2.0 * M_PI + nav_angle_offset);
582  msg.angle_increment = (float)(0.25 * M_PI / 180.0);
583  msg.scan_time = 1.0f / 8.0f;
584  msg.time_increment = msg.scan_time * msg.angle_increment / (float)(2.0 * M_PI);
585  uint32_t nav_timestampStart = 0; // timestamp of scan start in ms
586 
587  // Parse "sAN mNPOSGetData "
588  ROS_DEBUG_STREAM("NAV350: " << receiveBufferLength << " bytes received: " << DataDumper::binDataToAsciiString(&receiveBuffer[0], std::min<int>(64, receiveBufferLength)));
589  if (!parseNAV350BinaryPositionData(receiveBuffer, receiveBufferLength, navdata))
590  {
591  ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): parsing error");
592  return false;
593  }
594  navdata.angleOffset = nav_angle_offset;
595 
596  // Convert NAV350PoseData to sick_scan_msg::NAVPoseData
597  nav_pose_msg = sick_scan_msg::NAVPoseData();
598  nav_pose_msg.pose_valid = 0;
599  if (navdata.poseDataValid > 0)
600  {
601  nav_pose_msg.x = navdata.poseData.x;
602  nav_pose_msg.y = navdata.poseData.y;
603  nav_pose_msg.phi = navdata.poseData.phi;
604  nav_pose_msg.opt_pose_data_valid = navdata.poseData.optPoseDataValid;
605  if (navdata.poseData.optPoseDataValid > 0)
606  {
607  nav_pose_msg.output_mode = navdata.poseData.optPoseData.outputMode;
608  nav_pose_msg.timestamp = navdata.poseData.optPoseData.timestamp;
609  nav_pose_msg.mean_dev = navdata.poseData.optPoseData.meanDev;
610  nav_pose_msg.nav_mode = navdata.poseData.optPoseData.navMode;
611  nav_pose_msg.info_state = navdata.poseData.optPoseData.infoState;
612  nav_pose_msg.quant_used_reflectors = navdata.poseData.optPoseData.quantUsedReflectors;
613  }
614  // Convert pose into ros coordinates
615  convertNAVCartPos3DtoROSPos3D(nav_pose_msg.x, nav_pose_msg.y, nav_pose_msg.phi, nav_pose_msg.pose_x, nav_pose_msg.pose_y, nav_pose_msg.pose_yaw, nav_angle_offset); // position in ros coordinates in meter, yaw angle in radians
616  nav_pose_msg.pose_valid = 1;
617  ROS_DEBUG_STREAM("NAV350 PoseData: x=" << nav_pose_msg.pose_x << ", y=" << nav_pose_msg.pose_y << ", yaw=" << (nav_pose_msg.pose_yaw*180.0/M_PI)
618  << " (lidar: x=" << nav_pose_msg.x << ", y=" << nav_pose_msg.y << ", phi=" << nav_pose_msg.phi << ")");
619  }
620  else
621  {
622  ROS_DEBUG_STREAM("NAV350: no PoseData");
623  }
624 
625  // Convert NAV350LandmarkData to sick_scan_msg::NAVLandmarkData
626  nav_landmark_msg = sick_scan_msg::NAVLandmarkData();
627  if (navdata.landmarkDataValid > 0)
628  {
629  ROS_DEBUG_STREAM("NAV350 LandmarkData: " << navdata.landmarkData.reflectors.size() << " reflectors");
630  nav_landmark_msg.landmark_filter = navdata.landmarkData.landmarkFilter;
631  nav_landmark_msg.num_reflectors = navdata.landmarkData.numReflectors;
632  nav_landmark_msg.reflectors.resize(navdata.landmarkData.reflectors.size());
633  for(int reflector_cnt = 0; reflector_cnt < navdata.landmarkData.reflectors.size(); reflector_cnt++)
634  {
635  nav_landmark_msg.reflectors[reflector_cnt].cartesian_data_valid = navdata.landmarkData.reflectors[reflector_cnt].cartesianDataValid;
636  nav_landmark_msg.reflectors[reflector_cnt].polar_data_valid = navdata.landmarkData.reflectors[reflector_cnt].polarDataValid;
637  nav_landmark_msg.reflectors[reflector_cnt].opt_reflector_data_valid = navdata.landmarkData.reflectors[reflector_cnt].optReflectorDataValid;
638  nav_landmark_msg.reflectors[reflector_cnt].pos_valid = 0;
639  if (navdata.landmarkData.reflectors[reflector_cnt].cartesianDataValid > 0)
640  {
641  nav_landmark_msg.reflectors[reflector_cnt].x = navdata.landmarkData.reflectors[reflector_cnt].cartesianData.x;
642  nav_landmark_msg.reflectors[reflector_cnt].y = navdata.landmarkData.reflectors[reflector_cnt].cartesianData.y;
643  // Convert landmark position into ros coordinates in meter
644  convertNAVCartPos2DtoROSPos2D(nav_landmark_msg.reflectors[reflector_cnt].x, nav_landmark_msg.reflectors[reflector_cnt].y,
645  nav_landmark_msg.reflectors[reflector_cnt].pos_x, nav_landmark_msg.reflectors[reflector_cnt].pos_y, nav_angle_offset);
646  nav_landmark_msg.reflectors[reflector_cnt].pos_valid = 1;
647  }
648  if (navdata.landmarkData.reflectors[reflector_cnt].polarDataValid > 0)
649  {
650  nav_landmark_msg.reflectors[reflector_cnt].dist = navdata.landmarkData.reflectors[reflector_cnt].polarData.dist;
651  nav_landmark_msg.reflectors[reflector_cnt].phi = navdata.landmarkData.reflectors[reflector_cnt].polarData.phi;
652  }
653  if (navdata.landmarkData.reflectors[reflector_cnt].optReflectorDataValid > 0)
654  {
655  nav_landmark_msg.reflectors[reflector_cnt].local_id = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.localID;
656  nav_landmark_msg.reflectors[reflector_cnt].global_id = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.globalID;
657  nav_landmark_msg.reflectors[reflector_cnt].type = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.type;
658  nav_landmark_msg.reflectors[reflector_cnt].sub_type = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.subType;
659  nav_landmark_msg.reflectors[reflector_cnt].quality = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.quality;
660  nav_landmark_msg.reflectors[reflector_cnt].timestamp = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.timestamp;
661  nav_landmark_msg.reflectors[reflector_cnt].size = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.size;
662  nav_landmark_msg.reflectors[reflector_cnt].hit_count = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.hitCount;
663  nav_landmark_msg.reflectors[reflector_cnt].mean_echo = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.meanEcho;
664  nav_landmark_msg.reflectors[reflector_cnt].start_index = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.startIndex;
665  nav_landmark_msg.reflectors[reflector_cnt].end_index = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.endIndex;
666  }
667  ROS_DEBUG_STREAM("NAV350 LandmarkData: reflector[" << reflector_cnt << "]"
668  << ": cartesian=" << (int)(nav_landmark_msg.reflectors[reflector_cnt].cartesian_data_valid) << ", x=" << nav_landmark_msg.reflectors[reflector_cnt].x << ", y=" << nav_landmark_msg.reflectors[reflector_cnt].y
669  << ", polar=" << (int)(nav_landmark_msg.reflectors[reflector_cnt].polar_data_valid) << ", dist=" << nav_landmark_msg.reflectors[reflector_cnt].dist << ", phi=" << nav_landmark_msg.reflectors[reflector_cnt].phi
670  << ", pos_valid=" << (int)(nav_landmark_msg.reflectors[reflector_cnt].pos_valid) << ", pos_x=" << nav_landmark_msg.reflectors[reflector_cnt].pos_x << ", pos_y=" << nav_landmark_msg.reflectors[reflector_cnt].pos_y
671  << ", optValid=" << (int)(navdata.landmarkData.reflectors[reflector_cnt].optReflectorDataValid) << ", local_id=" << nav_landmark_msg.reflectors[reflector_cnt].local_id << ", global_id=" << nav_landmark_msg.reflectors[reflector_cnt].global_id);
672  }
673  }
674  else
675  {
676  ROS_DEBUG_STREAM("NAV350: no LandmarkData");
677  }
678 
679  // Convert scan data (NAV350ScanData) to LaserScan message
680  if (navdata.scanData.empty())
681  ROS_WARN_STREAM("## WARNING NAV350: no scan data channel in mNPOSGetData message");
682  for(int channel = 0; channel < navdata.scanData.size(); channel++)
683  ROS_DEBUG_STREAM("NAV350 scan data channel " << (channel + 1) << " of " << navdata.scanData.size() << ": " << navdata.scanData[channel].contentType << ", startAngle=" << navdata.scanData[channel].startAngle << ", angleRes=" << navdata.scanData[channel].angleRes << ", numPoints=" << navdata.scanData[channel].numData << ", timestampStart=" << navdata.scanData[channel].timestampStart);
684  msg.ranges.clear();
685  msg.intensities.clear();
686  if (navdata.scanDataValid > 0 && navdata.scanData.size() > 0)
687  {
688  msg.angle_min = (float)(0.001 * navdata.scanData[0].startAngle * M_PI / 180.0 + nav_angle_offset);
689  msg.angle_increment = (float)(0.001 * navdata.scanData[0].angleRes * M_PI / 180.0);
690  msg.angle_max = (float)(msg.angle_min + msg.angle_increment * navdata.scanData[0].numData);
691  nav_timestampStart = navdata.scanData[0].timestampStart;
692  NAV350ScanData* dist_scan_data = 0;
693  for(int channel = 0; dist_scan_data == 0 && channel < navdata.scanData.size(); channel++)
694  if (navdata.scanData[channel].contentType == "DIST1")
695  dist_scan_data = &navdata.scanData[channel];
696  if (!dist_scan_data || dist_scan_data->numData <= 0)
697  {
698  ROS_ERROR_STREAM("## ERROR NAV350: No channel DIST1 found in scan data of mNPOSGetData message");
699  }
700  else
701  {
702  float scaleFactor = dist_scan_data->scaleFactor; // multiplier, always 1 for NAV350
703  float scaleOffset = dist_scan_data->scaleOffset; // offset, always 0 for NAV350
704  msg.ranges.resize(dist_scan_data->numData);
705  if (std::fabs(scaleFactor - 1.0f) > FLT_EPSILON || std::fabs(scaleOffset) > FLT_EPSILON)
706  {
707  ROS_WARN_STREAM("## WARNING NAV350: using DIST1 scaleFactor=" << scaleFactor << " (expected: scaleFactor:=1) and scaleOffset=" << scaleOffset << " (expected: scaleOffset:=0) in mNPOSGetData message");
708  for(int point_cnt = 0; point_cnt < dist_scan_data->numData; point_cnt++)
709  {
710  msg.ranges[point_cnt] = 0.001f * (scaleFactor * (float)dist_scan_data->data[point_cnt] + scaleOffset); // DIST in mm to range in m
711  }
712  }
713  else
714  {
715  for(int point_cnt = 0; point_cnt < dist_scan_data->numData; point_cnt++)
716  {
717  msg.ranges[point_cnt] = 0.001f * (float)dist_scan_data->data[point_cnt]; // DIST in mm to range in m
718  }
719  }
720  }
721  }
722 
723  // Convert scan intensities (NAV350RemissionData) to LaserScan message
724  if (navdata.remissionDataValid > 0)
725  {
726  ROS_DEBUG_STREAM("NAV350 remission data: " << navdata.remissionData.contentType << ", startAngle=" << navdata.remissionData.startAngle << ", angleRes=" << navdata.remissionData.angleRes << ", numPoints=" << navdata.remissionData.numData);
727  if (navdata.remissionData.contentType != "RSSI1" || navdata.remissionData.numData != msg.ranges.size())
728  ROS_WARN_STREAM("## WARNING NAV350: remissionData.contentType=" << navdata.remissionData.contentType << " (expected: RSSI1), remissionData.numData=" << navdata.remissionData.numData << " (expected: " << msg.ranges.size() << " points) in mNPOSGetData message");
729  msg.intensities.resize(navdata.remissionData.numData);
730  for(int point_cnt = 0; point_cnt < navdata.remissionData.numData; point_cnt++)
731  {
732  msg.intensities[point_cnt] = (float)navdata.remissionData.data[point_cnt];
733  }
734  }
735 
736  // Update Software-PLL by sensor timestamp
737  if (nav_timestampStart > 0)
738  {
739  uint32_t recvTimeStampSec = (uint32_t)sec(recvTimeStamp), recvTimeStampNsec = (uint32_t)nsec(recvTimeStamp);
740  bool softwarePLLready = SoftwarePLL::instance().updatePLL(recvTimeStampSec, recvTimeStampNsec, nav_timestampStart);
741  if (softwarePLLready)
742  {
743  SoftwarePLL::instance().getCorrectedTimeStamp(recvTimeStampSec, recvTimeStampNsec, nav_timestampStart);
744  recvTimeStamp = rosTime(recvTimeStampSec, recvTimeStampNsec);
745  msg.header.stamp = recvTimeStamp + rosDurationFromSec(config_time_offset); // recvTimeStamp updated by software-pll
746  ROS_DEBUG_STREAM("NAV350: SoftwarePLL ready: NAV-timestamp=" << nav_timestampStart << " ms, ROS-timestamp=" << rosTimeToSeconds(recvTimeStamp) << " sec.");
747  }
748  else
749  {
750  ROS_DEBUG_STREAM("NAV350: SoftwarePLL still initializing, NAV-timestamp=" << nav_timestampStart << " ms, ROS-timestamp=" << rosTimeToSeconds(recvTimeStamp) << " sec.");
751  }
752  if (config_sw_pll_only_publish == true)
753  {
754  incSoftwarePLLPacketReceived();
755  }
756  }
757  nav_pose_msg.header = msg.header;
758  nav_landmark_msg.header = msg.header;
759 
760  return true;
761  }
762 
764  void rotateXYbyAngleOffset(float& x, float& y, double angle_offset)
765  {
766  if (std::abs(angle_offset - (+M_PI)) <= FLT_EPSILON || std::abs(angle_offset - (-M_PI)) <= FLT_EPSILON)
767  {
768  x = (-x);
769  y = (-y);
770  }
771  else if (std::abs(angle_offset) <= FLT_EPSILON)
772  {
773  x = (+x);
774  y = (+y);
775  }
776  else
777  {
778  x = (float)(x * cos(angle_offset) - y * sin(angle_offset));
779  y = (float)(x * sin(angle_offset) + y * cos(angle_offset));
780  }
781  }
782 
784  void convertNAVCartPos2DtoROSPos2D(int32_t nav_posx_mm, int32_t nav_posy_mm, float& ros_posx_m, float& ros_posy_m, double nav_angle_offset)
785  {
786  ros_posx_m = (float)(1.0e-3 * nav_posx_mm);
787  ros_posy_m = (float)(1.0e-3 * nav_posy_mm);
788  rotateXYbyAngleOffset(ros_posx_m, ros_posy_m, nav_angle_offset);
789  }
790 
792  void convertNAVCartPos3DtoROSPos3D(int32_t nav_posx_mm, int32_t nav_posy_mm, uint32_t nav_phi_mdeg, float& ros_posx_m, float& ros_posy_m, float& ros_yaw_rad, double nav_angle_offset)
793  {
794  convertNAVCartPos2DtoROSPos2D(nav_posx_mm, nav_posy_mm, ros_posx_m, ros_posy_m, nav_angle_offset); // position in ros coordinates in meter
795  ros_yaw_rad = (float)(1.0e-3 * nav_phi_mdeg * M_PI / 180.0 + nav_angle_offset); // yaw angle in radians
796  }
797 
799  ros_geometry_msgs::TransformStamped convertNAVPoseDataToTransform(NAV350PoseData& poseData, rosTime recvTimeStamp, double config_time_offset,
800  const std::string& tf_parent_frame_id, const std::string& tf_child_frame_id, SickGenericParser* parser_)
801  {
802  double nav_angle_offset = parser_->getCurrentParamPtr()->getScanAngleShift(); // NAV350: -PI
803  ros_geometry_msgs::TransformStamped tf;
804  // Convert to ROS transform
805  float posx = 0, posy = 0, yaw = 0;
806  convertNAVCartPos3DtoROSPos3D(poseData.x, poseData.y, poseData.phi, posx, posy, yaw, nav_angle_offset); // position in ros coordinates in meter, yaw angle in radians
807  // Convert timestamp from lidar to system time
808  if (poseData.optPoseDataValid > 0 && poseData.optPoseData.timestamp > 0 && SoftwarePLL::instance().IsInitialized())
809  {
810  uint32_t recvTimeStampSec = (uint32_t)sec(recvTimeStamp), recvTimeStampNsec = (uint32_t)nsec(recvTimeStamp);
811  SoftwarePLL::instance().getCorrectedTimeStamp(recvTimeStampSec, recvTimeStampNsec, poseData.optPoseData.timestamp);
812  tf.header.stamp = rosTime(recvTimeStampSec, recvTimeStampNsec) + rosDurationFromSec(config_time_offset); // recvTimeStamp updated by software-pll
813  // Check function convSystemtimeToLidarTimestamp inverse to getCorrectedTimeStamp (debugging only)
814  // uint32_t lidar_timestamp = 0;
815  // SoftwarePLL::instance().convSystemtimeToLidarTimestamp(recvTimeStampSec, recvTimeStampNsec, lidar_timestamp);
816  // if (poseData.optPoseData.timestamp != lidar_timestamp)
817  // ROS_ERROR_STREAM("## ERROR ticks_in=" << poseData.optPoseData.timestamp << ", time=" << rosTimeToSeconds(rosTime(recvTimeStampSec, recvTimeStampNsec)) << ", ticks_out=" << lidar_timestamp);
818  }
819  else
820  {
821  tf.header.stamp = recvTimeStamp;
822  }
823  // Convert to ros transform message
824  tf.header.frame_id = tf_parent_frame_id;
825  tf.child_frame_id = tf_child_frame_id;
826  tf.transform.translation.x = posx;
827  tf.transform.translation.y = posy;
828  tf.transform.translation.z = 0.0;
829  tf2::Quaternion q;
830  q.setRPY(0, 0, yaw);
831  tf.transform.rotation.x = q.x();
832  tf.transform.rotation.y = q.y();
833  tf.transform.rotation.z = q.z();
834  tf.transform.rotation.w = q.w();
835  return tf;
836  }
837 
839  ros_visualization_msgs::MarkerArray convertNAVLandmarkDataToMarker(const std::vector<NAV350ReflectorData>& reflectors, ros_std_msgs::Header& header, SickGenericParser* parser_)
840  {
841  double nav_angle_offset = parser_->getCurrentParamPtr()->getScanAngleShift(); // NAV350: -PI
842  ros_visualization_msgs::MarkerArray marker_array;
843  marker_array.markers.reserve(1 + 2 * reflectors.size());
844  marker_array.markers.push_back(ros_visualization_msgs::Marker());
845  marker_array.markers.back().header = header;
846  marker_array.markers.back().action = ros_visualization_msgs::Marker::DELETEALL;
847  for(int reflector_cnt = 0; reflector_cnt < reflectors.size(); reflector_cnt++)
848  {
849  if (reflectors[reflector_cnt].cartesianDataValid > 0)
850  {
851  float reflector_posx = 0, reflector_posy = 0;
852  convertNAVCartPos2DtoROSPos2D(reflectors[reflector_cnt].cartesianData.x, reflectors[reflector_cnt].cartesianData.y, reflector_posx, reflector_posy, nav_angle_offset);
853  float reflector_size = 0.001f * (float)((reflectors[reflector_cnt].optReflectorDataValid > 0) ? (reflectors[reflector_cnt].optReflectorData.size) : 80);
854  int32_t reflector_id = ((reflectors[reflector_cnt].optReflectorDataValid > 0) ? (reflectors[reflector_cnt].optReflectorData.localID) : (reflector_cnt + 1));
855  ros_visualization_msgs::Marker marker_cylinder;
856  marker_cylinder.ns = "sick_scan";
857  marker_cylinder.id = reflector_cnt;
858  marker_cylinder.type = ros_visualization_msgs::Marker::CYLINDER;
859  marker_cylinder.scale.x = reflector_size;
860  marker_cylinder.scale.y = reflector_size;
861  marker_cylinder.scale.z = reflector_size;
862  marker_cylinder.pose.position.x = reflector_posx;
863  marker_cylinder.pose.position.y = reflector_posy;
864  marker_cylinder.pose.position.z = 0.0;
865  marker_cylinder.pose.orientation.x = 0.0;
866  marker_cylinder.pose.orientation.y = 0.0;
867  marker_cylinder.pose.orientation.z = 0.0;
868  marker_cylinder.pose.orientation.w = 1.0;
869  marker_cylinder.action = ros_visualization_msgs::Marker::ADD;
870  marker_cylinder.color.r = 1.0f;
871  marker_cylinder.color.g = 0.0f;
872  marker_cylinder.color.b = 0.0f;
873  marker_cylinder.color.a = 0.5f;
874  marker_cylinder.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever
875  marker_cylinder.header = header;
876  marker_array.markers.push_back(marker_cylinder);
877 
878  ros_visualization_msgs::Marker marker_text;
879  marker_text.ns = "sick_scan";
880  marker_text.id = reflector_cnt + reflectors.size();
881  marker_text.type = ros_visualization_msgs::Marker::TEXT_VIEW_FACING;
882  marker_text.text = std::to_string(reflector_id);
883  marker_text.scale.x = reflector_size;
884  marker_text.scale.y = reflector_size;
885  marker_text.scale.z = 2.0f * reflector_size;
886  marker_text.pose.position.x = reflector_posx;
887  marker_text.pose.position.y = reflector_posy + 1.5f * reflector_size;
888  marker_text.pose.position.z = 0.0;
889  marker_text.pose.orientation.x = 0.0;
890  marker_text.pose.orientation.y = 0.0;
891  marker_text.pose.orientation.z = 0.0;
892  marker_text.pose.orientation.w = 1.0;
893  marker_text.action = ros_visualization_msgs::Marker::ADD;
894  marker_text.color.r = 1.0f;
895  marker_text.color.g = 1.0f;
896  marker_text.color.b = 1.0f;
897  marker_text.color.a = 1.0f;
898  marker_text.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever
899  marker_text.header = header;
900  marker_array.markers.push_back(marker_text);
901 
902  }
903  }
904  return marker_array;
905  }
906 
908  std::vector<sick_scan_xd::NAV350ImkLandmark> readNAVIMKfile(const std::string& nav_imk_file)
909  {
910  std::vector<sick_scan_xd::NAV350ImkLandmark> navImkLandmarks;
911  std::ifstream imk_stream(nav_imk_file);
912  if(imk_stream.is_open())
913  {
914  std::string line;
915  while (getline(imk_stream, line))
916  {
917  // Ignore comments and headers
918  for(int n = 0; n < line.size(); n++)
919  {
920  if (isspace(line[n]))
921  line[n] = ' ';
922  }
923  if (line.empty() || line[0] == '#' || strncmp(line.c_str(), "globID", 6) == 0)
924  continue;
925  // Split into arguments
926  std::stringstream ss(line);
927  std::string token;
928  std::vector<std::string> args;
929  while (std::getline(ss, token, ' '))
930  {
931  if (!token.empty())
932  args.push_back(token);
933  }
934  // Ignore empty lines
935  if (args.empty())
936  continue;
937  // Parse arguments
938  if (args.size() < 7)
939  {
940  ROS_WARN_STREAM("## ERROR readNAVIMKfile(" << nav_imk_file << "): parse error, line \"" << line << "\" ignored.");
941  continue;
942  }
943  sick_scan_xd::NAV350ImkLandmark landmark;
944  landmark.globID = (uint16_t)(atoi(args[0].c_str()) & 0xFFFF);
945  landmark.x_mm = (int32_t)(atoi(args[1].c_str()) & 0xFFFFFFFF);
946  landmark.y_mm = (int32_t)(atoi(args[2].c_str()) & 0xFFFFFFFF);
947  landmark.type = (uint8_t)(atoi(args[3].c_str()) & 0xFF);
948  landmark.subType = (uint16_t)(atoi(args[4].c_str()) & 0xFFFF);
949  landmark.size_mm = (uint16_t)(atoi(args[5].c_str()) & 0xFFFF);
950  for(int n = 6; n < args.size(); n++)
951  landmark.layerID.push_back((uint16_t)(atoi(args[n].c_str()) & 0xFFFF));
952  navImkLandmarks.push_back(landmark);
953  }
954  }
955  ROS_INFO_STREAM("sick_scan_xd::readNAVIMKfile(\"" << nav_imk_file << "\"): " << navImkLandmarks.size() << " landmarks:");
956  for(int n = 0; n < navImkLandmarks.size(); n++)
957  {
958  std::stringstream imk_stream;
959  imk_stream << (int)navImkLandmarks[n].globID << " " << (int)navImkLandmarks[n].x_mm << " " << (int)navImkLandmarks[n].y_mm << " " << (int)navImkLandmarks[n].type << " " << (int)navImkLandmarks[n].subType << " " << (int)navImkLandmarks[n].size_mm;
960  for(int m = 0; m < navImkLandmarks[n].layerID.size(); m++)
961  imk_stream << " " << (int)navImkLandmarks[n].layerID[m];
962  ROS_INFO_STREAM("landmark " << (n + 1) << ": " << imk_stream.str());
963  }
964  return navImkLandmarks;
965  }
966 
967 } /* namespace sick_scan_xd */
sick_scan_xd::NAVLandmarkData
::sick_scan_xd::NAVLandmarkData_< std::allocator< void > > NAVLandmarkData
Definition: NAVLandmarkData.h:66
sick_scan_xd::NAV350RemissionData::scaleOffset
float scaleOffset
Definition: sick_nav_scandata.h:191
sick_scan_xd::convertNAVLandmarkDataToMarker
ros_visualization_msgs::MarkerArray convertNAVLandmarkDataToMarker(const std::vector< NAV350ReflectorData > &reflectors, ros_std_msgs::Header &header, SickGenericParser *parser_)
Definition: sick_nav_scandata_parser.cpp:839
sick_scan_xd::NAV350ImkLandmark::x_mm
int32_t x_mm
Definition: sick_nav_scandata.h:224
sick_scan_xd::parseNAV350BinaryUnittest
bool parseNAV350BinaryUnittest()
Definition: sick_nav_scandata_parser.cpp:200
sick_scan_xd::NAV350mNPOSData::landmarkData
NAV350LandmarkData landmarkData
Definition: sick_nav_scandata.h:210
msg
msg
SoftwarePLL::updatePLL
bool updatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick)
Definition: softwarePLL.cpp:191
swap_endian
void swap_endian(unsigned char *ptr, int numBytes)
Universal swapping function.
Definition: sick_scan_common.cpp:125
sick_scan_xd::NAV350ImkLandmark
Definition: sick_nav_scandata.h:220
sick_scan_xd::NAV350mNPOSData::angleOffset
float angleOffset
Definition: sick_nav_scandata.h:215
sick_scan_xd::NAV350mNPOSData::remissionDataValid
uint16_t remissionDataValid
Definition: sick_nav_scandata.h:213
sick_scan_xd::NAV350ScanData::scaleOffset
float scaleOffset
Definition: sick_nav_scandata.h:177
test_server.type
type
Definition: test_server.py:210
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
imu_delay_tester.args
args
Definition: imu_delay_tester.py:124
sick_scan_xd::NAV350LandmarkDataDoMappingResponse
Definition: sick_nav_scandata.h:157
SoftwarePLL::instance
static SoftwarePLL & instance()
Definition: softwarePLL.h:24
sick_scan_xd::NAVPoseData
::sick_scan_xd::NAVPoseData_< std::allocator< void > > NAVPoseData
Definition: NAVPoseData.h:120
sick_scan_xd::NAV350ImkLandmark::y_mm
int32_t y_mm
Definition: sick_nav_scandata.h:225
sick_scan_xd::NAV350OptPoseData::navMode
uint8_t navMode
Definition: sick_nav_scandata.h:76
sick_nav_scandata_parser.h
sick_scan_xd::NAV350OptPoseData::infoState
uint32_t infoState
Definition: sick_nav_scandata.h:77
sick_scan_xd::SickGenericParser
Definition: sick_generic_parser.h:239
geometry_msgs::TransformStamped
::geometry_msgs::TransformStamped_< std::allocator< void > > TransformStamped
Definition: TransformStamped.h:60
sick_scan_xd::NAV350RemissionData::data
std::vector< uint16_t > data
Definition: sick_nav_scandata.h:196
sick_scan_xd::NAV350mNPOSData::scanData
std::vector< NAV350ScanData > scanData
Definition: sick_nav_scandata.h:212
sick_scan_xd::NAV350RemissionData::startAngle
int32_t startAngle
Definition: sick_nav_scandata.h:192
sick_scan_xd::NAV350mNPOSData::scanDataValid
uint16_t scanDataValid
Definition: sick_nav_scandata.h:211
sick_scan_xd::NAV350ImkLandmark::globID
uint16_t globID
Definition: sick_nav_scandata.h:223
sick_scan_xd::NAV350LandmarkData::landmarkFilter
uint8_t landmarkFilter
Definition: sick_nav_scandata.h:151
sick_scan_xd::NAV350LandmarkDataDoMappingResponse::errorCode
uint8_t errorCode
Definition: sick_nav_scandata.h:160
SoftwarePLL::getCorrectedTimeStamp
bool getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32_t tick)
Definition: softwarePLL.cpp:226
sick_scan_xd::NAV350RemissionData::scaleFactor
float scaleFactor
Definition: sick_nav_scandata.h:190
sick_scan_xd::NAV350ImkLandmark::subType
uint16_t subType
Definition: sick_nav_scandata.h:227
sick_scan_xd::NAV350mNPOSData
Definition: sick_nav_scandata.h:200
sick_scan_xd::NAV350RemissionData::numData
uint16_t numData
Definition: sick_nav_scandata.h:195
sick_scan_xd
Definition: abstract_parser.cpp:65
sick_scan_common.h
f
f
sick_scan_xd::NAV350PoseData::optPoseDataValid
uint16_t optPoseDataValid
Definition: sick_nav_scandata.h:88
DataDumper::binDataToAsciiString
static std::string binDataToAsciiString(const uint8_t *binary_data, int length)
Definition: dataDumper.cpp:108
sick_scan_xd::NAV350ScanData
Definition: sick_nav_scandata.h:172
sick_scan_xd::NAV350RemissionData::angleRes
uint16_t angleRes
Definition: sick_nav_scandata.h:193
sick_scan_xd::NAV350LandmarkData
Definition: sick_nav_scandata.h:148
sick_scan_xd::readFromBuffer
static bool readFromBuffer(const uint8_t *receiveBuffer, int &pos, int receiveBufferLength, T &value, const char *file, int line)
Definition: sick_nav_scandata_parser.cpp:78
ROS_HEADER_SEQ
#define ROS_HEADER_SEQ(msgheader, value)
Definition: sick_ros_wrapper.h:162
sick_scan_xd::NAV350ImkLandmark::layerID
std::vector< uint16_t > layerID
Definition: sick_nav_scandata.h:229
sick_scan_xd::NAV350mNPOSData::mask
uint8_t mask
Definition: sick_nav_scandata.h:206
sick_scan_xd::writeNAV350BinaryPositionData
static void writeNAV350BinaryPositionData(const NAV350mNPOSData &navdata, std::vector< uint8_t > &data_buffer)
Definition: sick_nav_scandata_parser.cpp:95
nsec
uint32_t nsec(const rosTime &time)
Definition: sick_ros_wrapper.h:175
sensor_msgs::LaserScan
::sensor_msgs::LaserScan_< std::allocator< void > > LaserScan
Definition: LaserScan.h:94
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
Definition: sick_scan_logging.h:123
sick_scan_xd::ScannerBasicParam::getScanAngleShift
double getScanAngleShift()
Definition: sick_generic_parser.cpp:505
ROS_INFO_STREAM
#define ROS_INFO_STREAM(...)
Definition: sick_scan_ros2_example.cpp:71
softwarePLL.h
imu_delay_tester.timestamp
timestamp
Definition: imu_delay_tester.py:142
sick_scan_xd::parseNAV350BinaryLandmarkDataDoMappingResponse
bool parseNAV350BinaryLandmarkDataDoMappingResponse(const uint8_t *receiveBuffer, int receiveBufferLength, NAV350LandmarkDataDoMappingResponse &landmarkData)
Definition: sick_nav_scandata_parser.cpp:466
rosTimeToSeconds
double rosTimeToSeconds(const rosTime &time)
Definition: sick_ros_wrapper.h:179
sick_scan_xd::NAV350OptPoseData::quantUsedReflectors
uint8_t quantUsedReflectors
Definition: sick_nav_scandata.h:78
visualization_msgs::Marker
::visualization_msgs::Marker_< std::allocator< void > > Marker
Definition: Marker.h:193
sick_scan_xd::createNAV350BinarySetSpeedRequest
std::vector< uint8_t > createNAV350BinarySetSpeedRequest(const sick_scan_msg::NAVOdomVelocity &msg)
Definition: sick_nav_scandata_parser.cpp:552
sick_scan_xd::NAV350LandmarkData::numReflectors
uint16_t numReflectors
Definition: sick_nav_scandata.h:152
sick_scan_xd::NAV350RemissionData::contentType
std::string contentType
Definition: sick_nav_scandata.h:189
visualization_msgs::MarkerArray
::visualization_msgs::MarkerArray_< std::allocator< void > > MarkerArray
Definition: MarkerArray.h:50
sick_scan_xd::NAV350OptPoseData::meanDev
int32_t meanDev
Definition: sick_nav_scandata.h:75
sick_scan_xd::rotateXYbyAngleOffset
void rotateXYbyAngleOffset(float &x, float &y, double angle_offset)
Definition: sick_nav_scandata_parser.cpp:764
sick_scan_xd::convertNAVPoseDataToTransform
ros_geometry_msgs::TransformStamped convertNAVPoseDataToTransform(NAV350PoseData &poseData, rosTime recvTimeStamp, double config_time_offset, const std::string &tf_parent_frame_id, const std::string &tf_child_frame_id, SickGenericParser *parser_)
Definition: sick_nav_scandata_parser.cpp:799
sick_scan_xd::NAV350mNPOSData::version
uint16_t version
Definition: sick_nav_scandata.h:203
sick_scan_xd::NAV350ScanData::scaleFactor
float scaleFactor
Definition: sick_nav_scandata.h:176
sick_scan_xd::SickGenericParser::getCurrentParamPtr
ScannerBasicParam * getCurrentParamPtr()
Gets Pointer to parameter object.
Definition: sick_generic_parser.cpp:1040
sick_scan_xd::NAV350ImkLandmark::size_mm
uint16_t size_mm
Definition: sick_nav_scandata.h:228
sick_scan_xd::NAV350LandmarkData::reflectors
std::vector< NAV350ReflectorData > reflectors
Definition: sick_nav_scandata.h:153
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
Definition: sick_scan_logging.h:113
sec
uint32_t sec(const rosTime &time)
Definition: sick_ros_wrapper.h:174
imu_delay_tester.line
line
Definition: imu_delay_tester.py:135
sick_lmd_scandata_parser.h
sick_scan_xd::NAV350mNPOSData::remissionData
NAV350RemissionData remissionData
Definition: sick_nav_scandata.h:214
sick_scan_xd::appendToBuffer
static void appendToBuffer(std::vector< uint8_t > &data_buffer, const T &value)
Definition: sick_nav_scandata_parser.cpp:68
sick_scan_xd::NAV350ScanData::data
std::vector< uint32_t > data
Definition: sick_nav_scandata.h:182
ros::Time
sick_scan_xd::NAV350ScanData::numData
uint16_t numData
Definition: sick_nav_scandata.h:181
sick_scan_xd::NAV350ImkLandmark::type
uint8_t type
Definition: sick_nav_scandata.h:226
sick_scan_xd::NAV350LandmarkDataDoMappingResponse::landmarkData
NAV350LandmarkData landmarkData
Definition: sick_nav_scandata.h:162
sick_scan_xd::readNAVIMKfile
std::vector< sick_scan_xd::NAV350ImkLandmark > readNAVIMKfile(const std::string &nav_imk_file)
Definition: sick_nav_scandata_parser.cpp:908
sick_scan_xd::SickGenericParser::get_range_max
float get_range_max(void)
Getting maximum range.
Definition: sick_generic_parser.cpp:1509
SoftwarePLL::IsInitialized
bool IsInitialized() const
Definition: softwarePLL.h:47
sick_scan_xd::NAV350RemissionData::timestampStart
uint32_t timestampStart
Definition: sick_nav_scandata.h:194
sick_scan_xd::NAV350PoseData::optPoseData
NAV350OptPoseData optPoseData
Definition: sick_nav_scandata.h:89
sick_scan_xd::NAV350OptPoseData::outputMode
uint8_t outputMode
Definition: sick_nav_scandata.h:73
sick_scan_xd::NAV350mNPOSData::errorCode
uint8_t errorCode
Definition: sick_nav_scandata.h:204
tf2::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: melodic/include/tf2/LinearMath/Quaternion.h:29
sick_scan_xd::NAV350mNPOSData::landmarkDataValid
uint16_t landmarkDataValid
Definition: sick_nav_scandata.h:209
sick_scan_xd::createNAV350BinaryAddLandmarkRequest
std::vector< uint8_t > createNAV350BinaryAddLandmarkRequest(const NAV350LandmarkData &landmarkData, int nav_curr_layer)
Definition: sick_nav_scandata_parser.cpp:509
sick_scan_xd::SickGenericParser::get_range_min
float get_range_min(void)
Getting minimum range.
Definition: sick_generic_parser.cpp:1519
rosDurationFromSec
rosDuration rosDurationFromSec(double seconds)
Definition: sick_ros_wrapper.h:211
tf
sick_scan_xd::NAV350mNPOSData::wait
uint8_t wait
Definition: sick_nav_scandata.h:205
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(...)
Definition: sick_scan_ros2_example.cpp:72
sick_scan_xd::NAV350mNPOSData::poseDataValid
uint16_t poseDataValid
Definition: sick_nav_scandata.h:207
sick_scan_xd::NAV350PoseData::phi
uint32_t phi
Definition: sick_nav_scandata.h:87
args
roswrap::message_traits::header
std_msgs::Header * header(M &m)
returns Header<M>::pointer(m);
Definition: message_traits.h:281
sick_scan_xd::NAV350PoseData::x
int32_t x
Definition: sick_nav_scandata.h:85
sick_scan_xd::incSoftwarePLLPacketReceived
void incSoftwarePLLPacketReceived()
Definition: sick_lmd_scandata_parser.cpp:68
sick_scan_xd::NAV350OptPoseData::timestamp
uint32_t timestamp
Definition: sick_nav_scandata.h:74
sick_scan_xd::parseNAV350BinaryPositionData
bool parseNAV350BinaryPositionData(const uint8_t *receiveBuffer, int receiveBufferLength, NAV350mNPOSData &navdata)
Definition: sick_nav_scandata_parser.cpp:344
sick_scan_xd::NAV350PoseData
Definition: sick_nav_scandata.h:82
sick_scan_xd::NAVOdomVelocity
::sick_scan_xd::NAVOdomVelocity_< std::allocator< void > > NAVOdomVelocity
Definition: NAVOdomVelocity.h:69
sick_scan_xd::NAV350PoseData::y
int32_t y
Definition: sick_nav_scandata.h:86
sick_scan_xd::convertNAVCartPos3DtoROSPos3D
void convertNAVCartPos3DtoROSPos3D(int32_t nav_posx_mm, int32_t nav_posy_mm, uint32_t nav_phi_mdeg, float &ros_posx_m, float &ros_posy_m, float &ros_yaw_rad, double nav_angle_offset)
Definition: sick_nav_scandata_parser.cpp:792
sick_scan_xd::convertNAVCartPos2DtoROSPos2D
void convertNAVCartPos2DtoROSPos2D(int32_t nav_posx_mm, int32_t nav_posy_mm, float &ros_posx_m, float &ros_posy_m, double nav_angle_offset)
Definition: sick_nav_scandata_parser.cpp:784
sick_scan_xd::NAV350mNPOSData::poseData
NAV350PoseData poseData
Definition: sick_nav_scandata.h:208
rosTime
ros::Time rosTime
Definition: sick_ros_wrapper.h:172
sick_scan_xd::NAV350LandmarkDataDoMappingResponse::landmarkDataValid
uint16_t landmarkDataValid
Definition: sick_nav_scandata.h:161
sick_scan_xd::parseNAV350BinaryLandmarkData
bool parseNAV350BinaryLandmarkData(const uint8_t *receiveBuffer, int &receivePos, int receiveBufferLength, NAV350LandmarkData &landmarkData)
Definition: sick_nav_scandata_parser.cpp:301

sick_scan_xd
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Fri Oct 25 2024 02:47:10