sick_tim310_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: 21.08.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 {
45 }
46 
48 {
49 }
50 
51 int SickTim310Parser::parse_datagram(char* datagram, size_t datagram_length, SickTimConfig &config,
52  sensor_msgs::LaserScan &msg)
53 {
54  static const size_t NUM_FIELDS = 592;
55  char* fields[NUM_FIELDS];
56  char* cur_field;
57  size_t count;
58 
59  // ----- only for debug output
60  char datagram_copy[datagram_length + 1];
61  strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
62  datagram_copy[datagram_length] = 0;
63 
64  // ----- tokenize
65  count = 0;
66  cur_field = strtok(datagram, " ");
67 
68  while (cur_field != NULL)
69  {
70  if (count < NUM_FIELDS)
71  fields[count++] = cur_field;
72 
73  // ROS_DEBUG("%zu: %s ", count, cur_field);
74  cur_field = strtok(NULL, " ");
75  }
76 
77  if (count < NUM_FIELDS)
78  {
79  ROS_WARN(
80  "received less fields than expected fields (actual: %zu, expected: %zu), ignoring scan", count, NUM_FIELDS);
81  ROS_WARN("are you using the correct node? (124 --> sick_tim310_1130000m01, 306 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)");
82  // ROS_DEBUG("received message was: %s", datagram_copy);
83  return ExitError;
84  }
85  else if (count > NUM_FIELDS)
86  {
87  ROS_WARN("received more fields than expected (actual: %zu, expected: %zu), ignoring scan", count, NUM_FIELDS);
88  ROS_WARN("are you using the correct node? (124 --> sick_tim310_1130000m01, 306 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)");
89  // ROS_DEBUG("received message was: %s", datagram_copy);
90  return ExitError;
91  }
92 
93  // ----- read fields into msg
94  msg.header.frame_id = config.frame_id;
95  ROS_DEBUG("publishing with frame_id %s", config.frame_id.c_str());
96 
97  ros::Time start_time = ros::Time::now(); // will be adjusted in the end
98 
99  // <STX> (\x02)
100  // 0: Type of command (SN)
101  // 1: Command (LMDscandata)
102  // 2: Firmware version number (1)
103  // 3: Device number (1)
104  // 4: Serial number (B96518)
105  // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
106  // 7: Telegram counter (99)
107  // 8: Scan counter (9A)
108  // 9: Time since startup (13C8E59)
109  // 10: Time of transmission (13C9CBE)
110  // 11 + 12: Input status (0 0)
111  // 13 + 14: Output status (8 0)
112  // 15: Reserved Byte A (0)
113 
114  // 16: Scanning Frequency (5DC)
115  unsigned short scanning_freq = -1;
116  sscanf(fields[16], "%hx", &scanning_freq);
117  msg.scan_time = 1.0 / (scanning_freq / 100.0);
118  // ROS_DEBUG("hex: %s, scanning_freq: %d, scan_time: %f", fields[16], scanning_freq, msg.scan_time);
119 
120  // 17: Measurement Frequency (36)
121  unsigned short measurement_freq = 36;
122  //sscanf(fields[17], "%hx", &measurement_freq);
123  msg.time_increment = 1.0 / (measurement_freq * 100.0);
124  // ROS_DEBUG("measurement_freq: %d, time_increment: %f", measurement_freq, msg.time_increment);
125 
126  // 18: Number of encoders (0)
127  // 19: Number of 16 bit channels (1)
128  // 22: Measured data contents (DIST1)
129 
130  // 23: Scaling factor (3F800000)
131  // ignored for now (is always 1.0):
132 // unsigned int scaling_factor_int = -1;
133 // sscanf(fields[21], "%x", &scaling_factor_int);
134 //
135 // float scaling_factor = reinterpret_cast<float&>(scaling_factor_int);
136 // // ROS_DEBUG("hex: %s, scaling_factor_int: %d, scaling_factor: %f", fields[21], scaling_factor_int, scaling_factor);
137 
138  // 24: Scaling offset (00000000) -- always 0
139  // 25: Starting angle (FFF92230)
140  int starting_angle = -1;
141  sscanf(fields[25], "%x", &starting_angle);
142  msg.angle_min = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
143  // ROS_DEBUG("starting_angle: %d, angle_min: %f", starting_angle, msg.angle_min);
144 
145  // 26: Angular step width (2710)
146  unsigned short angular_step_width = -1;
147  sscanf(fields[26], "%hx", &angular_step_width);
148  msg.angle_increment = (angular_step_width / 10000.0) / 180.0 * M_PI;
149  msg.angle_max = msg.angle_min + 270.0 * msg.angle_increment;
150 
151  // adjust angle_min to min_ang config param
152  int index_min = 0;
153  while (msg.angle_min + msg.angle_increment < config.min_ang)
154  {
155  msg.angle_min += msg.angle_increment;
156  index_min++;
157  }
158 
159  // adjust angle_max to max_ang config param
160  int index_max = 270;
161  while (msg.angle_max - msg.angle_increment > config.max_ang)
162  {
163  msg.angle_max -= msg.angle_increment;
164  index_max--;
165  }
166 
167  ROS_DEBUG("index_min: %d, index_max: %d", index_min, index_max);
168  // ROS_DEBUG("angular_step_width: %d, angle_increment: %f, angle_max: %f", angular_step_width, msg.angle_increment, msg.angle_max);
169 
170  // 27: Number of data (10F)
171 
172  // 28..298: Data_1 .. Data_n
173  msg.ranges.resize(index_max - index_min + 1);
174  for (int j = index_min; j <= index_max; ++j)
175  {
176  unsigned short range;
177  sscanf(fields[j + 28], "%hx", &range);
178  if (range == 0)
179  msg.ranges[j - index_min] = std::numeric_limits<float>::infinity();
180  else
181  msg.ranges[j - index_min] = range / 1000.0;
182  }
183 
184  // ---297: Number of 8 bit channels (1)---
185  // 299: Measured data contents (RSSI1)
186  // 300: Scaling factor (3F800000)
187  // 301: Scaling offset (00000000)
188  // 302: Starting angle (FFF92230)
189  // 303: Angular step width (2710)
190  // 304: Number of data (10F)
191  // 305..575: Data_1 .. Data_n
192  if (config.intensity)
193  {
194  msg.intensities.resize(index_max - index_min + 1);
195  for (int j = index_min; j <= index_max; ++j)
196  {
197  unsigned short intensity;
198  sscanf(fields[j + 305], "%hx", &intensity);
199  msg.intensities[j - index_min] = intensity;
200  }
201  }
202 
203  // 576: Position (0)
204  // 577: Name (0)
205  // 578: Comment (0)
206  // 579: Time information (0)
207  // 579: Event information (0)
208  // <ETX> (\x03)
209 
210  msg.range_min = 0.05;
211  msg.range_max = 4.0;
212 
213  // ----- adjust start time
214  // - last scan point = now ==> first scan point = now - 271 * time increment
215  double start_time_adjusted = start_time.toSec()
216  - 271 * msg.time_increment // shift backward to time of first scan point
217  + index_min * msg.time_increment // shift forward to time of first published scan point
218  + config.time_offset; // add time offset (usually negative) to account for USB latency etc.
219  if (start_time_adjusted >= 0.0) // ensure that ros::Time is not negative (otherwise runtime error)
220  {
221  msg.header.stamp.fromSec(start_time_adjusted);
222  } else {
223  ROS_WARN("ROS time is 0! Did you set the parameter use_sim_time to true?");
224  }
225 
226 
227  return ExitSuccess;
228 }
229 
230 } /* namespace sick_tim */
#define ROS_WARN(...)
virtual int parse_datagram(char *datagram, size_t datagram_length, SickTimConfig &config, sensor_msgs::LaserScan &msg)
static Time now()
#define ROS_DEBUG(...)


sick_tim
Author(s): Jochen Sprickerhof , Martin Günther , Sebastian Pütz
autogenerated on Tue May 7 2019 03:13:47