sicklms.cpp
Go to the documentation of this file.
00001 
00002 // this program just uses sicktoolbox to get laser scans, and then publishes
00003 // them as ROS messages
00004 //
00005 // Copyright (C) 2008, Morgan Quigley
00006 //
00007 // I am distributing this code under the BSD license:
00008 //
00009 // Redistribution and use in source and binary forms, with or without 
00010 // modification, are permitted provided that the following conditions are met:
00011 //   * Redistributions of source code must retain the above copyright notice, 
00012 //     this list of conditions and the following disclaimer.
00013 //   * Redistributions in binary form must reproduce the above copyright 
00014 //     notice, this list of conditions and the following disclaimer in the 
00015 //     documentation and/or other materials provided with the distribution.
00016 //   * Neither the name of Stanford University nor the names of its 
00017 //     contributors may be used to endorse or promote products derived from 
00018 //     this software without specific prior written permission.
00019 //   
00020 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
00021 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
00022 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
00023 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
00024 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
00025 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
00026 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
00027 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
00028 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
00029 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
00030 // POSSIBILITY OF SUCH DAMAGE.
00031 
00032 #include <csignal>
00033 #include <cstdio>
00034 #include <math.h>
00035 #include <limits>
00036 #include <sicklms-1.0/SickLMS.hh>
00037 #include "ros/ros.h"
00038 #include "sensor_msgs/LaserScan.h"
00039 #include <diagnostic_updater/diagnostic_updater.h> // Publishing over the diagnostics channels.
00040 #include <diagnostic_updater/publisher.h>
00041 using namespace SickToolbox;
00042 using namespace std;
00043 
00044 // Tick-tock transition variable, controls if the driver outputs NaNs and Infs
00045 bool use_rep_117_;
00046 
00047 void publish_scan(diagnostic_updater::DiagnosedPublisher<sensor_msgs::LaserScan> *pub, uint32_t *range_values,
00048                   uint32_t n_range_values, uint32_t *intensity_values,
00049                   uint32_t n_intensity_values, double scale, ros::Time start,
00050                   double scan_time, bool inverted, float angle_min,
00051                   float angle_max, std::string frame_id)
00052 {
00053   static int scan_count = 0;
00054   sensor_msgs::LaserScan scan_msg;
00055   scan_msg.header.frame_id = frame_id;
00056   scan_count++;
00057   if (inverted) {
00058     scan_msg.angle_min = angle_max;
00059     scan_msg.angle_max = angle_min;
00060   } else {
00061     scan_msg.angle_min = angle_min;
00062     scan_msg.angle_max = angle_max;
00063   }
00064   scan_msg.angle_increment = (scan_msg.angle_max - scan_msg.angle_min) / (double)(n_range_values-1);
00065   scan_msg.scan_time = scan_time;
00066   scan_msg.time_increment = scan_time / (2*M_PI) * scan_msg.angle_increment;
00067   scan_msg.range_min = 0;
00068   if (scale == 0.01) {
00069     scan_msg.range_max = 81;
00070   }
00071   else if (scale == 0.001) {
00072     scan_msg.range_max = 8.1;
00073   }
00074   scan_msg.ranges.resize(n_range_values);
00075   scan_msg.header.stamp = start;
00076   if(use_rep_117_){ // Output NaNs and Infs where appropriate
00077     for (size_t i = 0; i < n_range_values; i++) {
00078       // Check for overflow values, see pg 124 of the Sick LMS telegram listing
00079       switch (range_values[i])
00080       {
00081         // 8m or 80m operation
00082         case 8191: // Measurement not valid
00083           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00084           break;
00085         case 8190: // Dazzling
00086           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00087           break;
00088         case 8189: // Operation Overflow
00089           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00090           break;
00091         case 8187: // Signal to Noise ratio too small
00092           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00093           break;
00094         case 8186: // Erorr when reading channel 1
00095           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00096           break;
00097         case 8183: // Measured value > maximum Value
00098           scan_msg.ranges[i] = numeric_limits<float>::infinity();
00099           break;
00101         /*// 16m operation
00102         case 16383: // Measurement not valid
00103           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00104           break;
00105         case 16382: // Dazzling
00106           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00107           break;
00108         case 16381: // Operation Overflow
00109           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00110           break;
00111         case 16379: // Signal to Noise ratio too small
00112           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00113           break;
00114         case 16378: // Erorr when reading channel 1
00115           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00116           break;
00117         case 16385: // Measured value > maximum Value
00118           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00119           break;
00120         // 32m operation
00121         case 32767: // Measurement not valid
00122           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00123           break;
00124         case 32766 // Dazzling
00125           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00126           break;
00127         case 32765: // Operation Overflow
00128           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00129           break;
00130         case 32763: // Signal to Noise ratio too small
00131           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00132           break;
00133         case 32762: // Erorr when reading channel 1
00134           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00135           break;
00136         case 32759: // Measured value > maximum Value
00137           scan_msg.ranges[i] = numeric_limits<float>::quiet_NaN();
00138           break;*/
00139         default:
00140           scan_msg.ranges[i] = (float)range_values[i] * (float)scale;
00141       }
00142     }
00143   } else { // Use legacy output
00144     for (size_t i = 0; i < n_range_values; i++) {
00145       scan_msg.ranges[i] = (float)range_values[i] * (float)scale;
00146     }
00147   }
00148   scan_msg.intensities.resize(n_intensity_values);
00149   for (size_t i = 0; i < n_intensity_values; i++) {
00150     scan_msg.intensities[i] = (float)intensity_values[i];
00151   }
00152   
00153 
00154   pub->publish(scan_msg);
00155 }
00156 
00157 SickLMS::sick_lms_measuring_units_t StringToLmsMeasuringUnits(string units)
00158 {
00159   if (units.compare("mm") == 0)
00160     return SickLMS::SICK_MEASURING_UNITS_MM;
00161   else if (units.compare("cm") == 0)
00162     return SickLMS::SICK_MEASURING_UNITS_CM;
00163   
00164   return SickLMS::SICK_MEASURING_UNITS_UNKNOWN;
00165 }
00166 
00167 
00168 int main(int argc, char **argv)
00169 {
00170         ros::init(argc, argv, "sicklms");
00171         ros::NodeHandle nh;
00172         ros::NodeHandle nh_ns("~");
00173         string port;
00174         int baud;
00175         int delay;
00176         bool inverted;
00177         int angle;
00178         double resolution;
00179         std::string measuring_units;
00180         std::string frame_id;
00181         double scan_time = 0;
00182         double angle_increment = 0;
00183         float angle_min = 0.0;
00184         float angle_max = 0.0;
00185         
00186         // Diagnostic publisher parameters
00187         double desired_freq;
00188         nh_ns.param<double>("desired_frequency", desired_freq, 75.0);
00189         double min_freq;
00190         nh_ns.param<double>("min_frequency", min_freq, desired_freq);
00191         double max_freq;
00192         nh_ns.param<double>("max_frequency", max_freq, desired_freq);
00193         double freq_tolerance; // Tolerance before error, fractional percent of frequency.
00194         nh_ns.param<double>("frequency_tolerance", freq_tolerance, 0.3);
00195         int window_size; // Number of samples to consider in frequency
00196         nh_ns.param<int>("window_size", window_size, 30);
00197         double min_delay; // The minimum publishing delay (in seconds) before error.  Negative values mean future dated messages.
00198         nh_ns.param<double>("min_acceptable_delay", min_delay, 0.0);
00199         double max_delay; // The maximum publishing delay (in seconds) before error.
00200         nh_ns.param<double>("max_acceptable_delay", max_delay, 0.2);
00201         std::string hardware_id;
00202         nh_ns.param<std::string>("hardware_id", hardware_id, "SICK LMS");
00203         
00204         // Set up diagnostics
00205         diagnostic_updater::Updater updater;
00206         updater.setHardwareID(hardware_id);
00207         diagnostic_updater::DiagnosedPublisher<sensor_msgs::LaserScan> scan_pub(nh.advertise<sensor_msgs::LaserScan>("scan", 10), updater, 
00208             diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq, freq_tolerance, window_size),
00209             diagnostic_updater::TimeStampStatusParam(min_delay, max_delay));
00210         
00211         nh_ns.param("port", port, string("/dev/lms200"));
00212         nh_ns.param("baud", baud, 38400);
00213         nh_ns.param("connect_delay", delay, 0);
00214         nh_ns.param("inverted", inverted, false);
00215         nh_ns.param("angle", angle, 0);
00216         nh_ns.param("resolution", resolution, 0.0);
00217         nh_ns.param("units", measuring_units, string());
00218         nh_ns.param<std::string>("frame_id", frame_id, "laser");
00219         
00220         // Check whether or not to support REP 117
00221         std::string key;
00222         if (nh.searchParam("use_rep_117", key))
00223         {
00224           nh.getParam(key, use_rep_117_);
00225         } else {
00226     ROS_WARN("The use_rep_117 parameter has not been specified and has been automatically set to true.  This parameter will be removed in Hydromedusa.  Please see: http://ros.org/wiki/rep_117/migration");
00227           use_rep_117_ = true;
00228         }
00229         
00230         if(!use_rep_117_){ // Warn the user that they need to update their code.
00231           ROS_WARN("The use_rep_117 parameter is set to false.  This parameter will be removed in Hydromedusa.  Please see: http://ros.org/wiki/rep_117/migration");
00232         }
00233 
00234         SickLMS::sick_lms_baud_t desired_baud = SickLMS::IntToSickBaud(baud);
00235         if (desired_baud == SickLMS::SICK_BAUD_UNKNOWN)
00236         {
00237                 ROS_ERROR("Baud rate must be in {9600, 19200, 38400, 500000}");
00238                 return 1;
00239         }
00240         uint32_t range_values[SickLMS::SICK_MAX_NUM_MEASUREMENTS] = {0};
00241   uint32_t intensity_values[SickLMS::SICK_MAX_NUM_MEASUREMENTS] = {0};
00242         uint32_t n_range_values = 0;
00243   uint32_t n_intensity_values = 0;
00244         SickLMS sick_lms(port);
00245         double scale = 0;
00246   double angle_offset;
00247   uint32_t partial_scan_index;
00248 
00249         try
00250         {
00251                 uint32_t on_delay = 0;
00252                 if(delay > 0){
00253                   on_delay = delay;
00254                 }
00255                 sick_lms.Initialize(desired_baud, on_delay);
00256 
00257                 // Set the angle and resolution if possible (not an LMSFast) and
00258                 // the user specifies a setting.
00259                 int actual_angle = sick_lms.GetSickScanAngle();
00260                 double actual_resolution = sick_lms.GetSickScanResolution();
00261                 SickLMS::sick_lms_measuring_units_t actual_units = sick_lms.GetSickMeasuringUnits();
00262 
00263                 // Attempt to set measurement angles and angular resolution
00264                 try {
00265                   if ((angle && actual_angle != angle) || (resolution && actual_resolution != resolution)) {
00266                      ROS_INFO("Setting variant to (%i, %f)", angle, resolution);
00267                      sick_lms.SetSickVariant(sick_lms.IntToSickScanAngle(angle),
00268                                              sick_lms.DoubleToSickScanResolution(resolution));
00269                   } else {
00270                     ROS_INFO("Variant setup not requested or identical to actual (%i, %f)", actual_angle,
00271                       actual_resolution);
00272                     angle = actual_angle;
00273                     resolution = actual_resolution;
00274                   }
00275                 } catch (SickConfigException e) {
00276                   actual_angle = sick_lms.GetSickScanAngle();
00277                   actual_resolution = sick_lms.GetSickScanResolution();
00278                   if (angle != actual_angle) {
00279                     ROS_WARN("Unable to set scan angle. Using %i instead of %i.", actual_angle, angle);
00280                     angle = actual_angle;
00281                   }
00282                   if (resolution != actual_resolution) {
00283                     ROS_WARN("Unable to set resolution. Using %e instead of %e.", actual_resolution, resolution);
00284                     resolution = actual_resolution;
00285                   }
00286                 }
00287                 
00288                 // Attempt to set measurement output mode to cm or mm
00289                 try {
00290                   if (!measuring_units.empty() && (actual_units != StringToLmsMeasuringUnits(measuring_units))) {
00291                     ROS_INFO("Setting measuring units to '%s'", measuring_units.c_str());
00292                     actual_units = StringToLmsMeasuringUnits(measuring_units);
00293                     sick_lms.SetSickMeasuringUnits(actual_units);
00294                   } else {
00295                     ROS_INFO("Measuring units setup not requested or identical to actual ('%s')",
00296                       sick_lms.SickMeasuringUnitsToString(actual_units).c_str());
00297                   }
00298                 }  catch (SickConfigException e) {
00299                   actual_units = sick_lms.GetSickMeasuringUnits();
00300                   if (StringToLmsMeasuringUnits(measuring_units) != actual_units) {
00301                     ROS_WARN("Unable to set measuring units. Using '%s' instead of '%s'.",
00302                       sick_lms.SickMeasuringUnitsToString(actual_units).c_str(), measuring_units.c_str());
00303                     measuring_units = sick_lms.SickMeasuringUnitsToString(actual_units);
00304                   }
00305                 }
00306 
00307                 if (actual_units == SickLMS::SICK_MEASURING_UNITS_CM)
00308                         scale = 0.01;
00309                 else if (actual_units == SickLMS::SICK_MEASURING_UNITS_MM)
00310                         scale = 0.001;
00311                 else
00312                 {
00313                         ROS_ERROR("Invalid measuring unit.");
00314                         return 1;
00315                 }
00316 
00317     // The scan time is always 1/75 because that's how long it takes
00318     // for the mirror to rotate. If we have a higher resolution, the
00319     // SICKs interleave the readings, so the net result is we just
00320     // shift the measurements.
00321     if (angle == 180 || sick_lms.IsSickLMSFast()) {
00322       scan_time = 1.0 / 75;
00323     } 
00324     else {
00325       SickLMS::sick_lms_scan_resolution_t scan_resolution =
00326         SickLMS::DoubleToSickScanResolution(resolution);
00327       if ( scan_resolution == SickLMS::SICK_SCAN_RESOLUTION_25) {
00328         // 0.25 degrees
00329         scan_time = 4.0 / 75;   // 53.33 ms
00330       }
00331       else if ( scan_resolution == SickLMS::SICK_SCAN_RESOLUTION_50) {
00332         // 0.5 degrees
00333         scan_time = 2.0 / 75;   // 26.66 ms
00334       }
00335       else if ( scan_resolution == SickLMS::SICK_SCAN_RESOLUTION_100) {
00336         // 1 degree
00337         scan_time = 1.0 / 75;   // 13.33 ms
00338       }
00339       else {
00340         ROS_ERROR("Bogus scan resolution.");
00341         return 1;
00342       }
00343       if ( scan_resolution != SickLMS::SICK_SCAN_RESOLUTION_100) {
00344         ROS_WARN("You are using an angle smaller than 180 degrees and a "
00345                  "scan resolution less than 1 degree per scan. Thus, "
00346                  "you are in inteleaved mode and the returns will not "
00347                  "arrive sequentially how you read them. So, the "
00348                  "time_increment field will be misleading. If you need to "
00349                  "know when the measurement was made at a time resolution "
00350                  "better than the scan_time, use the whole 180 degree "
00351                  "field of view.");
00352       }
00353     }
00354 
00355     // The increment for the slower LMS is still 1.0 even if its set to
00356     // 0.5 or 0.25 degrees resolution because it is interleaved. So for
00357     // 0.5 degrees, two scans are needed, offset by 0.5 degrees. These
00358     // show up as two seperate LaserScan messages.
00359     angle_increment = sick_lms.IsSickLMSFast() ? 0.5 : 1.0;
00360 
00361     angle_offset = (180.0-angle)/2;
00362         }
00363         catch (...)
00364         {
00365                 ROS_ERROR("Initialize failed! are you using the correct device path?");
00366     return 2;
00367         }
00368         try
00369         {
00370                 while (ros::ok())
00371                 {
00372       if (sick_lms.IsSickLMSFast()) {
00373         // There's no inteleaving, but we can capture both the range
00374         // and intensity simultaneously
00375         sick_lms.GetSickScan(range_values, intensity_values,
00376                              n_range_values, n_intensity_values);
00377         angle_min = -M_PI/4;
00378         angle_max = M_PI/4;
00379       }
00380       else if (angle != 180) {
00381         // If the angle is not 180, we can't get partial scans as they
00382         // arrive. So, we have to wait for a full scan to get
00383         // there. 
00384         sick_lms.GetSickScan(range_values, n_range_values);
00385         angle_min = (-90.0 + angle_offset) * M_PI / 180.0;
00386         angle_max = (90.0 - angle_offset)  * M_PI / 180.0;
00387       }
00388       else {
00389         // We get scans that could be potentially interleaved
00390         // depending on the mode. We want to stream out the data as
00391         // soon as we get it otherwise the timing won't work right to
00392         // reconstruct the data if the sensor is moving.
00393         sick_lms.GetSickPartialScan(range_values, n_range_values,
00394                                     partial_scan_index);
00395         double partialScanOffset = 0.25 * partial_scan_index;
00396         angle_min = (-90.0 + angle_offset + partialScanOffset) * M_PI / 180.0;
00397         angle_max = (90.0 - angle_offset - fmod(1.0 - partialScanOffset, 1.0))
00398           * M_PI / 180.0;
00399       }
00400       // Figure out the time that the scan started. Since we just
00401       // fished receiving the data, we'll assume that the mirror is at
00402       // 180 degrees now, or half a scan time. In other words, we
00403       // assume a zero transfer time of the data.
00404       ros::Time end_of_scan = ros::Time::now();
00405       ros::Time start = end_of_scan - ros::Duration(scan_time / 2.0);
00406 
00407                         publish_scan(&scan_pub, range_values, n_range_values, intensity_values,
00408                    n_intensity_values, scale, start, scan_time, inverted,
00409                    angle_min, angle_max, frame_id);
00410                         ros::spinOnce();
00411                         // Update diagnostics
00412                         updater.update();
00413                 }
00414         }
00415         catch (...)
00416         {
00417                 ROS_ERROR("Unknown error.");
00418                 return 1;
00419         }
00420         try
00421         {
00422                 sick_lms.Uninitialize();
00423         }
00424         catch (...)
00425         {
00426                 ROS_ERROR("Error during uninitialize");
00427                 return 1;
00428         }
00429         ROS_INFO("Success.\n");
00430 
00431         return 0;
00432 }
00433 


sicktoolbox_wrapper
Author(s): Morgan Quigley
autogenerated on Tue Apr 22 2014 18:56:50