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_node/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 public:
00379   HokuyoNode(ros::NodeHandle &nh) :
00380     driver_base::DriverNode<HokuyoDriver>(nh),
00381     node_handle_(nh),
00382     scan_pub_(node_handle_.advertise<sensor_msgs::LaserScan>("scan", 100),
00383         diagnostic_,
00384         diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05),
00385         diagnostic_updater::TimeStampStatusParam()),
00386         hokuyo_diagnostic_task_("Hokuyo Diagnostics", boost::bind(&HokuyoNode::connectionStatus, this, _1))
00387   {
00388     desired_freq_ = 0;
00389     driver_.useScan_ = boost::bind(&HokuyoNode::publishScan, this, _1);
00390     driver_.setPostOpenHook(boost::bind(&HokuyoNode::postOpenHook, this));
00391 
00392     // Check if use_rep_117 is set.
00393     std::string key;
00394     if (nh.searchParam("use_rep_117", key))
00395     {
00396       ROS_ERROR("The use_rep_117 parameter has not been specified and is now ignored.  This parameter was removed in Hydromedusa.  Please see: http://ros.org/wiki/rep_117/migration");
00397     }
00398   }
00399 
00400   void postOpenHook()
00401   {
00402     private_node_handle_.setParam("min_ang_limit", (double) (driver_.laser_config_.min_angle));
00403     private_node_handle_.setParam("max_ang_limit", (double) (driver_.laser_config_.max_angle));
00404     private_node_handle_.setParam("min_range", (double) (driver_.laser_config_.min_range));
00405     private_node_handle_.setParam("max_range", (double) (driver_.laser_config_.max_range));
00406 
00407     diagnostic_.setHardwareID(driver_.getID());
00408 
00409     if (driver_.checkIntensitySupport(driver_.config_) || 
00410         driver_.checkAngleRange(driver_.config_)) // Might have been set before the device's range was known.
00411       reconfigure_server_.updateConfig(driver_.config_);
00412     
00413     scan_pub_.clear_window(); // Reduce glitches in the frequency diagnostic.
00414   }
00415 
00416   virtual void addOpenedTests()
00417   {
00418     self_test_.add( "Status Test", this, &HokuyoNode::statusTest );
00419     self_test_.add( "Laser Test", this, &HokuyoNode::laserTest );
00420     self_test_.add( "Polled Data Test", this, &HokuyoNode::polledDataTest );
00421     self_test_.add( "Streamed Data Test", this, &HokuyoNode::streamedDataTest );
00422     self_test_.add( "Streamed Intensity Data Test", this, &HokuyoNode::streamedIntensityDataTest );
00423     self_test_.add( "Laser Off Test", this, &HokuyoNode::laserOffTest );
00424   }
00425 
00426   virtual void addStoppedTests()
00427   { 
00428   }
00429 
00430   virtual void addRunningTests()
00431   { 
00432   }
00433 
00434   virtual void addDiagnostics()
00435   {
00436     driver_status_diagnostic_.addTask(&hokuyo_diagnostic_task_);
00437   }
00438   
00439   void reconfigureHook(int level)
00440   {
00441     if (private_node_handle_.hasParam("frameid"))
00442     {
00443       ROS_WARN("~frameid is deprecated, please use ~frame_id instead");
00444       private_node_handle_.getParam("frameid", driver_.config_.frame_id);
00445     }
00446 
00447     if (private_node_handle_.hasParam("min_ang_degrees"))
00448     {
00449       ROS_WARN("~min_ang_degrees is deprecated, please use ~min_ang instead");
00450       private_node_handle_.getParam("min_ang_degrees", driver_.config_.min_ang);
00451       driver_.config_.min_ang *= M_PI/180;
00452     }
00453 
00454     if (private_node_handle_.hasParam("max_ang_degrees"))
00455     {
00456       ROS_WARN("~max_ang_degrees is deprecated, please use ~max_ang instead");
00457       private_node_handle_.getParam("max_ang_degrees", driver_.config_.max_ang);
00458       driver_.config_.max_ang *= M_PI/180;
00459     }
00460 
00461     diagnostic_.force_update();   
00462     
00463     scan_pub_.clear_window(); // Reduce glitches in the frequency diagnostic.
00464   }
00465 
00466   int publishScan(const hokuyo::LaserScan &scan)
00467   {
00468     //ROS_DEBUG("publishScan");
00469 
00470     scan_msg_.angle_min = scan.config.min_angle;
00471     scan_msg_.angle_max = scan.config.max_angle;
00472     scan_msg_.angle_increment = scan.config.ang_increment;
00473     scan_msg_.time_increment = scan.config.time_increment;
00474     scan_msg_.scan_time = scan.config.scan_time;
00475     scan_msg_.range_min = scan.config.min_range;
00476     scan_msg_.range_max = scan.config.max_range;
00477     scan_msg_.ranges = scan.ranges;
00478     scan_msg_.intensities = scan.intensities;
00479     scan_msg_.header.stamp = ros::Time().fromNSec((uint64_t)scan.system_time_stamp) + ros::Duration(driver_.config_.time_offset);
00480     scan_msg_.header.frame_id = driver_.config_.frame_id;
00481   
00482     desired_freq_ = (1. / scan.config.scan_time);
00483 
00484     scan_pub_.publish(scan_msg_);
00485 
00486     //ROS_DEBUG("publishScan done");
00487 
00488     return(0);
00489   }
00490 
00491   void connectionStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
00492   {
00493     if (driver_.state_ == driver_.CLOSED)
00494       status.summary(2, "Not connected. " + driver_.device_status_ + " " + connect_fail_);
00495     else if (driver_.device_status_ != std::string("Sensor works well."))
00496       status.summaryf(2, "Abnormal status: %s", driver_.device_status_.c_str());
00497     else if (driver_.state_ == driver_.RUNNING)
00498     {
00499       if (driver_.first_scan_)
00500         status.summary(0, "Waiting for first scan");
00501       else
00502         status.summary(0, "Streaming");
00503     }
00504     else if (driver_.state_ == driver_.OPENED)
00505       status.summary(0, "Open");
00506     else 
00507       status.summary(2, "Unknown state");
00508 
00509     status.add("Device Status", driver_.device_status_);
00510     status.add("Port", driver_.config_.port);
00511     status.add("Device ID", driver_.device_id_);
00512     status.add("Scan Thread Lost Count", driver_.lost_scan_thread_count_);
00513     status.add("Corrupted Scan Count", driver_.corrupted_scan_count_);
00514     status.add("Vendor Name", driver_.vendor_name_);
00515     status.add("Product Name", driver_.product_name_);
00516     status.add("Firmware Version", driver_.firmware_version_);
00517     status.add("Protocol Version", driver_.protocol_version_);
00518     status.add("Computed Latency", driver_.laser_.getLatency());
00519     status.add("User Time Offset", driver_.config_.time_offset);
00520   }
00521 
00522   void statusTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00523   {
00524     std::string stat = driver_.laser_.getStatus();
00525 
00526     if (stat != std::string("Sensor works well."))
00527     {
00528       status.level = 2;
00529     } else {
00530       status.level = 0;
00531     }
00532 
00533     status.message = stat;
00534   }
00535 
00536   void laserTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00537   {
00538     driver_.laser_.laserOn();
00539 
00540     status.level = 0;
00541     status.message = "Laser turned on successfully.";
00542   }
00543 
00544   void polledDataTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00545   {
00546     hokuyo::LaserScan  scan;
00547 
00548     int res = driver_.laser_.pollScan(scan, driver_.laser_config_.min_angle, driver_.laser_config_.max_angle, 1, 1000);
00549 
00550     if (res != 0)
00551     {
00552       status.level = 2;
00553       ostringstream oss;
00554       oss << "Hokuyo error code: " << res << ". Consult manual for meaning.";
00555       status.message = oss.str();
00556 
00557     } else {
00558       status.level = 0;
00559       status.message = "Polled Hokuyo for data successfully.";
00560     }
00561   }
00562 
00563   void streamedDataTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00564   {
00565     hokuyo::LaserScan  scan;
00566 
00567     int res = driver_.laser_.requestScans(false, driver_.laser_config_.min_angle, driver_.laser_config_.max_angle, 1, 1, 99, 1000);
00568 
00569     if (res != 0)
00570     {
00571       status.level = 2;
00572       ostringstream oss;
00573       oss << "Hokuyo error code: " << res << ". Consult manual for meaning.";
00574       status.message = oss.str();
00575 
00576     } else {
00577 
00578       for (int i = 0; i < 99; i++)
00579       {
00580         driver_.laser_.serviceScan(scan, 1000);
00581       }
00582 
00583       status.level = 0;
00584       status.message = "Streamed data from Hokuyo successfully.";
00585 
00586     }
00587   }
00588 
00589   void streamedIntensityDataTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00590   {
00591     hokuyo::LaserScan  scan;
00592 
00593     int res = driver_.laser_.requestScans(false, driver_.laser_config_.min_angle, driver_.laser_config_.max_angle, 1, 1, 99, 1000);
00594 
00595     if (res != 0)
00596     {
00597       status.level = 2;
00598       ostringstream oss;
00599       oss << "Hokuyo error code: " << res << ". Consult manual for meaning.";
00600       status.message = oss.str();
00601 
00602     } else {
00603 
00604       int corrupted_data = 0;
00605 
00606       for (int i = 0; i < 99; i++)
00607       {
00608         try {
00609           driver_.laser_.serviceScan(scan, 1000);
00610         } catch (hokuyo::CorruptedDataException &e) {
00611           corrupted_data++;
00612         }
00613       }
00614       if (corrupted_data == 1)
00615       {
00616         status.level = 1;
00617         status.message = "Single corrupted message.  This is acceptable and unavoidable";
00618       } else if (corrupted_data > 1)
00619       {
00620         status.level = 2;
00621         ostringstream oss;
00622         oss << corrupted_data << " corrupted messages.";
00623         status.message = oss.str();
00624       } else
00625       {
00626         status.level = 0;
00627         status.message = "Stramed data with intensity from Hokuyo successfully.";
00628       }
00629     }
00630   }
00631 
00632   void laserOffTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00633   {
00634     driver_.laser_.laserOff();
00635 
00636     status.level = 0;
00637     status.message = "Laser turned off successfully.";
00638   }
00639 };
00640 
00641 int main(int argc, char **argv)
00642 { 
00643   return driver_base::main<HokuyoNode>(argc, argv, "hokuyo_node");
00644 }
00645 


hokuyo_node
Author(s): Brian P. Gerkey, Jeremy Leibs, Blaise Gassend
autogenerated on Sat Jun 8 2019 18:33:27