sick_mrs1000_parser.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017, 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  * Author: Sebastian Pütz <spuetz@uos.de>
30  *
31  */
32 
34 
35 #include <ros/ros.h>
36 
37 namespace sick_tim
38 {
39 
41  override_range_min_(0.2),
42  override_range_max_(64.0),
43  override_time_increment_(-1.0f),
44  modifier_(cloud_),
45  x_iter((modifier_.setPointCloud2FieldsByString(1, "xyz"), sensor_msgs::PointCloud2Iterator<float>(cloud_, "x"))),
46  y_iter(cloud_, "y"), z_iter(cloud_, "z"), layer_count_(0)
47 
48 {
49 }
50 
52 {
53 }
54 
55 int SickMRS1000Parser::parse_datagram(char* datagram, size_t datagram_length, SickTimConfig &config,
56  sensor_msgs::LaserScan &scan, sensor_msgs::PointCloud2& cloud)
57 {
58  // Only allow config changes for a whole new cloud / scan and not for partial fragments of a cloud.
59  if(layer_count_ == 0)
60  {
61  current_config_ = config;
62  }
63 
64  static const size_t HEADER_FIELDS = 32;
65  char* cur_field;
66  size_t count;
67 
68  // Reserve sufficient space
69  std::vector<char *> fields;
70  fields.reserve(datagram_length / 2);
71 
72  // ----- only for debug output
73  char datagram_copy[datagram_length + 1];
74  strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
75  datagram_copy[datagram_length] = 0;
76 
77  // ----- tokenize
78  cur_field = strtok(datagram, " ");
79 
80  while (cur_field != NULL)
81  {
82  fields.push_back(cur_field);
83  cur_field = strtok(NULL, " ");
84  }
85 
86  count = fields.size();
87 
88  // Validate header. Total number of tokens is highly unreliable as this may
89  // change when you change the scanning range or the device name using SOPAS ET
90  // tool. The header remains stable, however.
91  if (count < HEADER_FIELDS)
92  {
93  ROS_WARN(
94  "received less fields than minimum fields (actual: %zu, minimum: %zu), ignoring scan", count, HEADER_FIELDS);
95  ROS_WARN("are you using the correct node? (124 --> sick_tim310_1130000m01, > 33 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)");
96  // ROS_DEBUG("received message was: %s", datagram_copy);
97  return ExitError;
98  }
99  if (strcmp(fields[20], "DIST1"))
100  {
101  ROS_WARN("Field 20 of received data is not equal to DIST1 (%s). Unexpected data, ignoring scan", fields[20]);
102  return ExitError;
103  }
104 
105  // More in depth checks: check data length and RSSI availability
106  // 25: Number of data (<= 10F)
107  unsigned short int number_of_data = 0;
108  sscanf(fields[25], "%hx", &number_of_data);
109 
110  if (number_of_data < 1 || number_of_data > 1101)
111  {
112  ROS_WARN("Data length is outside acceptable range 1-1101 (%d). Ignoring scan", number_of_data);
113  return ExitError;
114  }
115  if (count < HEADER_FIELDS + number_of_data)
116  {
117  ROS_WARN("Less fields than expected for %d data points (%zu). Ignoring scan", number_of_data, count);
118  return ExitError;
119  }
120  ROS_DEBUG("Number of data: %d", number_of_data);
121 
122  // Calculate offset of field that contains indicator of whether or not RSSI data is included
123  size_t rssi_idx = 26 + number_of_data;
124  int tmp;
125  sscanf(fields[rssi_idx], "%d", &tmp);
126  bool rssi = tmp > 0;
127  unsigned short int number_of_rssi_data = 0;
128  if (rssi)
129  {
130  sscanf(fields[rssi_idx + 6], "%hx", &number_of_rssi_data);
131 
132  // Number of RSSI data should be equal to number of data
133  if (number_of_rssi_data != number_of_data)
134  {
135  ROS_WARN("Number of RSSI data is lower than number of range data (%d vs %d", number_of_data, number_of_rssi_data);
136  return ExitError;
137  }
138 
139  // Check if the total length is still appropriate.
140  // RSSI data size = number of RSSI readings + 6 fields describing the data
141  if (count < HEADER_FIELDS + number_of_data + number_of_rssi_data + 6)
142  {
143  ROS_WARN("Less fields than expected for %d data points (%zu). Ignoring scan", number_of_data, count);
144  return ExitError;
145  }
146 
147  if (strcmp(fields[rssi_idx + 1], "RSSI1"))
148  {
149  ROS_WARN("Field %zu of received data is not equal to RSSI1 (%s). Unexpected data, ignoring scan", rssi_idx + 1, fields[rssi_idx + 1]);
150  }
151  }
152 
153  short layer = -1;
154  sscanf(fields[15], "%hx", &layer);
155  scan.header.seq = layer;
156 
157  // Only set the frame id for the layer 0, because only the points of that layer 0 lie in a plane.
158  // If the frame_id is not set the caller will not and should not publish the scan.
159  scan.header.frame_id = layer == 0 ? current_config_.frame_id.c_str() : "";
160 
161  // ----- read fields into scan
162  ROS_DEBUG_STREAM("publishing with frame_id " << scan.header.frame_id);
163 
164  ros::Time start_time = ros::Time::now(); // will be adjusted in the end
165 
166  // 16: Scanning Frequency (5DC)
167  unsigned short scanning_freq = -1;
168  sscanf(fields[16], "%hx", &scanning_freq);
169  scan.scan_time = 1.0 / (scanning_freq / 100.0);
170  // ROS_DEBUG("hex: %s, scanning_freq: %d, scan_time: %f", fields[16], scanning_freq, scan.scan_time);
171 
172  // 17: Measurement Frequency (36)
173  unsigned short measurement_freq = -1;
174  sscanf(fields[17], "%hx", &measurement_freq);
175  // Measurement Frequency = Inverse of the time between two measurement shots -> 275 * 4 / 20ms = 55kHz
176  scan.time_increment = 1.0 / (4 * measurement_freq * 100.0);
177  if (override_time_increment_ > 0.0)
178  {
179  // Some lasers may report incorrect measurement frequency
180  scan.time_increment = override_time_increment_;
181  }
182 
183  // 23: Starting angle (FFF92230)
184  int starting_angle = -1;
185  sscanf(fields[23], "%x", &starting_angle);
186  scan.angle_min = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
187  // ROS_DEBUG("starting_angle: %d, angle_min: %f", starting_angle, scan.angle_min);
188 
189  // 24: Angular step width (2710)
190  unsigned short angular_step_width = -1;
191  sscanf(fields[24], "%hx", &angular_step_width);
192  scan.angle_increment = (angular_step_width / 10000.0) / 180.0 * M_PI;
193  scan.angle_max = scan.angle_min + (number_of_data - 1) * scan.angle_increment;
194 
195  // 25: Number of data (<= 10F)
196  // This is already determined above in number_of_data
197 
198  // adjust angle_min to min_ang config param
199  int index_min = 0;
200  while (scan.angle_min + scan.angle_increment < current_config_.min_ang)
201  {
202  scan.angle_min += scan.angle_increment;
203  index_min++;
204  }
205 
206  // adjust angle_max to max_ang config param
207  int index_max = number_of_data - 1;
208  while (scan.angle_max - scan.angle_increment > current_config_.max_ang)
209  {
210  scan.angle_max -= scan.angle_increment;
211  index_max--;
212  }
213 
214  ROS_DEBUG("index_min: %d, index_max: %d", index_min, index_max);
215  // ROS_DEBUG("angular_step_width: %d, angle_increment: %f, angle_max: %f", angular_step_width, scan.angle_increment, scan.angle_max);
216 
217  double phi = scan.angle_min;
218  // -250 -> 2.5 => x -> -x/100
219  // radiant(-x/100) -> (-x / 100) * (pi / 180) -> -x * pi / 18000
220  double alpha = -layer * M_PI / 18000;
221 
222  // order of layers: 2, 3, 1, 4
223  // layer 2: +0.0 degree, layer == 0
224  // layer 3: +2.5 degree, layer == -250
225  // layer 1: -2.5 degree, layer == 250
226  // layer 4: +5.0 degree, layer == -500
227 
228  // first layer
229  if(layer == 0){
230  modifier_.resize(4 * (index_max - index_min + 1));
234  // 26..26 + n - 1: Data_1 .. Data_n
235  scan.ranges.resize(index_max - index_min + 1);
236  // set time when first row is received.
237  cloud_.header.stamp = start_time + ros::Duration().fromSec(current_config_.time_offset); // FIXME: will throw runtime error if use_sim_time = true
238  }
239 
240  // check for space, space was allocated if the layer was layer == 0 (Layer2) sometime before.
241  if(modifier_.size() > 0){
242  layer_count_++;
243  for (int j = index_min; j <= index_max; ++j)
244  {
245 
246  unsigned short range;
247  sscanf(fields[j + 26], "%hx", &range);
248  float range_meter = range / 1000.0;
249 
250  // only copy data to laser scan for layer 2 (layer == 0 degree)
251  if(layer == 0)
252  {
253  if (range == 0)
254  scan.ranges[j - index_min] = std::numeric_limits<float>::infinity();
255  else
256  scan.ranges[j - index_min] = range_meter;
257  }
258 
259  /*
260  * Transform point from spherical coordinates to Cartesian coordinates.
261  * Alpha is measured from the xy-plane and not from the
262  * upper z axis ---> use "pi/2 - alpha" for transformation
263  * Simplified sin(pi/2 - alpha) to cos(alpha).
264  * Simplified cos(pi/2 - alpha) to sin(alpha).
265  */
266  *x_iter = range_meter * cos(alpha) * cos(phi);
267  *y_iter = range_meter * cos(alpha) * sin(phi);
268  *z_iter = range_meter * sin(alpha);
269 
270  ++x_iter;
271  ++y_iter;
272  ++z_iter;
273 
274  phi += scan.angle_increment;
275  }
276 
277  // last layer in the ordered list: 0, -250, 250, -500
278  if(layer == -500){
279  ROS_ASSERT_MSG(layer_count_ == 4, "Expected four layers and layer == -500 to be the last layer! Package loss in communication!");
280  layer_count_ = 0;
281  cloud = cloud_;
282  cloud.header.frame_id = current_config_.frame_id.c_str();
283  }
284  }
285 
286  if (current_config_.intensity) {
287  if (rssi)
288  {
289  // 26 + n: RSSI data included
290 
291  // 26 + n + 1 = RSSI Measured Data Contents (RSSI1)
292  // 26 + n + 2 = RSSI scaling factor (3F80000)
293  // 26 + n + 3 = RSSI Scaling offset (0000000)
294  // 26 + n + 4 = RSSI starting angle (equal to Range starting angle)
295  // 26 + n + 5 = RSSI angular step width (equal to Range angular step width)
296  // 26 + n + 6 = RSSI number of data (equal to Range number of data)
297  // 26 + n + 7 .. 26 + n + 7 + n - 1: RSSI_Data_1 .. RSSI_Data_n
298  // 26 + n + 7 + n .. 26 + n + 7 + n + 2 = unknown (but seems to be [0, 1, B] always)
299  // 26 + n + 7 + n + 2 .. count - 4 = device label
300  // count - 3 .. count - 1 = unknown (but seems to be 0 always)
301  // <ETX> (\x03)
302  scan.intensities.resize(index_max - index_min + 1);
303  size_t offset = 26 + number_of_data + 7;
304  for (int j = index_min; j <= index_max; ++j)
305  {
306  unsigned short intensity;
307  sscanf(fields[j + offset], "%hx", &intensity);
308  scan.intensities[j - index_min] = intensity;
309  }
310  } else {
311  ROS_WARN_ONCE("Intensity parameter is enabled, but the scanner is not configured to send RSSI values! "
312  "Please read the section 'Enabling intensity (RSSI) output' here: http://wiki.ros.org/sick_tim.");
313  }
314  }
315 
316  // 26 + n: RSSI data included
317  // IF RSSI not included:
318  // 26 + n + 1 .. 26 + n + 3 = unknown (but seems to be [0, 1, B] always)
319  // 26 + n + 4 .. count - 4 = device label
320  // count - 3 .. count - 1 = unknown (but seems to be 0 always)
321  // <ETX> (\x03)
322 
323  scan.range_min = override_range_min_; // TODO
324  scan.range_max = override_range_max_;
325 
326  // ----- adjust start time
327  // - last scan point = now ==> first scan point = now - number_of_data * time increment
328  double start_time_adjusted = start_time.toSec()
329  - number_of_data * scan.time_increment // shift backward to time of first scan point
330  + index_min * scan.time_increment // shift forward to time of first published scan point
331  + current_config_.time_offset; // add time offset (usually negative) to account for USB latency etc.
332  if (start_time_adjusted >= 0.0) // ensure that ros::Time is not negative (otherwise runtime error)
333  {
334  scan.header.stamp.fromSec(start_time_adjusted);
335  } else {
336  ROS_WARN("ROS time is 0! Did you set the parameter use_sim_time to true?");
337  }
338 
339  // ----- consistency check
340  float expected_time_increment = scan.scan_time * scan.angle_increment / (2.0 * M_PI);
341  if (fabs(expected_time_increment - scan.time_increment) > 0.00001)
342  {
343  ROS_WARN_THROTTLE(60, "The time_increment, scan_time and angle_increment values reported by the scanner are inconsistent! "
344  "Expected time_increment: %.9f, reported time_increment: %.9f. "
345  "Perhaps you should set the parameter time_increment to the expected value. This message will print every 60 seconds.",
346  expected_time_increment, scan.time_increment);
347  }
348 
349  return ExitSuccess;
350 }
351 
353 {
354  override_range_min_ = min;
355 }
356 
358 {
359  override_range_max_ = max;
360 }
361 
363 {
365 }
366 
367 } /* namespace sick_tim */
#define ROS_WARN_THROTTLE(rate,...)
f
virtual int parse_datagram(char *datagram, size_t datagram_length, SickTimConfig &config, sensor_msgs::LaserScan &scan, sensor_msgs::PointCloud2 &cloud)
sensor_msgs::PointCloud2Modifier modifier_
#define ROS_WARN(...)
sensor_msgs::PointCloud2 cloud_
Duration & fromSec(double t)
#define ROS_WARN_ONCE(...)
#define ROS_ASSERT_MSG(cond,...)
#define ROS_DEBUG_STREAM(args)
sick_tim::SickTimConfig current_config_
static Time now()
sensor_msgs::PointCloud2Iterator< float > x_iter
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
sensor_msgs::PointCloud2Iterator< float > y_iter
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
sensor_msgs::PointCloud2Iterator< float > z_iter
#define ROS_DEBUG(...)


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