lms400_node.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id: lms400_node.cpp 657 2010-09-25 23:31:31Z moesenle $
00028  *
00029  */
00030 
00042 #include <ros/ros.h>
00043 #include <geometry_msgs/Point.h>
00044 #include <sensor_msgs/LaserScan.h>
00045 
00046 #include <sick_lms400/sick_lms400.h>
00047 
00048 using namespace std;
00049 using namespace ros;
00050 using namespace sick_lms400;
00051 using namespace geometry_msgs;
00052 using namespace sensor_msgs;
00053 
00054 class LMS400Node
00055 {
00056   protected:
00057     ros::NodeHandle nh_;
00058   public:
00059 
00060     SickLMS400 lms_;
00061     LaserScan scan_;
00062 
00063     Publisher scan_pub_;
00064 
00065     // TCP/IP connection parameters
00066     string hostname_, password_;
00067     int    port_;
00068 
00069     // Filter settings
00070     int filter_;
00071     int mean_filter_params_;
00072     double range_filter_params_min_, range_filter_params_max_;
00073 
00074     // Turn intensity data on/off
00075     bool intensity_;
00076 
00077     // Turn laser on/off
00078     bool laser_enabled_;
00079 
00080     // Basic measurement parameters
00081     double angular_resolution_, scanning_frequency_;
00082     double min_angle_, max_angle_;
00083     int eRIS_;
00084 
00085     // Password for changing to userlevel 3 (service)
00086     bool loggedin_;
00087     int debug_;
00088 
00090     LMS400Node (ros::NodeHandle &n) : nh_(n), debug_ (0)
00091     {
00092       nh_.param ("hostname", hostname_, string ("192.168.0.1"));
00093       // Userlevel 3 password (hashed). Default: servicelevel/81BE23AA (Service (userlevel 3) password. Used for enabling/disabling and/or setting the filter parameters.
00094 
00095       nh_.param ("password", password_, string ("81BE23AA"));
00096       nh_.param ("port", port_, 2111);
00097 
00098       // Filter settings. Valid values are:
00099       // 0 (disabled)
00100       // 1 (enable median filter)
00101       // 2 (enable edge filter)
00102       // 4 (enable range filter)
00103       // 8 (enable mean filter)
00104       // Notes : 1) You can combine the filters as required. If several filters are active, then the filters act one after the other on the result of the
00105       //            previous filter. The processing in this case follows the following sequence: edge filter, median filter, range filter, mean filter.
00106       //         2) You can use PLAYER_LASER_REQ_SET_FILTER to enable/disable the filters as well as set their parameters from the client. The parameters of the
00107       //            filters are stored in a float array in the sequence mentioned above. Since the current LMS400 firmware version (1.20) supports setting the
00108       //            parameters for the range and mean filters, the order of the parameters in the (player_laser_set_filter_config_t - float parameters[]) array,
00109       //            provided that both range and mean are enabled is: [BottomLimit TopLimit Mean]
00110       nh_.param ("filter", filter_, 11);     // Enable median, edge and mean filter
00111 
00112       // Enable extended RIS detectivity. If you want to measure objects with remission values < 10%,
00113       // you can extend the so-called Remission Information System (RIS) on the LMS4000.
00114       nh_.param ("enable_eRIS", eRIS_, 1);
00115 
00116       // Define the number of means for the mean filter. Possible values: 2..200.
00117       nh_.param ("mean_filter_parameter", mean_filter_params_, 3);
00118 
00119       // Define a specific range within which measured values are valid and are output. Possible values: [+700.0...+3000.0 <bottom limit>...+3000.0]
00120       nh_.param ("range_filter_parameter_min", range_filter_params_min_, 700.0);
00121       nh_.param ("range_filter_parameter_max", range_filter_params_max_, 3000.0);
00122 
00123       {
00124         // Angular resolution. Valid values are: 0.1 ..1 (with 0.1 degree increments)
00125         nh_.param ("angular_resolution", angular_resolution_, 0.1);
00126 
00127         // Scanning frequency. Valid values are: - 200..500Hz (on the LMS400-0000) / 360..500Hz (on the LMS400-1000)
00128         nh_.param ("scanning_frequency", scanning_frequency_, 360.0);
00129 
00130         nh_.param ("enable_laser", laser_enabled_, true);
00131         nh_.param ("enable_intensity", intensity_, true);
00132 
00133         // Defines the minimum / maximum angle of the laser unit (where the scan should start / end). Valid values: 55-125 degrees.
00134         nh_.param ("min_angle", min_angle_, 55.0);
00135         nh_.param ("max_angle", max_angle_, 125.0);
00136       }
00137 
00138       scan_pub_ = nh_.advertise<LaserScan>("laser_scan", 1);
00139 
00140       loggedin_  = false;
00141     }
00142 
00144     ~LMS400Node ()
00145     {
00146       stop ();
00147     }
00148 
00150     // Start
00151     int
00152       start ()
00153     {
00154       // Open the LMS400 device
00155       lms_ = SickLMS400 (hostname_.c_str (), port_, debug_);
00156 
00157       // Attempt to connect to the laser unit
00158       if (lms_.Connect () != 0)
00159       {
00160         ROS_ERROR ("> [SickLMS400] Connecting to SICK LMS400 on [%s:%d]...[failed!]", hostname_.c_str (), port_);
00161         return (-1);
00162       }
00163       ROS_INFO ("> [SickLMS400] Connecting to SICK LMS400 on [%s:%d]... [done]", hostname_.c_str (), port_);
00164 
00165       // Stop the measurement process (in case it's running from another instance)
00166       lms_.StopMeasurement ();
00167 
00168       if (strncmp (password_.c_str (), "NULL", 4) != 0)
00169       {
00170         // Login to userlevel 3
00171         if (lms_.SetUserLevel (4, password_.c_str ()) != 0)
00172           ROS_WARN ("> [SickLMS400] Unable to change userlevel to 'Service' using %s", password_.c_str ());
00173         else
00174         {
00175           loggedin_ = true;
00176           // Enable or disable filters
00177           if ((mean_filter_params_ >= 2) && (mean_filter_params_ <= 200))
00178             lms_.SetMeanFilterParameters (mean_filter_params_);
00179 
00180           if ((range_filter_params_min_ >= 700.0) && (range_filter_params_max_> range_filter_params_min_) && (range_filter_params_max_ <= 3600.0))
00181             lms_.SetRangeFilterParameters ((float)range_filter_params_min_, (float)range_filter_params_max_);
00182 
00183           lms_.EnableFilters (filter_);
00184 
00185           ROS_INFO ("> [SickLMS400] Enabling selected filters (%d, %f, %f, %d)... [done]", mean_filter_params_, (float)range_filter_params_min_, (float)range_filter_params_max_, filter_);
00186         }
00187       }
00188       else
00189         ROS_WARN ("> [SickLMS400] Userlevel 3 password not given. Filter(s) disabled!");
00190 
00191       // Enable extended RIS detectivity
00192       if (eRIS_)
00193       {
00194         lms_.EnableRIS (1);
00195         ROS_INFO ("> [SickLMS400] Enabling extended RIS detectivity... [done]");
00196       }
00197       // Set scanning parameters
00198       if (lms_.SetResolutionAndFrequency (scanning_frequency_, angular_resolution_, min_angle_, max_angle_ - min_angle_) != 0)
00199         ROS_ERROR ("> [SickLMS400] Couldn't set values for resolution, frequency, and min/max angle. Using previously set values.");
00200       else
00201         ROS_INFO ("> [SickLMS400] Enabling user values for resolution (%f), frequency (%f) and min/max angle (%f/%f)... [done]",
00202                   angular_resolution_, scanning_frequency_, min_angle_, max_angle_);
00203 
00204       return (0);
00205     }
00206 
00208     // Stop
00209     int
00210       stop ()
00211     {
00212       // Stop the measurement process
00213       lms_.StopMeasurement ();
00214       // Set back to userlevel 0
00215       lms_.TerminateConfiguration ();
00216       // Disconnect from the laser unit
00217       lms_.Disconnect ();
00218 
00219       ROS_INFO ("> [SickLMS400] SICK LMS400 driver shutting down... [done]");
00220       return (0);
00221     }
00222 
00224     // Set new measurement values and restart scanning
00225     void
00226       restartMeasurementWithNewValues (float scanning_frequency, float angular_resolution,
00227                                        float min_angle, float diff_angle, int intensity,
00228                                        bool laser_enabled)
00229     {
00230       // Stop the measurement process
00231       lms_.StopMeasurement ();
00232 
00233       // Change userlevel to 3
00234       if (lms_.SetUserLevel (4, password_.c_str ()) != 0)
00235       {
00236         ROS_WARN ("> Unable to change userlevel to 'Service' using %s", password_.c_str ());
00237         if (laser_enabled)
00238           lms_.StartMeasurement (intensity);
00239       }
00240       else
00241       {
00242         // Set the angular resolution and frequency
00243         if (lms_.SetResolutionAndFrequency (scanning_frequency, angular_resolution, min_angle, diff_angle) == 0)
00244           // Re-start the measurement process
00245           if (laser_enabled)
00246             lms_.StartMeasurement (intensity);
00247       }
00248     }
00249 
00251     // Obtain a set of interesting parameters from the parameter server
00252     void
00253       getParametersFromServer ()
00254     {
00255       // LMS400 related parameters
00256       bool laser_enabled;
00257       nh_.getParam ("enable_laser", laser_enabled);
00258       // New value specified
00259       if (laser_enabled != laser_enabled_)
00260       {
00261         ROS_INFO ("New enable_laser parameter received (%d)", laser_enabled);
00262         laser_enabled_ = laser_enabled;
00263 
00264         if (!laser_enabled_)
00265           lms_.StopMeasurement ();
00266         else
00267           lms_.StartMeasurement (intensity_);
00268       }
00269 
00270       bool intensity;
00271       nh_.getParam ("enable_intensity", intensity);
00272       // New value specified
00273       if (intensity != intensity_)
00274       {
00275         ROS_INFO ("New enable_intensity parameter received (%d)", intensity);
00276         intensity_ = intensity;
00277       }
00278 
00279       double angular_resolution;
00280       nh_.getParam ("angular_resolution", angular_resolution);
00281       // New value specified
00282       if (angular_resolution != angular_resolution_)
00283       {
00284         ROS_INFO ("New angular_resolution parameter received (%f)", angular_resolution);
00285         angular_resolution_ = angular_resolution;
00286         restartMeasurementWithNewValues (scanning_frequency_, angular_resolution_, min_angle_, max_angle_ - min_angle_, intensity_, laser_enabled_);
00287       }
00288 
00289       double min_angle;
00290       nh_.getParam ("min_angle", min_angle);
00291       // New value specified
00292       if (min_angle != min_angle_)
00293       {
00294         ROS_INFO ("New min_angle parameter received (%f)", min_angle);
00295         min_angle_ = min_angle;
00296         restartMeasurementWithNewValues (scanning_frequency_, angular_resolution_, min_angle_, max_angle_ - min_angle_, intensity_, laser_enabled_);
00297       }
00298 
00299       double max_angle;
00300       nh_.getParam ("max_angle", max_angle);
00301       // New value specified
00302       if (max_angle != max_angle_)
00303       {
00304         ROS_INFO ("New max_angle parameter received (%f)", max_angle);
00305         max_angle_ = max_angle;
00306         restartMeasurementWithNewValues (scanning_frequency_, angular_resolution_, min_angle_, max_angle_ - min_angle_, intensity_, laser_enabled_);
00307       }
00308 
00309       double scanning_frequency;
00310       nh_.getParam ("scanning_frequency", scanning_frequency);
00311       // New value specified
00312       if (scanning_frequency != scanning_frequency_)
00313       {
00314         ROS_INFO ("New scanning_frequency parameter received (%f)", scanning_frequency);
00315         scanning_frequency_ = scanning_frequency;
00316         restartMeasurementWithNewValues (scanning_frequency_, angular_resolution_, min_angle_, max_angle_ - min_angle_, intensity_, laser_enabled_);
00317       }
00318     }
00319 
00321     // Spin (!)
00322     bool
00323       spin ()
00324     {
00325       // Start Continous measurements
00326       lms_.StartMeasurement (intensity_);
00327 
00328       while (nh_.ok ())
00329       {
00330         // Change certain parameters in the cameras, based on values from the parameter server
00331         //getParametersFromServer ();
00332 
00333         // Refresh data only if laser power is on
00334         if (laser_enabled_)
00335         {
00336           scan_ = lms_.ReadMeasurement ();
00337 
00338           if (scan_.ranges.size () != 0)
00339             scan_pub_.publish (scan_);
00340           //ROS_INFO ("Publishing %d measurements.", (int)scan_.ranges.size ());
00341         }
00342         ros::spinOnce ();
00343       }
00344 
00345       return (true);
00346     }
00347 };
00348 
00349 /* ---[ */
00350 int
00351   main (int argc, char** argv)
00352 {
00353   ros::init (argc, argv, "lms400_node");
00354   ros::NodeHandle n("~");
00355   LMS400Node c(n);
00356 
00357   if (c.start () == 0)
00358     c.spin ();
00359 
00360   return (0);
00361 }
00362 /* ]--- */
00363 


sick_lms400
Author(s): Radu Bogdan Rusu (rusu@cs.tum.edu), Dejan Pangercic (dejan.pangercic@cs.tum.edu)
autogenerated on Mon Oct 6 2014 09:36:31