sick_tim310_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: 21.08.2013
00030  *
00031  *      Author: Martin Günther <mguenthe@uos.de>
00032  *
00033  */
00034 
00035 #include <sick_tim/sick_tim310_parser.h>
00036 
00037 #include <ros/ros.h>
00038 
00039 namespace sick_tim
00040 {
00041 
00042 SickTim310Parser::SickTim310Parser() :
00043     AbstractParser()
00044 {
00045 }
00046 
00047 SickTim310Parser::~SickTim310Parser()
00048 {
00049 }
00050 
00051 int SickTim310Parser::parse_datagram(char* datagram, size_t datagram_length, SickTimConfig &config,
00052                                      sensor_msgs::LaserScan &msg)
00053 {
00054   static const size_t NUM_FIELDS = 592;
00055   char* fields[NUM_FIELDS];
00056   char* cur_field;
00057   size_t count;
00058 
00059   // ----- only for debug output
00060   char datagram_copy[datagram_length + 1];
00061   strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
00062   datagram_copy[datagram_length] = 0;
00063 
00064   // ----- tokenize
00065   count = 0;
00066   cur_field = strtok(datagram, " ");
00067 
00068   while (cur_field != NULL)
00069   {
00070     if (count < NUM_FIELDS)
00071       fields[count++] = cur_field;
00072 
00073     // ROS_DEBUG("%zu: %s ", count, cur_field);
00074     cur_field = strtok(NULL, " ");
00075   }
00076 
00077   if (count < NUM_FIELDS)
00078   {
00079     ROS_WARN(
00080         "received less fields than expected fields (actual: %zu, expected: %zu), ignoring scan", count, NUM_FIELDS);
00081     ROS_WARN("are you using the correct node? (124 --> sick_tim310_1130000m01, 306 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)");
00082     // ROS_DEBUG("received message was: %s", datagram_copy);
00083     return ExitError;
00084   }
00085   else if (count > NUM_FIELDS)
00086   {
00087     ROS_WARN("received more fields than expected (actual: %zu, expected: %zu), ignoring scan", count, NUM_FIELDS);
00088     ROS_WARN("are you using the correct node? (124 --> sick_tim310_1130000m01, 306 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)");
00089     // ROS_DEBUG("received message was: %s", datagram_copy);
00090     return ExitError;
00091   }
00092 
00093   // ----- read fields into msg
00094   msg.header.frame_id = config.frame_id;
00095   ROS_DEBUG("publishing with frame_id %s", config.frame_id.c_str());
00096 
00097   ros::Time start_time = ros::Time::now(); // will be adjusted in the end
00098 
00099   // <STX> (\x02)
00100   // 0: Type of command (SN)
00101   // 1: Command (LMDscandata)
00102   // 2: Firmware version number (1)
00103   // 3: Device number (1)
00104   // 4: Serial number (B96518)
00105   // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
00106   // 7: Telegram counter (99)
00107   // 8: Scan counter (9A)
00108   // 9: Time since startup (13C8E59)
00109   // 10: Time of transmission (13C9CBE)
00110   // 11 + 12: Input status (0 0)
00111   // 13 + 14: Output status (8 0)
00112   // 15: Reserved Byte A (0)
00113 
00114   // 16: Scanning Frequency (5DC)
00115   unsigned short scanning_freq = -1;
00116   sscanf(fields[16], "%hx", &scanning_freq);
00117   msg.scan_time = 1.0 / (scanning_freq / 100.0);
00118   // ROS_DEBUG("hex: %s, scanning_freq: %d, scan_time: %f", fields[16], scanning_freq, msg.scan_time);
00119 
00120   // 17: Measurement Frequency (36)
00121   unsigned short measurement_freq = 36;
00122   //sscanf(fields[17], "%hx", &measurement_freq);
00123   msg.time_increment = 1.0 / (measurement_freq * 100.0);
00124   // ROS_DEBUG("measurement_freq: %d, time_increment: %f", measurement_freq, msg.time_increment);
00125 
00126   // 18: Number of encoders (0)
00127   // 19: Number of 16 bit channels (1)
00128   // 22: Measured data contents (DIST1)
00129 
00130   // 23: Scaling factor (3F800000)
00131   // ignored for now (is always 1.0):
00132 //      unsigned int scaling_factor_int = -1;
00133 //      sscanf(fields[21], "%x", &scaling_factor_int);
00134 //
00135 //      float scaling_factor = reinterpret_cast<float&>(scaling_factor_int);
00136 //      // ROS_DEBUG("hex: %s, scaling_factor_int: %d, scaling_factor: %f", fields[21], scaling_factor_int, scaling_factor);
00137 
00138   // 24: Scaling offset (00000000) -- always 0
00139   // 25: Starting angle (FFF92230)
00140   int starting_angle = -1;
00141   sscanf(fields[25], "%x", &starting_angle);
00142   msg.angle_min = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
00143   // ROS_DEBUG("starting_angle: %d, angle_min: %f", starting_angle, msg.angle_min);
00144 
00145   // 26: Angular step width (2710)
00146   unsigned short angular_step_width = -1;
00147   sscanf(fields[26], "%hx", &angular_step_width);
00148   msg.angle_increment = (angular_step_width / 10000.0) / 180.0 * M_PI;
00149   msg.angle_max = msg.angle_min + 270.0 * msg.angle_increment;
00150 
00151   // adjust angle_min to min_ang config param
00152   int index_min = 0;
00153   while (msg.angle_min + msg.angle_increment < config.min_ang)
00154   {
00155     msg.angle_min += msg.angle_increment;
00156     index_min++;
00157   }
00158 
00159   // adjust angle_max to max_ang config param
00160   int index_max = 270;
00161   while (msg.angle_max - msg.angle_increment > config.max_ang)
00162   {
00163     msg.angle_max -= msg.angle_increment;
00164     index_max--;
00165   }
00166 
00167   ROS_DEBUG("index_min: %d, index_max: %d", index_min, index_max);
00168   // ROS_DEBUG("angular_step_width: %d, angle_increment: %f, angle_max: %f", angular_step_width, msg.angle_increment, msg.angle_max);
00169 
00170   // 27: Number of data (10F)
00171 
00172   // 28..298: Data_1 .. Data_n
00173   msg.ranges.resize(index_max - index_min + 1);
00174   for (int j = index_min; j <= index_max; ++j)
00175   {
00176     unsigned short range;
00177     sscanf(fields[j + 28], "%hx", &range);
00178     if (range == 0)
00179       msg.ranges[j - index_min] = std::numeric_limits<float>::infinity();
00180     else
00181       msg.ranges[j - index_min] = range / 1000.0;
00182   }
00183 
00184   // ---297: Number of 8 bit channels (1)---
00185   // 299: Measured data contents (RSSI1)
00186   // 300: Scaling factor (3F800000)
00187   // 301: Scaling offset (00000000)
00188   // 302: Starting angle (FFF92230)
00189   // 303: Angular step width (2710)
00190   // 304: Number of data (10F)
00191   // 305..575: Data_1 .. Data_n
00192   if (config.intensity)
00193   {
00194     msg.intensities.resize(index_max - index_min + 1);
00195     for (int j = index_min; j <= index_max; ++j)
00196     {
00197       unsigned short intensity;
00198       sscanf(fields[j + 305], "%hx", &intensity);
00199       msg.intensities[j - index_min] = intensity;
00200     }
00201   }
00202 
00203   // 576: Position (0)
00204   // 577: Name (0)
00205   // 578: Comment (0)
00206   // 579: Time information (0)
00207   // 579: Event information (0)
00208   // <ETX> (\x03)
00209 
00210   msg.range_min = 0.05;
00211   msg.range_max = 4.0;
00212 
00213   // ----- adjust start time
00214   // - last scan point = now  ==>  first scan point = now - 271 * time increment
00215   double start_time_adjusted = start_time.toSec()
00216             - 271 * msg.time_increment              // shift backward to time of first scan point
00217             + index_min * msg.time_increment        // shift forward to time of first published scan point
00218             + config.time_offset;                   // add time offset (usually negative) to account for USB latency etc.
00219   if (start_time_adjusted >= 0.0)   // ensure that ros::Time is not negative (otherwise runtime error)
00220   {
00221     msg.header.stamp.fromSec(start_time_adjusted);
00222   } else {
00223     ROS_WARN("ROS time is 0! Did you set the parameter use_sim_time to true?");
00224   }
00225 
00226 
00227   return ExitSuccess;
00228 }
00229 
00230 } /* namespace sick_tim */


sick_tim
Author(s): Jochen Sprickerhof , Martin Günther , Sebastian Pütz
autogenerated on Thu May 9 2019 02:32:02