sick_tim551_2050001_parser.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2013, Osnabrück University
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Osnabrück University nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  *  Created on: 14.11.2013
00030  *
00031  *      Author: Martin Günther <mguenthe@uos.de>
00032  *
00033  */
00034 
00035 #include <sick_tim3xx/sick_tim551_2050001_parser.h>
00036 
00037 #include <ros/ros.h>
00038 
00039 namespace sick_tim3xx
00040 {
00041 
00042 SickTim5512050001Parser::SickTim5512050001Parser() :
00043     AbstractParser()
00044 {
00045 }
00046 
00047 SickTim5512050001Parser::~SickTim5512050001Parser()
00048 {
00049 }
00050 
00051 int SickTim5512050001Parser::parse_datagram(char* datagram, size_t datagram_length, SickTim3xxConfig &config,
00052                                      sensor_msgs::LaserScan &msg)
00053 {
00054   static const size_t HEADER_FIELDS = 33;
00055   char* cur_field;
00056   size_t count;
00057 
00058   // Reserve sufficient space
00059   std::vector<char *> fields;
00060   fields.reserve(datagram_length / 2);
00061 
00062   // ----- only for debug output
00063   char datagram_copy[datagram_length + 1];
00064   strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
00065   datagram_copy[datagram_length] = 0;
00066 
00067   // ----- tokenize
00068   count = 0;
00069   cur_field = strtok(datagram, " ");
00070   fields.push_back(cur_field);
00071 
00072   while (cur_field != NULL)
00073   {
00074     cur_field = strtok(NULL, " ");
00075     fields.push_back(cur_field);
00076   }
00077 
00078   count = fields.size();
00079 
00080   // Validate header. Total number of tokens is highly unreliable as this may
00081   // change when you change the scanning range or the device name using SOPAS ET
00082   // tool. The header remains stable, however.
00083   if (count < HEADER_FIELDS)
00084   {
00085     ROS_WARN(
00086         "received less fields than minimum fields (actual: %zu, minimum: %zu), ignoring scan", count, HEADER_FIELDS);
00087     ROS_WARN("are you using the correct node? (124 --> sick_tim310_1130000m01, > 33 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)");
00088     // ROS_DEBUG("received message was: %s", datagram_copy);
00089     return EXIT_FAILURE;
00090   }
00091   if (strcmp(fields[15], "0"))
00092   {
00093     ROS_WARN("Field 15 of received data is not equal to 0 (%s). Unexpected data, ignoring scan", fields[15]);
00094     return EXIT_FAILURE;
00095   }
00096   if (strcmp(fields[20], "DIST1"))
00097   {
00098     ROS_WARN("Field 20 of received data is not equal to DIST1i (%s). Unexpected data, ignoring scan", fields[20]);
00099     return EXIT_FAILURE;
00100   }
00101 
00102   // More in depth checks: check data length and RSSI availability
00103   // 25: Number of data (<= 10F)
00104   unsigned short int number_of_data = 0;
00105   sscanf(fields[25], "%hx", &number_of_data);
00106 
00107   if (number_of_data < 1 || number_of_data > 271)
00108   {
00109     ROS_WARN("Data length is outside acceptable range 1-271 (%d). Ignoring scan", number_of_data);
00110     return EXIT_FAILURE;
00111   }
00112   if (count < HEADER_FIELDS + number_of_data)
00113   {
00114     ROS_WARN("Less fields than expected for %d data points (%zu). Ignoring scan", number_of_data, count);
00115     return EXIT_FAILURE;
00116   }
00117   ROS_DEBUG("Number of data: %d", number_of_data);
00118 
00119   // Calculate offset of field that contains indicator of whether or not RSSI data is included
00120   size_t rssi_idx = 26 + number_of_data;
00121   int tmp;
00122   sscanf(fields[rssi_idx], "%d", &tmp);
00123   bool rssi = tmp > 0;
00124   unsigned short int number_of_rssi_data = 0;
00125   if (rssi)
00126   {
00127     sscanf(fields[rssi_idx + 6], "%hx", &number_of_rssi_data);
00128 
00129     // Number of RSSI data should be equal to number of data
00130     if (number_of_rssi_data != number_of_data)
00131     {
00132       ROS_WARN("Number of RSSI data is lower than number of range data (%d vs %d", number_of_data, number_of_rssi_data);
00133       return EXIT_FAILURE;
00134     }
00135 
00136     // Check if the total length is still appropriate.
00137     // RSSI data size = number of RSSI readings + 6 fields describing the data
00138     if (count < HEADER_FIELDS + number_of_data + number_of_rssi_data + 6)
00139     {
00140       ROS_WARN("Less fields than expected for %d data points (%zu). Ignoring scan", number_of_data, count);
00141       return EXIT_FAILURE;
00142     }
00143 
00144     if (strcmp(fields[rssi_idx + 1], "RSSI1"))
00145     {
00146       ROS_WARN("Field %zu of received data is not equal to RSSI1 (%s). Unexpected data, ignoring scan", rssi_idx + 1, fields[rssi_idx + 1]);
00147     }
00148   }
00149 
00150   // ----- read fields into msg
00151   msg.header.frame_id = config.frame_id;
00152   ROS_DEBUG("publishing with frame_id %s", config.frame_id.c_str());
00153 
00154   ros::Time start_time = ros::Time::now(); // will be adjusted in the end
00155 
00156   // <STX> (\x02)
00157   // 0: Type of command (SN)
00158   // 1: Command (LMDscandata)
00159   // 2: Firmware version number (1)
00160   // 3: Device number (1)
00161   // 4: Serial number (eg. B96518)
00162   // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
00163   // 7: Telegram counter (eg. 99)
00164   // 8: Scan counter (eg. 9A)
00165   // 9: Time since startup (eg. 13C8E59)
00166   // 10: Time of transmission (eg. 13C9CBE)
00167   // 11 + 12: Input status (0 0)
00168   // 13 + 14: Output status (8 0)
00169   // 15: Reserved Byte A (0)
00170 
00171   // 16: Scanning Frequency (5DC)
00172   unsigned short scanning_freq = -1;
00173   sscanf(fields[16], "%hx", &scanning_freq);
00174   msg.scan_time = 1.0 / (scanning_freq / 100.0);
00175   // ROS_DEBUG("hex: %s, scanning_freq: %d, scan_time: %f", fields[16], scanning_freq, msg.scan_time);
00176 
00177   // 17: Measurement Frequency (36)
00178   unsigned short measurement_freq = -1;
00179   sscanf(fields[17], "%hx", &measurement_freq);
00180   msg.time_increment = 1.0 / (measurement_freq * 100.0);
00181   // ROS_DEBUG("measurement_freq: %d, time_increment: %f", measurement_freq, msg.time_increment);
00182 
00183   // 18: Number of encoders (0)
00184   // 19: Number of 16 bit channels (1)
00185   // 20: Measured data contents (DIST1)
00186 
00187   // 21: Scaling factor (3F800000)
00188   // ignored for now (is always 1.0):
00189 //      unsigned int scaling_factor_int = -1;
00190 //      sscanf(fields[21], "%x", &scaling_factor_int);
00191 //
00192 //      float scaling_factor = reinterpret_cast<float&>(scaling_factor_int);
00193 //      // ROS_DEBUG("hex: %s, scaling_factor_int: %d, scaling_factor: %f", fields[21], scaling_factor_int, scaling_factor);
00194 
00195   // 22: Scaling offset (00000000) -- always 0
00196   // 23: Starting angle (FFF92230)
00197   int starting_angle = -1;
00198   sscanf(fields[23], "%x", &starting_angle);
00199   msg.angle_min = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
00200   // ROS_DEBUG("starting_angle: %d, angle_min: %f", starting_angle, msg.angle_min);
00201 
00202   // 24: Angular step width (2710)
00203   unsigned short angular_step_width = -1;
00204   sscanf(fields[24], "%hx", &angular_step_width);
00205   msg.angle_increment = (angular_step_width / 10000.0) / 180.0 * M_PI;
00206   msg.angle_max = msg.angle_min + (number_of_data - 1) * msg.angle_increment;
00207 
00208   // 25: Number of data (<= 10F)
00209   // This is already determined above in number_of_data
00210 
00211   // adjust angle_min to min_ang config param
00212   int index_min = 0;
00213   while (msg.angle_min + msg.angle_increment < config.min_ang)
00214   {
00215     msg.angle_min += msg.angle_increment;
00216     index_min++;
00217   }
00218 
00219   // adjust angle_max to max_ang config param
00220   int index_max = number_of_data - 1;
00221   while (msg.angle_max - msg.angle_increment > config.max_ang)
00222   {
00223     msg.angle_max -= msg.angle_increment;
00224     index_max--;
00225   }
00226 
00227   ROS_DEBUG("index_min: %d, index_max: %d", index_min, index_max);
00228   // ROS_DEBUG("angular_step_width: %d, angle_increment: %f, angle_max: %f", angular_step_width, msg.angle_increment, msg.angle_max);
00229 
00230 
00231   // 26..26 + n - 1: Data_1 .. Data_n
00232   msg.ranges.resize(index_max - index_min + 1);
00233   for (int j = index_min; j <= index_max; ++j)
00234   {
00235     unsigned short range;
00236     sscanf(fields[j + 26], "%hx", &range);
00237     msg.ranges[j - index_min] = range / 1000.0;
00238   }
00239 
00240   if (rssi)
00241   {
00242     // 26 + n: RSSI data included
00243 
00244     //   26 + n + 1 = RSSI Measured Data Contents (RSSI1)
00245     //   26 + n + 2 = RSSI scaling factor (3F80000)
00246     //   26 + n + 3 = RSSI Scaling offset (0000000)
00247     //   26 + n + 4 = RSSI starting angle (equal to Range starting angle)
00248     //   26 + n + 5 = RSSI angular step width (equal to Range angular step width)
00249     //   26 + n + 6 = RSSI number of data (equal to Range number of data)
00250     //   26 + n + 7 .. 26 + n + 7 + n - 1: RSSI_Data_1 .. RSSI_Data_n
00251     //   26 + n + 7 + n .. 26 + n + 7 + n + 2 = unknown (but seems to be [0, 1, B] always)
00252     //   26 + n + 7 + n + 2 .. count - 4 = device label
00253     //   count - 3 .. count - 1 = unknown (but seems to be 0 always)
00254     //   <ETX> (\x03)
00255     msg.intensities.resize(index_max - index_min + 1);
00256     size_t offset = 26 + number_of_data + 7;
00257     for (int j = index_min; j <= index_max; ++j)
00258     {
00259       unsigned short intensity;
00260       sscanf(fields[j + offset], "%hx", &intensity);
00261       msg.intensities[j - index_min] = intensity;
00262     }
00263   }
00264 
00265   // 26 + n: RSSI data included
00266   // IF RSSI not included:
00267   //   26 + n + 1 .. 26 + n + 3 = unknown (but seems to be [0, 1, B] always)
00268   //   26 + n + 4 .. count - 4 = device label
00269   //   count - 3 .. count - 1 = unknown (but seems to be 0 always)
00270   //   <ETX> (\x03)
00271 
00272   msg.range_min = 0.05;
00273   msg.range_max = 10.0;
00274 
00275   // ----- adjust start time
00276   // - last scan point = now  ==>  first scan point = now - 271 * time increment
00277   msg.header.stamp = start_time - ros::Duration().fromSec(271 * msg.time_increment);
00278 
00279   // - shift forward to time of first published scan point
00280   msg.header.stamp += ros::Duration().fromSec((double)index_min * msg.time_increment);
00281 
00282   // - add time offset (to account for USB latency etc.)
00283   msg.header.stamp += ros::Duration().fromSec(config.time_offset);
00284 
00285   return EXIT_SUCCESS;
00286 }
00287 
00288 } /* namespace sick_tim3xx */


sick_tim3xx
Author(s): Jochen Sprickerhof , Martin Günther
autogenerated on Fri Jul 25 2014 06:50:00