urg_node_driver.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 /*
31  * Author: Chad Rockey, Michael Carroll, Mike O'Driscoll
32  */
33 
35 
36 #include <tf/tf.h> // tf header for resolving tf prefix
37 #include <string>
38 #include <diagnostic_msgs/DiagnosticStatus.h>
39 #include <urg_node/Status.h>
40 
41 namespace urg_node
42 {
43 
44 // Useful typedefs
46 
48  nh_(nh),
49  pnh_(private_nh)
50 {
51  initSetup();
52 }
53 
55  nh_(),
56  pnh_("~")
57 {
58  initSetup();
59 }
60 
62 {
63  close_diagnostics_ = true;
64  close_scan_ = true;
65  service_yield_ = false;
66 
67  error_code_ = 0;
68  lockout_status_ = false;
69  // Initialize node and nodehandles
70 
71  // Get parameters so we can change these later.
72  pnh_.param<std::string>("ip_address", ip_address_, "");
73  pnh_.param<int>("ip_port", ip_port_, 10940);
74  pnh_.param<std::string>("serial_port", serial_port_, "/dev/ttyACM0");
75  pnh_.param<int>("serial_baud", serial_baud_, 115200);
76  pnh_.param<bool>("calibrate_time", calibrate_time_, false);
77  pnh_.param<bool>("synchronize_time", synchronize_time_, false);
78  pnh_.param<bool>("publish_intensity", publish_intensity_, true);
79  pnh_.param<bool>("publish_multiecho", publish_multiecho_, false);
80  pnh_.param<int>("error_limit", error_limit_, 4);
81  pnh_.param<double>("diagnostics_tolerance", diagnostics_tolerance_, 0.05);
82  pnh_.param<double>("diagnostics_window_time", diagnostics_window_time_, 5.0);
83  pnh_.param<bool>("get_detailed_status", detailed_status_, false);
84 
85  // Set up publishers and diagnostics updaters, we only need one
86  if (publish_multiecho_)
87  {
89  }
90  else
91  {
92  laser_pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 20);
93  }
94 
95  status_service_ = nh_.advertiseService("update_laser_status", &UrgNode::statusCallback, this);
96  status_pub_ = nh_.advertise<urg_node::Status>("laser_status", 1, true);
97 
99  diagnostic_updater_->add("Hardware Status", this, &UrgNode::populateDiagnosticsStatus);
100 }
101 
103 {
104  if (diagnostics_thread_.joinable())
105  {
106  // Clean up our diagnostics thread.
107  close_diagnostics_ = true;
108  diagnostics_thread_.join();
109  }
110  if (scan_thread_.joinable())
111  {
112  close_scan_ = true;
113  scan_thread_.join();
114  }
115 }
116 
118 {
119  bool result = false;
120  service_yield_ = true;
121  boost::mutex::scoped_lock lock(lidar_mutex_);
122 
123  if (urg_)
124  {
125  device_status_ = urg_->getSensorStatus();
126 
127  if (detailed_status_)
128  {
129  URGStatus status;
130  if (urg_->getAR00Status(status))
131  {
132  urg_node::Status msg;
133  msg.operating_mode = status.operating_mode;
134  msg.error_status = status.error_status;
135  msg.error_code = status.error_code;
136  msg.lockout_status = status.lockout_status;
137 
139  error_code_ = status.error_code;
140 
141  UrgDetectionReport report;
142  if (urg_->getDL00Status(report))
143  {
144  msg.area_number = report.area;
145  msg.distance = report.distance;
146  msg.angle = report.angle;
147  }
148  else
149  {
150  ROS_WARN("Failed to get detection report.");
151  }
152 
153  // Publish the status on the latched topic for inspection.
154  status_pub_.publish(msg);
155  result = true;
156  }
157  else
158  {
159  ROS_WARN("Failed to retrieve status");
160 
161  urg_node::Status msg;
162  status_pub_.publish(msg);
163  }
164  }
165  }
166  return result;
167 }
168 
169 bool UrgNode::statusCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
170 {
171  ROS_INFO("Got update lidar status callback");
172  res.success = false;
173  res.message = "Laser not ready";
174 
175  if (updateStatus())
176  {
177  res.message = "Status retrieved";
178  res.success = true;
179  }
180  else
181  {
182  res.message = "Failed to update status";
183  res.success = false;
184  }
185 
186  return true;
187 }
188 
189 bool UrgNode::reconfigure_callback(urg_node::URGConfig& config, int level)
190 {
191  if (!urg_)
192  {
193  ROS_ERROR("Reconfigure failed, not ready");
194  return false;
195  }
196 
197  if (level < 0) // First call, initialize, laser not yet started
198  {
199  urg_->setAngleLimitsAndCluster(config.angle_min, config.angle_max, config.cluster);
200  urg_->setSkip(config.skip);
201  }
202  else if (level > 0) // Must stop
203  {
204  urg_->stop();
205  ROS_INFO("Stopped data due to reconfigure.");
206 
207  // Change values that required stopping
208  urg_->setAngleLimitsAndCluster(config.angle_min, config.angle_max, config.cluster);
209  urg_->setSkip(config.skip);
210 
211  try
212  {
213  urg_->start();
214  ROS_INFO("Streaming data after reconfigure.");
215  }
216  catch (std::runtime_error& e)
217  {
218  ROS_FATAL("%s", e.what());
219  ros::Duration(1.0).sleep();
220  ros::shutdown();
221  return false;
222  }
223  }
224 
225  // The publish frequency changes based on the number of skipped scans.
226  // Update accordingly here.
227  freq_min_ = 1.0 / (urg_->getScanPeriod() * (config.skip + 1));
228 
229  std::string frame_id = tf::resolve(config.tf_prefix, config.frame_id);
230  urg_->setFrameId(frame_id);
231  urg_->setUserLatency(config.time_offset);
232 
233  return true;
234 }
235 
237 {
238  if (!urg_ || !srv_)
239  {
240  ROS_DEBUG_THROTTLE(10, "Unable to update reconfigure limits. Not Ready.");
241  return;
242  }
243 
244  urg_node::URGConfig min, max;
245  srv_->getConfigMin(min);
246  srv_->getConfigMax(max);
247 
248  min.angle_min = urg_->getAngleMinLimit();
249  min.angle_max = min.angle_min;
250  max.angle_max = urg_->getAngleMaxLimit();
251  max.angle_min = max.angle_max;
252 
253  srv_->setConfigMin(min);
254  srv_->setConfigMax(max);
255 }
256 
258 {
259  boost::mutex::scoped_lock lock(lidar_mutex_);
260  if (!urg_)
261  {
262  ROS_DEBUG_THROTTLE(10, "Unable to calibrate time offset. Not Ready.");
263  return;
264  }
265  try
266  {
267  // Don't let outside interruption effect lidar offset.
268  ROS_INFO("Starting calibration. This will take a few seconds.");
269  ROS_WARN("Time calibration is still experimental.");
270  ros::Duration latency = urg_->computeLatency(10);
271  ROS_INFO("Calibration finished. Latency is: %.4f.", latency.toSec());
272  }
273  catch (std::runtime_error& e)
274  {
275  ROS_FATAL("Could not calibrate time offset: %s", e.what());
276  ros::Duration(1.0).sleep();
277  ros::shutdown();
278  }
279 }
280 
281 
282 // Diagnostics update task to be run in a thread.
284 {
285  while (!close_diagnostics_)
286  {
287  diagnostic_updater_->update();
288  boost::this_thread::sleep_for(boost::chrono::milliseconds(10));
289  }
290 }
291 
292 // Populate a diagnostics status message.
294 {
295  if (!urg_)
296  {
297  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
298  "Not Connected");
299  return;
300  }
301 
302  if (!urg_->getIPAddress().empty())
303  {
304  stat.add("IP Address", urg_->getIPAddress());
305  stat.add("IP Port", urg_->getIPPort());
306  }
307  else
308  {
309  stat.add("Serial Port", urg_->getSerialPort());
310  stat.add("Serial Baud", urg_->getSerialBaud());
311  }
312 
313  if (!urg_->isStarted())
314  {
315  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
316  "Not Connected: " + device_status_);
317  }
318  else if (device_status_ != std::string("Sensor works well.") &&
319  device_status_ != std::string("Stable 000 no error.") &&
320  device_status_ != std::string("sensor is working normally"))
321  {
322  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
323  "Abnormal status: " + device_status_);
324  }
325  else if (error_code_ != 0)
326  {
327  stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR,
328  "Lidar reporting error code: %X",
329  error_code_);
330  }
331  else if (lockout_status_)
332  {
333  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
334  "Lidar locked out.");
335  }
336  else
337  {
338  stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
339  "Streaming");
340  }
341 
342  stat.add("Vendor Name", vendor_name_);
343  stat.add("Product Name", product_name_);
344  stat.add("Firmware Version", firmware_version_);
345  stat.add("Firmware Date", firmware_date_);
346  stat.add("Protocol Version", protocol_version_);
347  stat.add("Device ID", device_id_);
348  stat.add("Computed Latency", urg_->getComputedLatency());
349  stat.add("User Time Offset", urg_->getUserTimeOffset());
350 
351  // Things not explicitly required by REP-0138, but still interesting.
352  stat.add("Device Status", device_status_);
353  stat.add("Scan Retrieve Error Count", error_count_);
354 
355  stat.add("Lidar Error Code", error_code_);
356  stat.add("Locked out", lockout_status_);
357 }
358 
360 {
361  // Don't let external access to retrieve
362  // status during the connection process.
363  boost::mutex::scoped_lock lock(lidar_mutex_);
364 
365  try
366  {
367  urg_.reset(); // Clear any previous connections();
368  if (!ip_address_.empty())
369  {
372  }
373  else
374  {
377  }
378 
379  std::stringstream ss;
380  ss << "Connected to";
381  if (publish_multiecho_)
382  {
383  ss << " multiecho";
384  }
385  if (!ip_address_.empty())
386  {
387  ss << " network";
388  }
389  else
390  {
391  ss << " serial";
392  }
393  ss << " device with";
394  if (publish_intensity_)
395  {
396  ss << " intensity and";
397  }
398  ss << " ID: " << urg_->getDeviceID();
399  ROS_INFO_STREAM(ss.str());
400 
401  device_status_ = urg_->getSensorStatus();
402  vendor_name_ = urg_->getVendorName();
403  product_name_ = urg_->getProductName();
404  firmware_version_ = urg_->getFirmwareVersion();
405  firmware_date_ = urg_->getFirmwareDate();
406  protocol_version_ = urg_->getProtocolVersion();
407  device_id_ = urg_->getDeviceID();
408 
409  if (diagnostic_updater_ && urg_)
410  {
411  diagnostic_updater_->setHardwareID(urg_->getDeviceID());
412  }
413 
414  return true;
415  }
416  catch (std::runtime_error& e)
417  {
418  ROS_ERROR_THROTTLE(10.0, "Error connecting to Hokuyo: %s", e.what());
419  urg_.reset();
420  return false;
421  }
422  catch (std::exception& e)
423  {
424  ROS_ERROR_THROTTLE(10.0, "Unknown error connecting to Hokuyo: %s", e.what());
425  urg_.reset();
426  return false;
427  }
428 
429  return false;
430 }
431 
433 {
434  while (!close_scan_)
435  {
436  if (!urg_)
437  {
438  if (!connect())
439  {
440  boost::this_thread::sleep(boost::posix_time::milliseconds(500));
441  continue; // Connect failed, sleep, try again.
442  }
443  }
444 
445  // Configure limits (Must do this after creating the urgwidget)
447 
448  if (calibrate_time_)
449  {
451  }
452 
453  // Clear the dynamic reconfigure server
454  srv_.reset();
455  // Spin once to de-register it's services before making a new
456  // service next.
457  ros::spinOnce();
458 
459  if (!urg_ || !ros::ok)
460  {
461  continue;
462  }
463  else
464  {
465  // Set up dynamic reconfigure
466  srv_.reset(new dynamic_reconfigure::Server<urg_node::URGConfig>(pnh_));
467  // Configure limits (Must do this after creating the urgwidget)
469  srv_->setCallback(boost::bind(&UrgNode::reconfigure_callback, this, _1, _2));
470  }
471 
472  // Before starting, update the status
473  updateStatus();
474 
475  // Start the urgwidget
476  try
477  {
478  // If the connection failed, don't try and connect
479  // pointer is invalid.
480  if (!urg_)
481  {
482  continue; // Return to top of main loop, not connected.
483  }
484  device_status_ = urg_->getSensorStatus();
485  urg_->start();
486  ROS_INFO("Streaming data.");
487  // Clear the error count.
488  error_count_ = 0;
489  }
490  catch (std::runtime_error& e)
491  {
492  ROS_ERROR_THROTTLE(10.0, "Error starting Hokuyo: %s", e.what());
493  urg_.reset();
494  ros::Duration(1.0).sleep();
495  continue; // Return to top of main loop
496  }
497  catch (...)
498  {
499  ROS_ERROR_THROTTLE(10.0, "Unknown error starting Hokuyo");
500  urg_.reset();
501  ros::Duration(1.0).sleep();
502  continue; // Return to top of main loop
503  }
504 
505  while (!close_scan_)
506  {
507  // Don't allow external access during grabbing the scan.
508  try
509  {
510  boost::mutex::scoped_lock lock(lidar_mutex_);
511  if (publish_multiecho_)
512  {
513  const sensor_msgs::MultiEchoLaserScanPtr msg(new sensor_msgs::MultiEchoLaserScan());
514  if (urg_->grabScan(msg))
515  {
516  echoes_pub_.publish(msg);
517  echoes_freq_->tick();
518  }
519  else
520  {
521  ROS_WARN_THROTTLE(10.0, "Could not grab multi echo scan.");
522  device_status_ = urg_->getSensorStatus();
523  error_count_++;
524  }
525  }
526  else
527  {
528  const sensor_msgs::LaserScanPtr msg(new sensor_msgs::LaserScan());
529  if (urg_->grabScan(msg))
530  {
531  laser_pub_.publish(msg);
532  laser_freq_->tick();
533  }
534  else
535  {
536  ROS_WARN_THROTTLE(10.0, "Could not grab single echo scan.");
537  device_status_ = urg_->getSensorStatus();
538  error_count_++;
539  }
540  }
541  }
542  catch (...)
543  {
544  ROS_ERROR_THROTTLE(10.0, "Unknown error grabbing Hokuyo scan.");
545  error_count_++;
546  }
547 
548  if (service_yield_)
549  {
550  boost::this_thread::sleep_for(boost::chrono::milliseconds(10));
551  service_yield_ = false;
552  }
553 
554  // Reestablish conneciton if things seem to have gone wrong.
556  {
557  ROS_ERROR_THROTTLE(10.0, "Error count exceeded limit, reconnecting.");
558  urg_.reset();
559  ros::Duration(2.0).sleep();
560 
561  break; // Return to top of main loop
562  }
563  }
564  }
565 }
566 
568 {
569  // Setup initial connection
570  connect();
571 
572  // Stop diagnostics
573  if (!close_diagnostics_)
574  {
575  close_diagnostics_ = true;
576  diagnostics_thread_.join();
577  }
578 
579  if (publish_multiecho_)
580  {
584  }
585  else
586  {
590  }
591 
592  // Now that we are setup, kick off diagnostics.
593  close_diagnostics_ = false;
594  diagnostics_thread_ = boost::thread(boost::bind(&UrgNode::updateDiagnostics, this));
595 
596  // Start scanning now that everything is configured.
597  close_scan_ = false;
598  scan_thread_ = boost::thread(boost::bind(&UrgNode::scanThread, this));
599 }
600 } // namespace urg_node
msg
static LaserPublisher advertiseLaser(ros::NodeHandle &nh, uint32_t queue_size, bool latch=false)
ros::Publisher status_pub_
std::string product_name_
#define ROS_FATAL(...)
bool updateStatus()
Trigger an update of the lidar&#39;s status publish the latest known information about the lidar on latch...
void publish(const boost::shared_ptr< M > &message) const
std::string protocol_version_
boost::shared_ptr< urg_node::URGCWrapper > urg_
void summary(unsigned char lvl, const std::string s)
std::string vendor_name_
double diagnostics_window_time_
bool sleep() const
boost::shared_ptr< diagnostic_updater::HeaderlessTopicDiagnostic > laser_freq_
std::string serial_port_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ros::NodeHandle pnh_
boost::thread diagnostics_thread_
#define ROS_WARN(...)
boost::shared_ptr< diagnostic_updater::Updater > diagnostic_updater_
boost::thread scan_thread_
boost::shared_ptr< diagnostic_updater::HeaderlessTopicDiagnostic > echoes_freq_
void summaryf(unsigned char lvl, const char *format,...)
std::string resolve(const std::string &prefix, const std::string &frame_name)
std::string firmware_version_
boost::mutex lidar_mutex_
std::string device_status_
volatile bool service_yield_
#define ROS_WARN_THROTTLE(period,...)
laser_proc::LaserPublisher echoes_pub_
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define ROS_ERROR_THROTTLE(period,...)
std::string ip_address_
ROSCPP_DECL bool ok()
std::string device_id_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle nh_
bool statusCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
std::string firmware_date_
boost::shared_ptr< dynamic_reconfigure::Server< urg_node::URGConfig > > srv_
Dynamic reconfigure server.
#define ROS_INFO_STREAM(args)
int min(int a, int b)
void publish(const sensor_msgs::MultiEchoLaserScan &msg) const
diagnostic_updater::FrequencyStatusParam FrequencyStatusParam
ROSCPP_DECL void shutdown()
ros::Publisher laser_pub_
void add(const std::string &key, const T &val)
ros::ServiceServer status_service_
ROSCPP_DECL void spinOnce()
void populateDiagnosticsStatus(diagnostic_updater::DiagnosticStatusWrapper &stat)
void run()
Start&#39;s the nodes threads to run the lidar.
#define ROS_ERROR(...)
#define ROS_DEBUG_THROTTLE(period,...)
bool reconfigure_callback(urg_node::URGConfig &config, int level)


urg_node
Author(s): Chad Rockey , Mike O'Driscoll
autogenerated on Wed Jun 5 2019 19:29:02