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_tim3xx/sick_tim310_parser.h>
00036 
00037 #include <ros/ros.h>
00038 
00039 namespace sick_tim3xx
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, SickTim3xxConfig &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   fields[count] = cur_field;
00068   // ROS_DEBUG("%zu: %s ", count, fields[count]);
00069 
00070   while (cur_field != NULL)
00071   {
00072     count++;
00073     cur_field = strtok(NULL, " ");
00074     if (count <= NUM_FIELDS)
00075       fields[count] = cur_field;
00076 
00077     // ROS_DEBUG("%zu: %s ", count, cur_field);
00078   }
00079 
00080   if (count < NUM_FIELDS)
00081   {
00082     ROS_WARN(
00083         "received less fields than expected fields (actual: %zu, expected: %zu), ignoring scan", count, NUM_FIELDS);
00084     ROS_WARN("are you using the correct node? (124 --> sick_tim310_1130000m01, 306 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)");
00085     // ROS_DEBUG("received message was: %s", datagram_copy);
00086     return EXIT_FAILURE;
00087   }
00088   else if (count > NUM_FIELDS)
00089   {
00090     ROS_WARN("received more fields than expected (actual: %zu, expected: %zu), ignoring scan", count, NUM_FIELDS);
00091     ROS_WARN("are you using the correct node? (124 --> sick_tim310_1130000m01, 306 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)");
00092     // ROS_DEBUG("received message was: %s", datagram_copy);
00093     return EXIT_FAILURE;
00094   }
00095 
00096   // ----- read fields into msg
00097   msg.header.frame_id = config.frame_id;
00098   ROS_DEBUG("publishing with frame_id %s", config.frame_id.c_str());
00099 
00100   ros::Time start_time = ros::Time::now(); // will be adjusted in the end
00101 
00102   // <STX> (\x02)
00103   // 0: Type of command (SN)
00104   // 1: Command (LMDscandata)
00105   // 2: Firmware version number (1)
00106   // 3: Device number (1)
00107   // 4: Serial number (B96518)
00108   // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
00109   // 7: Telegram counter (99)
00110   // 8: Scan counter (9A)
00111   // 9: Time since startup (13C8E59)
00112   // 10: Time of transmission (13C9CBE)
00113   // 11 + 12: Input status (0 0)
00114   // 13 + 14: Output status (8 0)
00115   // 15: Reserved Byte A (0)
00116 
00117   // 16: Scanning Frequency (5DC)
00118   unsigned short scanning_freq = -1;
00119   sscanf(fields[16], "%hx", &scanning_freq);
00120   msg.scan_time = 1.0 / (scanning_freq / 100.0);
00121   // ROS_DEBUG("hex: %s, scanning_freq: %d, scan_time: %f", fields[16], scanning_freq, msg.scan_time);
00122 
00123   // 17: Measurement Frequency (36)
00124   unsigned short measurement_freq = 36;
00125   //sscanf(fields[17], "%hx", &measurement_freq);
00126   msg.time_increment = 1.0 / (measurement_freq * 100.0);
00127   // ROS_DEBUG("measurement_freq: %d, time_increment: %f", measurement_freq, msg.time_increment);
00128 
00129   // 18: Number of encoders (0)
00130   // 19: Number of 16 bit channels (1)
00131   // 22: Measured data contents (DIST1)
00132 
00133   // 23: Scaling factor (3F800000)
00134   // ignored for now (is always 1.0):
00135 //      unsigned int scaling_factor_int = -1;
00136 //      sscanf(fields[21], "%x", &scaling_factor_int);
00137 //
00138 //      float scaling_factor = reinterpret_cast<float&>(scaling_factor_int);
00139 //      // ROS_DEBUG("hex: %s, scaling_factor_int: %d, scaling_factor: %f", fields[21], scaling_factor_int, scaling_factor);
00140 
00141   // 24: Scaling offset (00000000) -- always 0
00142   // 25: Starting angle (FFF92230)
00143   int starting_angle = -1;
00144   sscanf(fields[25], "%x", &starting_angle);
00145   msg.angle_min = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
00146   // ROS_DEBUG("starting_angle: %d, angle_min: %f", starting_angle, msg.angle_min);
00147 
00148   // 26: Angular step width (2710)
00149   unsigned short angular_step_width = -1;
00150   sscanf(fields[26], "%hx", &angular_step_width);
00151   msg.angle_increment = (angular_step_width / 10000.0) / 180.0 * M_PI;
00152   msg.angle_max = msg.angle_min + 270.0 * msg.angle_increment;
00153 
00154   // adjust angle_min to min_ang config param
00155   int index_min = 0;
00156   while (msg.angle_min + msg.angle_increment < config.min_ang)
00157   {
00158     msg.angle_min += msg.angle_increment;
00159     index_min++;
00160   }
00161 
00162   // adjust angle_max to max_ang config param
00163   int index_max = 270;
00164   while (msg.angle_max - msg.angle_increment > config.max_ang)
00165   {
00166     msg.angle_max -= msg.angle_increment;
00167     index_max--;
00168   }
00169 
00170   ROS_DEBUG("index_min: %d, index_max: %d", index_min, index_max);
00171   // ROS_DEBUG("angular_step_width: %d, angle_increment: %f, angle_max: %f", angular_step_width, msg.angle_increment, msg.angle_max);
00172 
00173   // 27: Number of data (10F)
00174 
00175   // 28..298: Data_1 .. Data_n
00176   msg.ranges.resize(index_max - index_min + 1);
00177   for (int j = index_min; j <= index_max; ++j)
00178   {
00179     unsigned short range;
00180     sscanf(fields[j + 28], "%hx", &range);
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   msg.header.stamp = start_time - ros::Duration().fromSec(271 * msg.time_increment);
00216 
00217   // - shift forward to time of first published scan point
00218   msg.header.stamp += ros::Duration().fromSec((double)index_min * msg.time_increment);
00219 
00220   // - add time offset (to account for USB latency etc.)
00221   msg.header.stamp += ros::Duration().fromSec(config.time_offset);
00222 
00223   return EXIT_SUCCESS;
00224 }
00225 
00226 } /* namespace sick_tim3xx */


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