52 sensor_msgs::LaserScan &msg)
54 static const size_t NUM_FIELDS = 592;
55 char* fields[NUM_FIELDS];
60 char datagram_copy[datagram_length + 1];
61 strncpy(datagram_copy, datagram, datagram_length);
62 datagram_copy[datagram_length] = 0;
66 cur_field = strtok(datagram,
" ");
68 while (cur_field != NULL)
70 if (count < NUM_FIELDS)
71 fields[count++] = cur_field;
74 cur_field = strtok(NULL,
" ");
77 if (count < NUM_FIELDS)
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)");
85 else if (count > NUM_FIELDS)
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)");
94 msg.header.frame_id = config.frame_id;
95 ROS_DEBUG(
"publishing with frame_id %s", config.frame_id.c_str());
115 unsigned short scanning_freq = -1;
116 sscanf(fields[16],
"%hx", &scanning_freq);
117 msg.scan_time = 1.0 / (scanning_freq / 100.0);
121 unsigned short measurement_freq = 36;
123 msg.time_increment = 1.0 / (measurement_freq * 100.0);
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;
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;
153 while (msg.angle_min + msg.angle_increment < config.min_ang)
155 msg.angle_min += msg.angle_increment;
161 while (msg.angle_max - msg.angle_increment > config.max_ang)
163 msg.angle_max -= msg.angle_increment;
167 ROS_DEBUG(
"index_min: %d, index_max: %d", index_min, index_max);
173 msg.ranges.resize(index_max - index_min + 1);
174 for (
int j = index_min; j <= index_max; ++j)
176 unsigned short range;
177 sscanf(fields[j + 28],
"%hx", &range);
179 msg.ranges[j - index_min] = std::numeric_limits<float>::infinity();
181 msg.ranges[j - index_min] = range / 1000.0;
192 if (config.intensity)
194 msg.intensities.resize(index_max - index_min + 1);
195 for (
int j = index_min; j <= index_max; ++j)
197 unsigned short intensity;
198 sscanf(fields[j + 305],
"%hx", &intensity);
199 msg.intensities[j - index_min] = intensity;
210 msg.range_min = 0.05;
215 double start_time_adjusted = start_time.toSec()
216 - 271 * msg.time_increment
217 + index_min * msg.time_increment
218 + config.time_offset;
219 if (start_time_adjusted >= 0.0)
221 msg.header.stamp.fromSec(start_time_adjusted);
223 ROS_WARN(
"ROS time is 0! Did you set the parameter use_sim_time to true?");
virtual ~SickTim310Parser()
virtual int parse_datagram(char *datagram, size_t datagram_length, SickTimConfig &config, sensor_msgs::LaserScan &msg)