sick_tim551_2050001_parser.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013, Osnabrück University
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of Osnabrück University nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Created on: 14.11.2013
30  *
31  * Author: Martin Günther <mguenthe@uos.de>
32  *
33  */
34 
36 
37 #include <ros/ros.h>
38 
39 namespace sick_tim
40 {
41 
44  override_range_min_(0.05),
45  override_range_max_(10.0),
46  override_time_increment_(-1.0)
47 {
48 }
49 
51 {
52 }
53 
54 int SickTim5512050001Parser::parse_datagram(char* datagram, size_t datagram_length, SickTimConfig &config,
55  sensor_msgs::LaserScan &msg)
56 {
57  // general message structure:
58  //
59  // - message header 20 fields
60  // - DIST1 header 6 fields
61  // - DIST1 data N fields
62  // - RSSI included? 1 field
63  // - RSSI1 header 6 fields (optional)
64  // - RSSI1 data N fields (optional)
65  // - footer >= 5 fields, depending on number of spaces in device label
66  static const size_t HEADER_FIELDS = 26;
67  static const size_t MIN_FOOTER_FIELDS = 5;
68  char* cur_field;
69  size_t count;
70 
71  // Reserve sufficient space
72  std::vector<char *> fields;
73  fields.reserve(datagram_length / 2);
74 
75  // ----- only for debug output
76  char datagram_copy[datagram_length + 1];
77  strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
78  datagram_copy[datagram_length] = 0;
79 
80  // ----- tokenize
81  count = 0;
82  cur_field = strtok(datagram, " ");
83 
84  while (cur_field != NULL)
85  {
86  fields.push_back(cur_field);
87  cur_field = strtok(NULL, " ");
88  }
89 
90  count = fields.size();
91 
92  // Validate header. Total number of tokens is highly unreliable as this may
93  // change when you change the scanning range or the device name using SOPAS ET
94  // tool. The header remains stable, however.
95  if (count < HEADER_FIELDS + 1 + MIN_FOOTER_FIELDS)
96  {
97  ROS_WARN(
98  "received less fields than minimum fields (actual: %zu, minimum: %zu), ignoring scan", count, HEADER_FIELDS + 1 + MIN_FOOTER_FIELDS);
99  ROS_WARN("are you using the correct node? (124 --> sick_tim310_1130000m01, > 32 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)");
100  // ROS_DEBUG("received message was: %s", datagram_copy);
101  return ExitError;
102  }
103  if (strcmp(fields[15], "0"))
104  {
105  ROS_WARN("Field 15 of received data is not equal to 0 (%s). Unexpected data, ignoring scan", fields[15]);
106  return ExitError;
107  }
108  if (strcmp(fields[20], "DIST1"))
109  {
110  ROS_WARN("Field 20 of received data is not equal to DIST1i (%s). Unexpected data, ignoring scan", fields[20]);
111  return ExitError;
112  }
113 
114  // More in depth checks: check data length and RSSI availability
115  // 25: Number of data (<= 10F)
116  unsigned short int number_of_data = 0;
117  sscanf(fields[25], "%hx", &number_of_data);
118 
119  if (number_of_data < 1 || number_of_data > 811)
120  {
121  ROS_WARN("Data length is outside acceptable range 1-811 (%d). Ignoring scan", number_of_data);
122  return ExitError;
123  }
124  if (count < HEADER_FIELDS + number_of_data + 1 + MIN_FOOTER_FIELDS)
125  {
126  ROS_WARN("Less fields than expected (expected: >= %zu, actual: %zu). Ignoring scan",
127  HEADER_FIELDS + number_of_data + 1 + MIN_FOOTER_FIELDS, count);
128  return ExitError;
129  }
130  ROS_DEBUG("Number of data: %d", number_of_data);
131 
132  // Calculate offset of field that contains indicator of whether or not RSSI data is included
133  size_t rssi_idx = HEADER_FIELDS + number_of_data;
134  int tmp;
135  sscanf(fields[rssi_idx], "%d", &tmp);
136  bool rssi = tmp > 0;
137  unsigned short int number_of_rssi_data = 0;
138  if (rssi)
139  {
140  sscanf(fields[rssi_idx + 6], "%hx", &number_of_rssi_data);
141 
142  // Number of RSSI data should be equal to number of data
143  if (number_of_rssi_data != number_of_data)
144  {
145  ROS_WARN("Number of RSSI data (%d) is not equal to number of range data (%d)", number_of_rssi_data, number_of_data);
146  return ExitError;
147  }
148 
149  // Check if the total length is still appropriate.
150  // RSSI data size = number of RSSI readings + 6 fields describing the data
151  if (count < HEADER_FIELDS + number_of_data + 1 + 6 + number_of_rssi_data + MIN_FOOTER_FIELDS)
152  {
153  ROS_WARN("Less fields than expected with RSSI data (expected: >= %zu, actual: %zu). Ignoring scan",
154  HEADER_FIELDS + number_of_data + 1 + 6 + number_of_rssi_data + MIN_FOOTER_FIELDS, count);
155  return ExitError;
156  }
157 
158  if (strcmp(fields[rssi_idx + 1], "RSSI1"))
159  {
160  ROS_WARN("Field %zu of received data is not equal to RSSI1 (%s). Unexpected data, ignoring scan", rssi_idx + 1, fields[rssi_idx + 1]);
161  }
162  }
163 
164  // ----- read fields into msg
165  msg.header.frame_id = config.frame_id;
166  ROS_DEBUG("publishing with frame_id %s", config.frame_id.c_str());
167 
168  ros::Time start_time = ros::Time::now(); // will be adjusted in the end
169 
170  // <STX> (\x02)
171  // 0: Type of command (SN)
172  // 1: Command (LMDscandata)
173  // 2: Firmware version number (1)
174  // 3: Device number (1)
175  // 4: Serial number (eg. B96518)
176  // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
177  // 7: Telegram counter (eg. 99)
178  // 8: Scan counter (eg. 9A)
179  // 9: Time since startup (eg. 13C8E59)
180  // 10: Time of transmission (eg. 13C9CBE)
181  // 11 + 12: Input status (0 0)
182  // 13 + 14: Output status (8 0)
183  // 15: Reserved Byte A (0)
184 
185  // 16: Scanning Frequency (5DC)
186  unsigned short scanning_freq = -1;
187  sscanf(fields[16], "%hx", &scanning_freq);
188  msg.scan_time = 1.0 / (scanning_freq / 100.0);
189  // ROS_DEBUG("hex: %s, scanning_freq: %d, scan_time: %f", fields[16], scanning_freq, msg.scan_time);
190 
191  // 17: Measurement Frequency (36)
192  unsigned short measurement_freq = -1;
193  sscanf(fields[17], "%hx", &measurement_freq);
194  msg.time_increment = 1.0 / (measurement_freq * 100.0);
195  if (override_time_increment_ > 0.0)
196  {
197  // Some lasers may report incorrect measurement frequency
198  msg.time_increment = override_time_increment_;
199  }
200  // ROS_DEBUG("measurement_freq: %d, time_increment: %f", measurement_freq, msg.time_increment);
201 
202  // 18: Number of encoders (0)
203  // 19: Number of 16 bit channels (1)
204  // 20: Measured data contents (DIST1)
205 
206  // 21: Scaling factor (3F800000)
207  // ignored for now (is always 1.0):
208 // unsigned int scaling_factor_int = -1;
209 // sscanf(fields[21], "%x", &scaling_factor_int);
210 //
211 // float scaling_factor = reinterpret_cast<float&>(scaling_factor_int);
212 // // ROS_DEBUG("hex: %s, scaling_factor_int: %d, scaling_factor: %f", fields[21], scaling_factor_int, scaling_factor);
213 
214  // 22: Scaling offset (00000000) -- always 0
215  // 23: Starting angle (FFF92230)
216  int starting_angle = -1;
217  sscanf(fields[23], "%x", &starting_angle);
218  msg.angle_min = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
219  // ROS_DEBUG("starting_angle: %d, angle_min: %f", starting_angle, msg.angle_min);
220 
221  // 24: Angular step width (2710)
222  unsigned short angular_step_width = -1;
223  sscanf(fields[24], "%hx", &angular_step_width);
224  msg.angle_increment = (angular_step_width / 10000.0) / 180.0 * M_PI;
225  msg.angle_max = msg.angle_min + (number_of_data - 1) * msg.angle_increment;
226 
227  // 25: Number of data (<= 10F)
228  // This is already determined above in number_of_data
229 
230  // adjust angle_min to min_ang config param
231  int index_min = 0;
232  while (msg.angle_min + msg.angle_increment < config.min_ang)
233  {
234  msg.angle_min += msg.angle_increment;
235  index_min++;
236  }
237 
238  // adjust angle_max to max_ang config param
239  int index_max = number_of_data - 1;
240  while (msg.angle_max - msg.angle_increment > config.max_ang)
241  {
242  msg.angle_max -= msg.angle_increment;
243  index_max--;
244  }
245 
246  ROS_DEBUG("index_min: %d, index_max: %d", index_min, index_max);
247  // ROS_DEBUG("angular_step_width: %d, angle_increment: %f, angle_max: %f", angular_step_width, msg.angle_increment, msg.angle_max);
248 
249  // 26..26 + n - 1: Data_1 .. Data_n
250  msg.ranges.resize(index_max - index_min + 1);
251  for (int j = index_min; j <= index_max; ++j)
252  {
253  unsigned short range;
254  sscanf(fields[j + HEADER_FIELDS], "%hx", &range);
255  if (range == 0)
256  msg.ranges[j - index_min] = std::numeric_limits<float>::infinity();
257  else
258  msg.ranges[j - index_min] = range / 1000.0;
259  }
260 
261  if (config.intensity) {
262  if (rssi)
263  {
264  // 26 + n: RSSI data included
265 
266  // 26 + n + 1 = RSSI Measured Data Contents (RSSI1)
267  // 26 + n + 2 = RSSI scaling factor (3F80000)
268  // 26 + n + 3 = RSSI Scaling offset (0000000)
269  // 26 + n + 4 = RSSI starting angle (equal to Range starting angle)
270  // 26 + n + 5 = RSSI angular step width (equal to Range angular step width)
271  // 26 + n + 6 = RSSI number of data (equal to Range number of data)
272  // 26 + n + 7 .. 26 + n + 7 + n - 1: RSSI_Data_1 .. RSSI_Data_n
273  // 26 + n + 7 + n = unknown (seems to be always 0)
274  // 26 + n + 7 + n + 1 = device label included? (0 = no, 1 = yes)
275  // 26 + n + 7 + n + 2 .. count - 4 = device label as a length-prefixed string, e.g. 0xA "Scipio_LRF" or 0xB "not defined"
276  // count - 3 .. count - 1 = unknown (but seems to be 0 always)
277  // <ETX> (\x03)
278  msg.intensities.resize(index_max - index_min + 1);
279  size_t offset = HEADER_FIELDS + number_of_data + 7;
280  for (int j = index_min; j <= index_max; ++j)
281  {
282  unsigned short intensity;
283  sscanf(fields[j + offset], "%hx", &intensity);
284  msg.intensities[j - index_min] = intensity;
285  }
286  } else {
287  ROS_WARN_ONCE("Intensity parameter is enabled, but the scanner is not configured to send RSSI values! "
288  "Please read the section 'Enabling intensity (RSSI) output' here: http://wiki.ros.org/sick_tim.");
289  }
290  }
291 
292  // 26 + n: RSSI data included
293  // IF RSSI not included:
294  // 26 + n + 1 .. 26 + n + 3 = unknown (but seems to be [0, 1, B] always)
295  // 26 + n + 4 .. count - 4 = device label
296  // count - 3 .. count - 1 = unknown (but seems to be 0 always)
297  // <ETX> (\x03)
298 
299  msg.range_min = override_range_min_;
300  msg.range_max = override_range_max_;
301 
302  // ----- adjust start time
303  // - last scan point = now ==> first scan point = now - number_of_data * time increment
304  double start_time_adjusted = start_time.toSec()
305  - number_of_data * msg.time_increment // shift backward to time of first scan point
306  + index_min * msg.time_increment // shift forward to time of first published scan point
307  + config.time_offset; // add time offset (usually negative) to account for USB latency etc.
308  if (start_time_adjusted >= 0.0) // ensure that ros::Time is not negative (otherwise runtime error)
309  {
310  msg.header.stamp.fromSec(start_time_adjusted);
311  } else {
312  ROS_WARN("ROS time is 0! Did you set the parameter use_sim_time to true?");
313  }
314 
315  // ----- consistency check
316  float expected_time_increment = msg.scan_time * msg.angle_increment / (2.0 * M_PI);
317  if (fabs(expected_time_increment - msg.time_increment) > 0.00001)
318  {
319  ROS_WARN_THROTTLE(60, "The time_increment, scan_time and angle_increment values reported by the scanner are inconsistent! "
320  "Expected time_increment: %.9f, reported time_increment: %.9f. "
321  "Perhaps you should set the parameter time_increment to the expected value. This message will print every 60 seconds.",
322  expected_time_increment, msg.time_increment);
323  }
324 
325  return ExitSuccess;
326 }
327 
329 {
330  override_range_min_ = min;
331 }
332 
334 {
335  override_range_max_ = max;
336 }
337 
339 {
341 }
342 
343 } /* namespace sick_tim */
#define ROS_WARN_THROTTLE(rate,...)
virtual int parse_datagram(char *datagram, size_t datagram_length, SickTimConfig &config, sensor_msgs::LaserScan &msg)
#define ROS_WARN(...)
#define ROS_WARN_ONCE(...)
static Time now()
#define ROS_DEBUG(...)


sick_tim
Author(s): Jochen Sprickerhof , Martin Günther , Sebastian Pütz
autogenerated on Wed Jun 17 2020 04:05:36