urg_node_driver.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Willow Garage, Inc.
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 the Willow Garage, Inc. 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 
00030 /*
00031  * Author: Chad Rockey, Michael Carroll, Mike O'Driscoll
00032  */
00033 
00034 #include "urg_node/urg_node_driver.h"
00035 
00036 #include <tf/tf.h>  // tf header for resolving tf prefix
00037 #include <string>
00038 #include <diagnostic_msgs/DiagnosticStatus.h>
00039 #include <urg_node/Status.h>
00040 
00041 namespace urg_node
00042 {
00043 
00044 // Useful typedefs
00045 typedef diagnostic_updater::FrequencyStatusParam FrequencyStatusParam;
00046 
00047 UrgNode::UrgNode(ros::NodeHandle nh, ros::NodeHandle private_nh) :
00048   nh_(nh),
00049   pnh_(private_nh)
00050 {
00051   initSetup();
00052 }
00053 
00054 UrgNode::UrgNode():
00055   nh_(),
00056   pnh_("~")
00057 {
00058   initSetup();
00059 }
00060 
00061 void UrgNode::initSetup()
00062 {
00063   close_diagnostics_ = true;
00064   close_scan_ = true;
00065   service_yield_ = false;
00066 
00067   error_code_ = 0;
00068   lockout_status_ = false;
00069   // Initialize node and nodehandles
00070 
00071   // Get parameters so we can change these later.
00072   pnh_.param<std::string>("ip_address", ip_address_, "");
00073   pnh_.param<int>("ip_port", ip_port_, 10940);
00074   pnh_.param<std::string>("serial_port", serial_port_, "/dev/ttyACM0");
00075   pnh_.param<int>("serial_baud", serial_baud_, 115200);
00076   pnh_.param<bool>("calibrate_time", calibrate_time_, false);
00077   pnh_.param<bool>("synchronize_time", synchronize_time_, false);
00078   pnh_.param<bool>("publish_intensity", publish_intensity_, true);
00079   pnh_.param<bool>("publish_multiecho", publish_multiecho_, false);
00080   pnh_.param<int>("error_limit", error_limit_, 4);
00081   pnh_.param<double>("diagnostics_tolerance", diagnostics_tolerance_, 0.05);
00082   pnh_.param<double>("diagnostics_window_time", diagnostics_window_time_, 5.0);
00083   pnh_.param<bool>("get_detailed_status", detailed_status_, false);
00084 
00085   // Set up publishers and diagnostics updaters, we only need one
00086   if (publish_multiecho_)
00087   {
00088     echoes_pub_ = laser_proc::LaserTransport::advertiseLaser(nh_, 20);
00089   }
00090   else
00091   {
00092     laser_pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 20);
00093   }
00094 
00095   status_service_ = nh_.advertiseService("update_laser_status", &UrgNode::statusCallback, this);
00096   status_pub_ = nh_.advertise<urg_node::Status>("laser_status", 1, true);
00097 
00098   diagnostic_updater_.reset(new diagnostic_updater::Updater);
00099   diagnostic_updater_->add("Hardware Status", this, &UrgNode::populateDiagnosticsStatus);
00100 }
00101 
00102 UrgNode::~UrgNode()
00103 {
00104   if (diagnostics_thread_.joinable())
00105   {
00106     // Clean up our diagnostics thread.
00107     close_diagnostics_ = true;
00108     diagnostics_thread_.join();
00109   }
00110   if (scan_thread_.joinable())
00111   {
00112     close_scan_ = true;
00113     scan_thread_.join();
00114   }
00115 }
00116 
00117 bool UrgNode::updateStatus()
00118 {
00119   bool result = false;
00120   service_yield_ = true;
00121   boost::mutex::scoped_lock lock(lidar_mutex_);
00122 
00123   if (urg_)
00124   {
00125     device_status_ = urg_->getSensorStatus();
00126 
00127     if (detailed_status_)
00128     {
00129       URGStatus status;
00130       if (urg_->getAR00Status(status))
00131       {
00132         urg_node::Status msg;
00133         msg.operating_mode = status.operating_mode;
00134         msg.error_status = status.error_status;
00135         msg.error_code = status.error_code;
00136         msg.lockout_status = status.lockout_status;
00137 
00138         lockout_status_ = status.lockout_status;
00139         error_code_ = status.error_code;
00140 
00141         UrgDetectionReport report;
00142         if (urg_->getDL00Status(report))
00143         {
00144           msg.area_number = report.area;
00145           msg.distance = report.distance;
00146           msg.angle = report.angle;
00147         }
00148         else
00149         {
00150            ROS_WARN("Failed to get detection report.");
00151         }
00152 
00153         // Publish the status on the latched topic for inspection.
00154         status_pub_.publish(msg);
00155         result = true;
00156       }
00157       else
00158       {
00159         ROS_WARN("Failed to retrieve status");
00160 
00161         urg_node::Status msg;
00162         status_pub_.publish(msg);
00163       }
00164     }
00165   }
00166   return result;
00167 }
00168 
00169 bool UrgNode::statusCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00170 {
00171   ROS_INFO("Got update lidar status callback");
00172   res.success = false;
00173   res.message = "Laser not ready";
00174 
00175   if (updateStatus())
00176   {
00177     res.message = "Status retrieved";
00178     res.success = true;
00179   }
00180   else
00181   {
00182     res.message = "Failed to update status";
00183     res.success = false;
00184   }
00185 
00186   return true;
00187 }
00188 
00189 bool UrgNode::reconfigure_callback(urg_node::URGConfig& config, int level)
00190 {
00191   if (!urg_)
00192   {
00193     ROS_ERROR("Reconfigure failed, not ready");
00194     return false;
00195   }
00196 
00197   if (level < 0)  // First call, initialize, laser not yet started
00198   {
00199     urg_->setAngleLimitsAndCluster(config.angle_min, config.angle_max, config.cluster);
00200     urg_->setSkip(config.skip);
00201   }
00202   else if (level > 0)   // Must stop
00203   {
00204     urg_->stop();
00205     ROS_INFO("Stopped data due to reconfigure.");
00206 
00207     // Change values that required stopping
00208     urg_->setAngleLimitsAndCluster(config.angle_min, config.angle_max, config.cluster);
00209     urg_->setSkip(config.skip);
00210 
00211     try
00212     {
00213       urg_->start();
00214       ROS_INFO("Streaming data after reconfigure.");
00215     }
00216     catch (std::runtime_error& e)
00217     {
00218       ROS_FATAL("%s", e.what());
00219       ros::Duration(1.0).sleep();
00220       ros::shutdown();
00221       return false;
00222     }
00223   }
00224 
00225   // The publish frequency changes based on the number of skipped scans.
00226   // Update accordingly here.
00227   freq_min_ = 1.0 / (urg_->getScanPeriod() * (config.skip + 1));
00228 
00229   std::string frame_id = tf::resolve(config.tf_prefix, config.frame_id);
00230   urg_->setFrameId(frame_id);
00231   urg_->setUserLatency(config.time_offset);
00232 
00233   return true;
00234 }
00235 
00236 void UrgNode::update_reconfigure_limits()
00237 {
00238   if (!urg_ || !srv_)
00239   {
00240     ROS_DEBUG_THROTTLE(10, "Unable to update reconfigure limits. Not Ready.");
00241     return;
00242   }
00243 
00244   urg_node::URGConfig min, max;
00245   srv_->getConfigMin(min);
00246   srv_->getConfigMax(max);
00247 
00248   min.angle_min = urg_->getAngleMinLimit();
00249   min.angle_max = min.angle_min;
00250   max.angle_max = urg_->getAngleMaxLimit();
00251   max.angle_min = max.angle_max;
00252 
00253   srv_->setConfigMin(min);
00254   srv_->setConfigMax(max);
00255 }
00256 
00257 void UrgNode::calibrate_time_offset()
00258 {
00259   boost::mutex::scoped_lock lock(lidar_mutex_);
00260   if (!urg_)
00261   {
00262     ROS_DEBUG_THROTTLE(10, "Unable to calibrate time offset. Not Ready.");
00263     return;
00264   }
00265   try
00266   {
00267     // Don't let outside interruption effect lidar offset.
00268     ROS_INFO("Starting calibration. This will take a few seconds.");
00269     ROS_WARN("Time calibration is still experimental.");
00270     ros::Duration latency = urg_->computeLatency(10);
00271     ROS_INFO("Calibration finished. Latency is: %.4f.", latency.toSec());
00272   }
00273   catch (std::runtime_error& e)
00274   {
00275     ROS_FATAL("Could not calibrate time offset: %s", e.what());
00276     ros::Duration(1.0).sleep();
00277     ros::shutdown();
00278   }
00279 }
00280 
00281 
00282 // Diagnostics update task to be run in a thread.
00283 void UrgNode::updateDiagnostics()
00284 {
00285   while (!close_diagnostics_)
00286   {
00287     diagnostic_updater_->update();
00288     boost::this_thread::sleep_for(boost::chrono::milliseconds(10));
00289   }
00290 }
00291 
00292 // Populate a diagnostics status message.
00293 void UrgNode::populateDiagnosticsStatus(diagnostic_updater::DiagnosticStatusWrapper &stat)
00294 {
00295   if (!urg_)
00296   {
00297     stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
00298         "Not Connected");
00299     return;
00300   }
00301 
00302   if (!urg_->getIPAddress().empty())
00303   {
00304     stat.add("IP Address", urg_->getIPAddress());
00305     stat.add("IP Port", urg_->getIPPort());
00306   }
00307   else
00308   {
00309     stat.add("Serial Port", urg_->getSerialPort());
00310     stat.add("Serial Baud", urg_->getSerialBaud());
00311   }
00312 
00313   if (!urg_->isStarted())
00314   {
00315     stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
00316         "Not Connected: " + device_status_);
00317   }
00318   else if (device_status_ != std::string("Sensor works well.") &&
00319            device_status_ != std::string("Stable 000 no error.") &&
00320            device_status_ != std::string("sensor is working normally"))
00321   {
00322     stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
00323         "Abnormal status: " + device_status_);
00324   }
00325   else if (error_code_ != 0)
00326   {
00327     stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR,
00328         "Lidar reporting error code: %X",
00329         error_code_);
00330   }
00331   else if (lockout_status_)
00332   {
00333     stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
00334         "Lidar locked out.");
00335   }
00336   else
00337   {
00338     stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00339         "Streaming");
00340   }
00341 
00342   stat.add("Vendor Name", vendor_name_);
00343   stat.add("Product Name", product_name_);
00344   stat.add("Firmware Version", firmware_version_);
00345   stat.add("Firmware Date", firmware_date_);
00346   stat.add("Protocol Version", protocol_version_);
00347   stat.add("Device ID", device_id_);
00348   stat.add("Computed Latency", urg_->getComputedLatency());
00349   stat.add("User Time Offset", urg_->getUserTimeOffset());
00350 
00351   // Things not explicitly required by REP-0138, but still interesting.
00352   stat.add("Device Status", device_status_);
00353   stat.add("Scan Retrieve Error Count", error_count_);
00354 
00355   stat.add("Lidar Error Code", error_code_);
00356   stat.add("Locked out", lockout_status_);
00357 }
00358 
00359 bool UrgNode::connect()
00360 {
00361   // Don't let external access to retrieve
00362   // status during the connection process.
00363   boost::mutex::scoped_lock lock(lidar_mutex_);
00364 
00365   try
00366   {
00367     urg_.reset();  // Clear any previous connections();
00368     if (!ip_address_.empty())
00369     {
00370       urg_.reset(new urg_node::URGCWrapper(ip_address_, ip_port_,
00371           publish_intensity_, publish_multiecho_, synchronize_time_));
00372     }
00373     else
00374     {
00375       urg_.reset(new urg_node::URGCWrapper(serial_baud_, serial_port_,
00376           publish_intensity_, publish_multiecho_, synchronize_time_));
00377     }
00378 
00379     std::stringstream ss;
00380     ss << "Connected to";
00381     if (publish_multiecho_)
00382     {
00383       ss << " multiecho";
00384     }
00385     if (!ip_address_.empty())
00386     {
00387       ss << " network";
00388     }
00389     else
00390     {
00391       ss << " serial";
00392     }
00393     ss << " device with";
00394     if (publish_intensity_)
00395     {
00396       ss << " intensity and";
00397     }
00398     ss << " ID: " << urg_->getDeviceID();
00399     ROS_INFO_STREAM(ss.str());
00400 
00401     device_status_ = urg_->getSensorStatus();
00402     vendor_name_ = urg_->getVendorName();
00403     product_name_ = urg_->getProductName();
00404     firmware_version_ = urg_->getFirmwareVersion();
00405     firmware_date_ = urg_->getFirmwareDate();
00406     protocol_version_ = urg_->getProtocolVersion();
00407     device_id_ = urg_->getDeviceID();
00408 
00409     if (diagnostic_updater_ && urg_)
00410     {
00411       diagnostic_updater_->setHardwareID(urg_->getDeviceID());
00412     }
00413 
00414     return true;
00415   }
00416   catch (std::runtime_error& e)
00417   {
00418     ROS_ERROR_THROTTLE(10.0, "Error connecting to Hokuyo: %s", e.what());
00419     urg_.reset();
00420     return false;
00421   }
00422   catch (std::exception& e)
00423   {
00424     ROS_ERROR_THROTTLE(10.0, "Unknown error connecting to Hokuyo: %s", e.what());
00425     urg_.reset();
00426     return false;
00427   }
00428 
00429   return false;
00430 }
00431 
00432 void UrgNode::scanThread()
00433 {
00434   while (!close_scan_)
00435   {
00436     if (!urg_)
00437     {
00438       if (!connect())
00439       {
00440         boost::this_thread::sleep(boost::posix_time::milliseconds(500));
00441         continue;  // Connect failed, sleep, try again.
00442       }
00443     }
00444 
00445     // Configure limits (Must do this after creating the urgwidget)
00446     update_reconfigure_limits();
00447 
00448     if (calibrate_time_)
00449     {
00450       calibrate_time_offset();
00451     }
00452 
00453     // Clear the dynamic reconfigure server
00454     srv_.reset();
00455     // Spin once to de-register it's services before making a new
00456     // service next.
00457     ros::spinOnce();
00458 
00459     if (!urg_ || !ros::ok)
00460     {
00461       continue;
00462     }
00463     else
00464     {
00465       // Set up dynamic reconfigure
00466       srv_.reset(new dynamic_reconfigure::Server<urg_node::URGConfig>(pnh_));
00467       // Configure limits (Must do this after creating the urgwidget)
00468       update_reconfigure_limits();
00469       srv_->setCallback(boost::bind(&UrgNode::reconfigure_callback, this, _1, _2));
00470     }
00471 
00472     // Before starting, update the status
00473     updateStatus();
00474 
00475     // Start the urgwidget
00476     try
00477     {
00478       // If the connection failed, don't try and connect
00479       // pointer is invalid.
00480       if (!urg_)
00481       {
00482         continue;  // Return to top of main loop, not connected.
00483       }
00484       device_status_ = urg_->getSensorStatus();
00485       urg_->start();
00486       ROS_INFO("Streaming data.");
00487       // Clear the error count.
00488       error_count_ = 0;
00489     }
00490     catch (std::runtime_error& e)
00491     {
00492       ROS_ERROR_THROTTLE(10.0, "Error starting Hokuyo: %s", e.what());
00493       urg_.reset();
00494       ros::Duration(1.0).sleep();
00495       continue;  // Return to top of main loop
00496     }
00497     catch (...)
00498     {
00499       ROS_ERROR_THROTTLE(10.0, "Unknown error starting Hokuyo");
00500       urg_.reset();
00501       ros::Duration(1.0).sleep();
00502       continue;  // Return to top of main loop
00503     }
00504 
00505     while (!close_scan_)
00506     {
00507       // Don't allow external access during grabbing the scan.
00508       try
00509       {
00510         boost::mutex::scoped_lock lock(lidar_mutex_);
00511         if (publish_multiecho_)
00512         {
00513           const sensor_msgs::MultiEchoLaserScanPtr msg(new sensor_msgs::MultiEchoLaserScan());
00514           if (urg_->grabScan(msg))
00515           {
00516             echoes_pub_.publish(msg);
00517             echoes_freq_->tick();
00518           }
00519           else
00520           {
00521             ROS_WARN_THROTTLE(10.0, "Could not grab multi echo scan.");
00522             device_status_ = urg_->getSensorStatus();
00523             error_count_++;
00524           }
00525         }
00526         else
00527         {
00528           const sensor_msgs::LaserScanPtr msg(new sensor_msgs::LaserScan());
00529           if (urg_->grabScan(msg))
00530           {
00531             laser_pub_.publish(msg);
00532             laser_freq_->tick();
00533           }
00534           else
00535           {
00536             ROS_WARN_THROTTLE(10.0, "Could not grab single echo scan.");
00537             device_status_ = urg_->getSensorStatus();
00538             error_count_++;
00539           }
00540         }
00541       }
00542       catch (...)
00543       {
00544         ROS_ERROR_THROTTLE(10.0, "Unknown error grabbing Hokuyo scan.");
00545         error_count_++;
00546       }
00547 
00548       if (service_yield_)
00549       {
00550         boost::this_thread::sleep_for(boost::chrono::milliseconds(10));
00551         service_yield_ = false;
00552       }
00553 
00554       // Reestablish conneciton if things seem to have gone wrong.
00555       if (error_count_ > error_limit_)
00556       {
00557         ROS_ERROR_THROTTLE(10.0, "Error count exceeded limit, reconnecting.");
00558         urg_.reset();
00559         ros::Duration(2.0).sleep();
00560 
00561         break;  // Return to top of main loop
00562       }
00563     }
00564   }
00565 }
00566 
00567 void UrgNode::run()
00568 {
00569   // Setup initial connection
00570   connect();
00571 
00572   // Stop diagnostics
00573   if (!close_diagnostics_)
00574   {
00575     close_diagnostics_ = true;
00576     diagnostics_thread_.join();
00577   }
00578 
00579   if (publish_multiecho_)
00580   {
00581     echoes_freq_.reset(new diagnostic_updater::HeaderlessTopicDiagnostic("Laser Echoes",
00582           *diagnostic_updater_,
00583           FrequencyStatusParam(&freq_min_, &freq_min_, diagnostics_tolerance_, diagnostics_window_time_)));
00584   }
00585   else
00586   {
00587     laser_freq_.reset(new diagnostic_updater::HeaderlessTopicDiagnostic("Laser Scan",
00588           *diagnostic_updater_,
00589           FrequencyStatusParam(&freq_min_, &freq_min_, diagnostics_tolerance_, diagnostics_window_time_)));
00590   }
00591 
00592   // Now that we are setup, kick off diagnostics.
00593   close_diagnostics_ = false;
00594   diagnostics_thread_ = boost::thread(boost::bind(&UrgNode::updateDiagnostics, this));
00595 
00596   // Start scanning now that everything is configured.
00597   close_scan_ = false;
00598   scan_thread_ = boost::thread(boost::bind(&UrgNode::scanThread, this));
00599 }
00600 }  // namespace urg_node


urg_node
Author(s): Chad Rockey , Mike O'Driscoll
autogenerated on Sat Jun 8 2019 19:16:00