hokuyo_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008-2010, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include "driver_base/driver.h"
00036 #include "driver_base/driver_node.h"
00037 #include <diagnostic_updater/publisher.h>
00038 
00039 #include <assert.h>
00040 #include <math.h>
00041 #include <iostream>
00042 #include <sstream>
00043 #include <iomanip>
00044 
00045 #include "ros/ros.h"
00046 
00047 #include "sensor_msgs/LaserScan.h"
00048 
00049 #include "hokuyo_node/HokuyoConfig.h"
00050 
00051 #include "hokuyo/hokuyo.h"
00052 
00053 using namespace std;
00054 
00055 class HokuyoDriver : public driver_base::Driver
00056 {
00057   friend class HokuyoNode;
00058 
00059   typedef boost::function<void(const hokuyo::LaserScan &)> UseScanFunction;
00060   UseScanFunction useScan_;
00061 
00062   boost::shared_ptr<boost::thread> scan_thread_;
00063 
00064   std::string device_status_;
00065   std::string device_id_;
00066   std::string last_seen_device_id_;
00067   
00068   bool first_scan_;
00069 
00070   std::string vendor_name_;
00071   std::string product_name_;
00072   std::string protocol_version_;
00073   std::string firmware_version_;
00074   
00075   std::string connect_fail_;
00076   
00077   hokuyo::LaserScan  scan_;
00078   hokuyo::Laser laser_;
00079   hokuyo::LaserConfig laser_config_;
00080 
00081   bool calibrated_;
00082   int lost_scan_thread_count_;
00083   int corrupted_scan_count_;
00084 
00085 public:
00086   hokuyo_node::HokuyoConfig config_;
00087   typedef hokuyo_node::HokuyoConfig Config;
00088 
00089   HokuyoDriver()
00090   {
00091     calibrated_ = false;
00092     lost_scan_thread_count_ = 0;
00093     corrupted_scan_count_ = 0;
00094   }
00095 
00096   bool checkAngleRange(Config &conf)
00097   {
00098     bool changed = false;
00099 
00100     if (conf.min_ang < laser_config_.min_angle)
00101     {
00102       changed = true;
00103       if (laser_config_.min_angle - conf.min_ang > 1e-10)  
00104       {
00105         ROS_WARN("Requested angle (%f rad) out of range, using minimum scan angle supported by device: %f rad.", 
00106             conf.min_ang, laser_config_.min_angle);
00107       }
00108       conf.min_ang = laser_config_.min_angle;
00109     }                                    
00110     
00111     double max_safe_angular_range_per_cluster_deg = 95;
00112     if (firmware_version_ == "1.16.01(16/Nov./2009)")
00113       max_safe_angular_range_per_cluster_deg = 190;
00114     
00115     int real_cluster = conf.cluster == 0 ? 1 : conf.cluster;
00116     double max_safe_angular_range = (real_cluster * max_safe_angular_range_per_cluster_deg) * M_PI / 180;
00117 
00118     if (conf.intensity && (conf.max_ang - conf.min_ang) > max_safe_angular_range + 1e-8 &&
00119         !config_.allow_unsafe_settings && laser_.getProductName() ==
00120             "SOKUIKI Sensor TOP-URG UTM-30LX")
00121     {
00122       changed = true;
00123       conf.max_ang = conf.min_ang + max_safe_angular_range;
00124       ROS_WARN("More than %f degree/cluster scan range requested on UTM-30LX firmware version %s in intensity mode with cluster=%i. The max_ang was adjusted to limit the range. You may extend the scanner's angular range using the allow_unsafe_settings option, but this may result in incorrect data or laser crashes that will require a power cycle of the laser.", max_safe_angular_range_per_cluster_deg, firmware_version_.c_str(), real_cluster);
00125     }
00126 
00127     if (conf.max_ang - laser_config_.max_angle > 1e-10)   
00128     {
00129       changed = true;
00130       ROS_WARN("Requested angle (%f rad) out of range, using maximum scan angle supported by device: %f rad.", 
00131           conf.max_ang, laser_config_.max_angle);
00132       conf.max_ang = laser_config_.max_angle;
00133     }
00134     
00135     if (conf.min_ang > conf.max_ang)
00136     {
00137       changed = true;
00138       if (conf.max_ang < laser_config_.min_angle)
00139       {
00140         if (laser_config_.min_angle - conf.max_ang > 1e-10)  
00141           ROS_WARN("Requested angle (%f rad) out of range, using minimum scan angle supported by device: %f rad.", 
00142               conf.max_ang, laser_config_.min_angle);
00143         conf.max_ang = laser_config_.min_angle;
00144       }
00145       ROS_WARN("Minimum angle must be greater than maximum angle. Adjusting min_ang.");
00146       conf.min_ang = conf.max_ang;
00147     }                                    
00148       
00149     return changed;
00150   }
00151 
00152   bool checkIntensitySupport(Config &conf)
00153   {
00154     if (conf.intensity && !laser_.isIntensitySupported())
00155     {
00156       ROS_WARN("This unit does not appear to support intensity mode. Turning intensity off.");
00157       conf.intensity = false;
00158       return true;
00159     }
00160     return false;
00161   }
00162 
00163   void doOpen()
00164   {
00165     try
00166     {
00167       std::string old_device_id = device_id_;
00168       device_id_ = "unknown";
00169       device_status_ =  "unknown";
00170       first_scan_ = true;
00171       
00172       laser_.open(config_.port.c_str());
00173       
00174       device_id_ = getID();
00175       vendor_name_ = laser_.getVendorName();
00176       firmware_version_ = laser_.getFirmwareVersion();
00177       product_name_ = laser_.getProductName();
00178       protocol_version_ = laser_.getProtocolVersion();
00179 
00180       device_status_ = laser_.getStatus();
00181       if (device_status_ != std::string("Sensor works well."))
00182       {
00183         doClose();
00184         setStatusMessagef("Laser returned abnormal status message, aborting: %s You may be able to find further information at http://www.ros.org/wiki/hokuyo_node/Troubleshooting/", device_status_.c_str());
00185         return;
00186       }
00187       
00188       if (old_device_id != device_id_)
00189       {
00190         ROS_INFO("Connected to device with ID: %s", device_id_.c_str());
00191         
00192         if (last_seen_device_id_ != device_id_)
00193         {
00194           // Recalibrate when the device changes.
00195           last_seen_device_id_ = device_id_;
00196           calibrated_ = false;
00197         }
00198 
00199         // Do this elaborate retry circuis if we were just plugged in.
00200         for (int retries = 10;; retries--)
00201           try {
00202             laser_.laserOn();
00203             break;
00204           }
00205         catch (hokuyo::Exception &e)
00206         { 
00207           if (!retries)
00208             throw e; // After trying for 10 seconds, give up and throw the exception.
00209           else if (retries == 10)
00210             ROS_WARN("Could not turn on laser. This may happen just after the device is plugged in. Will retry for 10 seconds.");
00211           ros::Duration(1).sleep();
00212         }
00213       }
00214       else
00215         laser_.laserOn(); // Otherwise, it should just work, so no tolerance.
00216 
00217       if (config_.calibrate_time && !calibrated_)
00218       {
00219         ROS_INFO("Starting calibration. This will take up a few seconds.");
00220         double latency = laser_.calcLatency(false && config_.intensity, config_.min_ang, config_.max_ang, config_.cluster, config_.skip) * 1e-9;
00221         calibrated_ = true; // This is a slow step that we only want to do once.
00222         ROS_INFO("Calibration finished. Latency is: %0.4f", latency);
00223       }
00224       else
00225       {
00226         calibrated_ = false;
00227         laser_.clearLatency();
00228       }
00229 
00230       setStatusMessage("Device opened successfully.", true);
00231       laser_.getConfig(laser_config_);
00232       
00233       state_ = OPENED;
00234     } 
00235     catch (hokuyo::Exception& e) 
00236     {
00237       doClose();
00238       setStatusMessagef("Exception thrown while opening Hokuyo.\n%s", e.what());
00239       return;
00240     }
00241   }
00242 
00243   void doClose()
00244   {
00245     try
00246     {
00247       laser_.close();
00248       setStatusMessage("Device closed successfully.", true);
00249     } catch (hokuyo::Exception& e) {
00250       setStatusMessagef("Exception thrown while trying to close:\n%s",e.what());
00251     }
00252     
00253     state_ = CLOSED; // If we can't close, we are done for anyways.
00254   }
00255 
00256   void doStart()
00257   {
00258     try
00259     {
00260       laser_.laserOn();
00261       
00262       int status = laser_.requestScans(config_.intensity, config_.min_ang, config_.max_ang, config_.cluster, config_.skip);
00263 
00264       if (status != 0) {
00265         setStatusMessagef("Failed to request scans from device.  Status: %d.", status);
00266         corrupted_scan_count_++;
00267         return;
00268       }
00269     
00270       setStatusMessagef("Waiting for first scan.", true);
00271       state_ = RUNNING;
00272       scan_thread_.reset(new boost::thread(boost::bind(&HokuyoDriver::scanThread, this)));
00273     } 
00274     catch (hokuyo::Exception& e) 
00275     {
00276       doClose();
00277       setStatusMessagef("Exception thrown while starting Hokuyo.\n%s", e.what());
00278       connect_fail_ = e.what();
00279       return;
00280     }
00281   }
00282 
00283   void doStop()
00284   {
00285     if (state_ != RUNNING) // RUNNING can exit asynchronously.
00286       return;
00287 
00288     state_ = OPENED;
00289 
00290     if (scan_thread_ && !scan_thread_->timed_join((boost::posix_time::milliseconds) 2000))
00291     {
00292       ROS_ERROR("scan_thread_ did not die after two seconds. Pretending that it did. This is probably a bad sign.");
00293       lost_scan_thread_count_++;
00294     }
00295     scan_thread_.reset();
00296     
00297     setStatusMessagef("Stopped.", true);
00298   }
00299 
00300   virtual std::string getID()
00301   {
00302     std::string id = laser_.getID();
00303     if (id == std::string("H0000000"))
00304       return "unknown";
00305     return id;
00306   }
00307 
00308   void config_update(Config &new_config, int level = 0)
00309   {
00310     ROS_DEBUG("Reconfigure called from state %i", state_);
00311     
00312     if (state_ == OPENED) 
00313       // If it is closed, we don't know what to check for. If it is running those parameters haven't changed,
00314       // and talking to the hokuyo would cause loads of trouble.
00315     {
00316       checkIntensitySupport(new_config);
00317       checkAngleRange(new_config);
00318     }
00319 
00320     config_ = new_config;
00321   }
00322 
00323   void scanThread()
00324   {
00325     while (state_ == RUNNING)
00326     {
00327       try
00328       {
00329         int status = laser_.serviceScan(scan_);
00330 
00331         if(status != 0)
00332         {
00333           ROS_WARN("Error getting scan: %d", status);
00334           break;
00335         }
00336       } catch (hokuyo::CorruptedDataException &e) {
00337         ROS_WARN("Skipping corrupted data");
00338         continue;
00339       } catch (hokuyo::Exception& e) {
00340         ROS_WARN("Exception thrown while trying to get scan.\n%s", e.what());
00341         doClose();
00342         return;
00343       }
00344 
00345       if (first_scan_)
00346       {
00347         first_scan_ = false;
00348         setStatusMessage("Streaming data.", true, true);
00349         ROS_INFO("Streaming data.");
00350       }
00351 
00352       useScan_(scan_);
00353     }
00354 
00355     try
00356     {
00357       laser_.stopScanning(); // This actually just calls laser Off internally.
00358     } catch (hokuyo::Exception &e)
00359     {
00360         ROS_WARN("Exception thrown while trying to stop scan.\n%s", e.what());
00361     }
00362     state_ = OPENED;
00363   }
00364 };
00365 
00366 class HokuyoNode : public driver_base::DriverNode<HokuyoDriver>
00367 {
00368 private:   
00369   string connect_fail_;
00370   
00371   double desired_freq_;
00372 
00373   ros::NodeHandle node_handle_;
00374   diagnostic_updater::DiagnosedPublisher<sensor_msgs::LaserScan> scan_pub_;
00375   sensor_msgs::LaserScan scan_msg_;
00376   diagnostic_updater::FunctionDiagnosticTask hokuyo_diagnostic_task_;
00377   
00378   // Tick-tock transition variable, controls if the driver outputs NaNs and Infs
00379   bool use_rep_117_;
00380 
00381 public:
00382   HokuyoNode(ros::NodeHandle &nh) :
00383     driver_base::DriverNode<HokuyoDriver>(nh),
00384     node_handle_(nh),
00385     scan_pub_(node_handle_.advertise<sensor_msgs::LaserScan>("scan", 100),
00386         diagnostic_,
00387         diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05),
00388         diagnostic_updater::TimeStampStatusParam()),
00389         hokuyo_diagnostic_task_("Hokuyo Diagnostics", boost::bind(&HokuyoNode::connectionStatus, this, _1))
00390   {
00391     desired_freq_ = 0;
00392     driver_.useScan_ = boost::bind(&HokuyoNode::publishScan, this, _1);
00393     driver_.setPostOpenHook(boost::bind(&HokuyoNode::postOpenHook, this));
00394 
00395     // Check whether or not to support REP 117
00396     std::string key;
00397     if (nh.searchParam("use_rep_117", key))
00398     {
00399       nh.getParam(key, use_rep_117_);
00400     } else {
00401       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");
00402       use_rep_117_ = true;
00403     }
00404     
00405     if(!use_rep_117_){ // Warn the user that they need to update their code.
00406       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");
00407     }
00408   }
00409 
00410   void postOpenHook()
00411   {
00412     private_node_handle_.setParam("min_ang_limit", (double) (driver_.laser_config_.min_angle));
00413     private_node_handle_.setParam("max_ang_limit", (double) (driver_.laser_config_.max_angle));
00414     private_node_handle_.setParam("min_range", (double) (driver_.laser_config_.min_range));
00415     private_node_handle_.setParam("max_range", (double) (driver_.laser_config_.max_range));
00416 
00417     diagnostic_.setHardwareID(driver_.getID());
00418 
00419     if (driver_.checkIntensitySupport(driver_.config_) || 
00420         driver_.checkAngleRange(driver_.config_)) // Might have been set before the device's range was known.
00421       reconfigure_server_.updateConfig(driver_.config_);
00422     
00423     scan_pub_.clear_window(); // Reduce glitches in the frequency diagnostic.
00424   }
00425 
00426   virtual void addOpenedTests()
00427   {
00428     self_test_.add( "Status Test", this, &HokuyoNode::statusTest );
00429     self_test_.add( "Laser Test", this, &HokuyoNode::laserTest );
00430     self_test_.add( "Polled Data Test", this, &HokuyoNode::polledDataTest );
00431     self_test_.add( "Streamed Data Test", this, &HokuyoNode::streamedDataTest );
00432     self_test_.add( "Streamed Intensity Data Test", this, &HokuyoNode::streamedIntensityDataTest );
00433     self_test_.add( "Laser Off Test", this, &HokuyoNode::laserOffTest );
00434   }
00435 
00436   virtual void addStoppedTests()
00437   { 
00438   }
00439 
00440   virtual void addRunningTests()
00441   { 
00442   }
00443 
00444   virtual void addDiagnostics()
00445   {
00446     driver_status_diagnostic_.addTask(&hokuyo_diagnostic_task_);
00447   }
00448   
00449   void reconfigureHook(int level)
00450   {
00451     if (private_node_handle_.hasParam("frameid"))
00452     {
00453       ROS_WARN("~frameid is deprecated, please use ~frame_id instead");
00454       private_node_handle_.getParam("frameid", driver_.config_.frame_id);
00455     }
00456 
00457     if (private_node_handle_.hasParam("min_ang_degrees"))
00458     {
00459       ROS_WARN("~min_ang_degrees is deprecated, please use ~min_ang instead");
00460       private_node_handle_.getParam("min_ang_degrees", driver_.config_.min_ang);
00461       driver_.config_.min_ang *= M_PI/180;
00462     }
00463 
00464     if (private_node_handle_.hasParam("max_ang_degrees"))
00465     {
00466       ROS_WARN("~max_ang_degrees is deprecated, please use ~max_ang instead");
00467       private_node_handle_.getParam("max_ang_degrees", driver_.config_.max_ang);
00468       driver_.config_.max_ang *= M_PI/180;
00469     }
00470 
00471     diagnostic_.force_update();   
00472     
00473     scan_pub_.clear_window(); // Reduce glitches in the frequency diagnostic.
00474   }
00475 
00476   int publishScan(const hokuyo::LaserScan &scan)
00477   {
00478     //ROS_DEBUG("publishScan");
00479 
00480     scan_msg_.angle_min = scan.config.min_angle;
00481     scan_msg_.angle_max = scan.config.max_angle;
00482     scan_msg_.angle_increment = scan.config.ang_increment;
00483     scan_msg_.time_increment = scan.config.time_increment;
00484     scan_msg_.scan_time = scan.config.scan_time;
00485     scan_msg_.range_min = scan.config.min_range;
00486     scan_msg_.range_max = scan.config.max_range;
00487     scan_msg_.ranges = scan.ranges;
00488     scan_msg_.intensities = scan.intensities;
00489     scan_msg_.header.stamp = ros::Time().fromNSec((uint64_t)scan.system_time_stamp) + ros::Duration(driver_.config_.time_offset);
00490     scan_msg_.header.frame_id = driver_.config_.frame_id;
00491   
00492     desired_freq_ = (1. / scan.config.scan_time);
00493     
00494     if(!use_rep_117_){ // Filter out all NaNs, -Infs, and +Infs; replace them with 0 since that is more consistent with the previous behavior
00495       for(uint i = 0; i < scan_msg_.ranges.size(); i++){
00496         if(std::isnan(scan_msg_.ranges[i]) || std::isinf(scan_msg_.ranges[i])){
00497           scan_msg_.ranges[i] = 0;
00498         }
00499       }
00500     }
00501 
00502     scan_pub_.publish(scan_msg_);
00503 
00504     //ROS_DEBUG("publishScan done");
00505 
00506     return(0);
00507   }
00508 
00509   void connectionStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
00510   {
00511     if (driver_.state_ == driver_.CLOSED)
00512       status.summary(2, "Not connected. " + driver_.device_status_ + " " + connect_fail_);
00513     else if (driver_.device_status_ != std::string("Sensor works well."))
00514       status.summaryf(2, "Abnormal status: %s", driver_.device_status_.c_str());
00515     else if (driver_.state_ == driver_.RUNNING)
00516     {
00517       if (driver_.first_scan_)
00518         status.summary(0, "Waiting for first scan");
00519       else
00520         status.summary(0, "Streaming");
00521     }
00522     else if (driver_.state_ == driver_.OPENED)
00523       status.summary(0, "Open");
00524     else 
00525       status.summary(2, "Unknown state");
00526 
00527     status.add("Device Status", driver_.device_status_);
00528     status.add("Port", driver_.config_.port);
00529     status.add("Device ID", driver_.device_id_);
00530     status.add("Scan Thread Lost Count", driver_.lost_scan_thread_count_);
00531     status.add("Corrupted Scan Count", driver_.corrupted_scan_count_);
00532     status.add("Vendor Name", driver_.vendor_name_);
00533     status.add("Product Name", driver_.product_name_);
00534     status.add("Firmware Version", driver_.firmware_version_);
00535     status.add("Protocol Version", driver_.protocol_version_);
00536     status.add("Computed Latency", driver_.laser_.getLatency());
00537     status.add("User Time Offset", driver_.config_.time_offset);
00538   }
00539 
00540   void statusTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00541   {
00542     std::string stat = driver_.laser_.getStatus();
00543 
00544     if (stat != std::string("Sensor works well."))
00545     {
00546       status.level = 2;
00547     } else {
00548       status.level = 0;
00549     }
00550 
00551     status.message = stat;
00552   }
00553 
00554   void laserTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00555   {
00556     driver_.laser_.laserOn();
00557 
00558     status.level = 0;
00559     status.message = "Laser turned on successfully.";
00560   }
00561 
00562   void polledDataTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00563   {
00564     hokuyo::LaserScan  scan;
00565 
00566     int res = driver_.laser_.pollScan(scan, driver_.laser_config_.min_angle, driver_.laser_config_.max_angle, 1, 1000);
00567 
00568     if (res != 0)
00569     {
00570       status.level = 2;
00571       ostringstream oss;
00572       oss << "Hokuyo error code: " << res << ". Consult manual for meaning.";
00573       status.message = oss.str();
00574 
00575     } else {
00576       status.level = 0;
00577       status.message = "Polled Hokuyo for data successfully.";
00578     }
00579   }
00580 
00581   void streamedDataTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00582   {
00583     hokuyo::LaserScan  scan;
00584 
00585     int res = driver_.laser_.requestScans(false, driver_.laser_config_.min_angle, driver_.laser_config_.max_angle, 1, 1, 99, 1000);
00586 
00587     if (res != 0)
00588     {
00589       status.level = 2;
00590       ostringstream oss;
00591       oss << "Hokuyo error code: " << res << ". Consult manual for meaning.";
00592       status.message = oss.str();
00593 
00594     } else {
00595 
00596       for (int i = 0; i < 99; i++)
00597       {
00598         driver_.laser_.serviceScan(scan, 1000);
00599       }
00600 
00601       status.level = 0;
00602       status.message = "Streamed data from Hokuyo successfully.";
00603 
00604     }
00605   }
00606 
00607   void streamedIntensityDataTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00608   {
00609     hokuyo::LaserScan  scan;
00610 
00611     int res = driver_.laser_.requestScans(false, driver_.laser_config_.min_angle, driver_.laser_config_.max_angle, 1, 1, 99, 1000);
00612 
00613     if (res != 0)
00614     {
00615       status.level = 2;
00616       ostringstream oss;
00617       oss << "Hokuyo error code: " << res << ". Consult manual for meaning.";
00618       status.message = oss.str();
00619 
00620     } else {
00621 
00622       int corrupted_data = 0;
00623 
00624       for (int i = 0; i < 99; i++)
00625       {
00626         try {
00627           driver_.laser_.serviceScan(scan, 1000);
00628         } catch (hokuyo::CorruptedDataException &e) {
00629           corrupted_data++;
00630         }
00631       }
00632       if (corrupted_data == 1)
00633       {
00634         status.level = 1;
00635         status.message = "Single corrupted message.  This is acceptable and unavoidable";
00636       } else if (corrupted_data > 1)
00637       {
00638         status.level = 2;
00639         ostringstream oss;
00640         oss << corrupted_data << " corrupted messages.";
00641         status.message = oss.str();
00642       } else
00643       {
00644         status.level = 0;
00645         status.message = "Stramed data with intensity from Hokuyo successfully.";
00646       }
00647     }
00648   }
00649 
00650   void laserOffTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00651   {
00652     driver_.laser_.laserOff();
00653 
00654     status.level = 0;
00655     status.message = "Laser turned off successfully.";
00656   }
00657 };
00658 
00659 int main(int argc, char **argv)
00660 { 
00661   return driver_base::main<HokuyoNode>(argc, argv, "hokuyo_node");
00662 }
00663 


hokuyo_node
Author(s): Brian P. Gerkey, Jeremy Leibs, Blaise Gassend
autogenerated on Tue Apr 22 2014 18:56:48