Fawkes API  Fawkes Development Version
urg_aqt.cpp
1 
2 /***************************************************************************
3  * urg_aqt.cpp - Thread to retrieve laser data from Hokuyo URG
4  *
5  * Created: Sat Nov 28 01:31:26 2009
6  * Copyright 2008-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "urg_aqt.h"
24 
25 #include <core/threading/mutex.h>
26 #include <sys/file.h>
27 #include <urg/RangeSensorParameter.h>
28 #include <urg/UrgCtrl.h>
29 #include <utils/time/wait.h>
30 
31 #include <cerrno>
32 #include <cmath>
33 #include <cstdio>
34 #include <cstdlib>
35 #include <limits>
36 #include <memory>
37 #include <string>
38 #include <unistd.h>
39 #ifdef HAVE_LIBUDEV
40 # include <cstring>
41 # ifdef __cplusplus
42 extern "C" {
43 # endif
44 # include <libudev.h>
45 # ifdef __cplusplus
46 }
47 # endif
48 #endif
49 
50 using namespace qrk;
51 using namespace fawkes;
52 
53 /** @class HokuyoUrgAcquisitionThread "urg_aqt.h"
54  * Laser acqusition thread for Hokuyo URG laser range finders.
55  * This thread fetches the data from the laser.
56  * @author Tim Niemueller
57  */
58 
59 /** Constructor.
60  * @param cfg_name short name of configuration group
61  * @param cfg_prefix configuration path prefix
62  */
64  std::string &cfg_prefix)
65 : LaserAcquisitionThread("HokuyoUrgAcquisitionThread")
66 {
67  set_name("HokuyoURG(%s)", cfg_name.c_str());
68  pre_init_done_ = false;
69  cfg_name_ = cfg_name;
70  cfg_prefix_ = cfg_prefix;
71 }
72 
73 void
75 {
76  if (pre_init_done_)
77  return;
78 
79  number_of_values_ = _distances_size = 360;
80 
81  pre_init_done_ = true;
82 }
83 
84 void
86 {
88 
89 #ifdef HAVE_LIBUDEV
90  try {
91  cfg_device_ = config->get_string((cfg_prefix_ + "device").c_str());
92  } catch (Exception &e) {
93  // check if bus/port numbers are given
94  try {
95  cfg_device_ = "";
96  cfg_serial_ = config->get_string((cfg_prefix_ + "serial").c_str());
97 
98  // try to find device using udev
99  struct udev * udev;
100  struct udev_enumerate * enumerate;
101  struct udev_list_entry *devices, *dev_list_entry;
102  udev = udev_new();
103  if (!udev) {
104  throw Exception("HokuyoURG: Failed to initialize udev for "
105  "device detection");
106  }
107 
108  enumerate = udev_enumerate_new(udev);
109  udev_enumerate_add_match_subsystem(enumerate, "tty");
110  udev_enumerate_scan_devices(enumerate);
111 
112  devices = udev_enumerate_get_list_entry(enumerate);
113  udev_list_entry_foreach(dev_list_entry, devices)
114  {
115  const char * path;
116  struct udev_device *dev, *usb_device;
117 
118  path = udev_list_entry_get_name(dev_list_entry);
119  dev = udev_device_new_from_syspath(udev, path);
120 
121  usb_device = udev_device_get_parent_with_subsystem_devtype(dev, "usb", "usb_device");
122  if (!dev || !usb_device)
123  continue;
124 
125  if ((strcmp(udev_device_get_sysattr_value(usb_device, "manufacturer"),
126  "Hokuyo Data Flex for USB")
127  == 0)
128  && (strcmp(udev_device_get_sysattr_value(usb_device, "product"),
129  "URG-Series USB Driver")
130  == 0)) {
131  const char *devpath = udev_device_get_devnode(dev);
132  int urgfd = open(devpath, 0, O_RDONLY);
133  if (urgfd == -1) {
134  logger->log_info(name(),
135  "Failed to probe %s, cannot open file: %s",
136  devpath,
137  strerror(errno));
138  continue;
139  }
140  if (flock(urgfd, LOCK_EX | LOCK_NB) != 0) {
141  logger->log_info(name(),
142  "Failed to probe %s, cannot lock file: %s",
143  devpath,
144  strerror(errno));
145  close(urgfd);
146  continue;
147  }
148  UrgCtrl probe_ctrl;
149  if (!probe_ctrl.connect(devpath)) {
150  logger->log_info(name(), "Failed to probe %s: %s", devpath, probe_ctrl.what());
151  flock(urgfd, LOCK_UN);
152  close(urgfd);
153  continue;
154  }
155 
156  std::map<std::string, std::string> devinfo;
157  try {
158  devinfo = get_device_info(&probe_ctrl);
159  } catch (Exception &e) {
160  logger->log_info(name(), "Failed to probe device info %s: %s", devpath, e.what());
161  flock(urgfd, LOCK_UN);
162  close(urgfd);
163  continue;
164  }
165  flock(urgfd, LOCK_UN);
166  close(urgfd);
167 
168  if (devinfo["SERI"] == cfg_serial_) {
169  cfg_device_ = devpath;
170 
171  logger->log_info(name(),
172  "Matching URG at %s (vendor: %s (%s), "
173  "product: %s (%s), serial %s)",
174  devpath,
175  udev_device_get_sysattr_value(usb_device, "manufacturer"),
176  udev_device_get_sysattr_value(usb_device, "idVendor"),
177  udev_device_get_sysattr_value(usb_device, "product"),
178  udev_device_get_sysattr_value(usb_device, "idProduct"),
179  devinfo["SERI"].c_str());
180 
181  break;
182  } else {
183  logger->log_info(name(),
184  "Non-matching URG with serial %s at %s",
185  devinfo["SERI"].c_str(),
186  devpath);
187  }
188  }
189  }
190  udev_enumerate_unref(enumerate);
191  udev_unref(udev);
192 
193  if (cfg_device_ == "") {
194  throw Exception("No Hokuyo URG with serial %s found", cfg_serial_.c_str());
195  }
196 
197  } catch (Exception &e2) {
198  e.append(e2);
199  throw e;
200  }
201  }
202 #else
203  cfg_device_ = config->get_string((cfg_prefix_ + "device").c_str());
204 #endif
205 
206  ctrl_ = new UrgCtrl();
207 #if __cplusplus >= 201103L
208  std::unique_ptr<UrgCtrl> ctrl(ctrl_);
209 #else
210  std::auto_ptr<UrgCtrl> ctrl(ctrl_);
211 #endif
212  fd_ = open(cfg_device_.c_str(), 0, O_RDONLY);
213  if (fd_ == -1) {
214  throw Exception(errno, "Failed to open URG device %s", cfg_device_.c_str());
215  }
216  if (flock(fd_, LOCK_EX | LOCK_NB) != 0) {
217  close(fd_);
218  throw Exception("Failed to acquire lock for URG device %s", cfg_device_.c_str());
219  }
220  if (!ctrl_->connect(cfg_device_.c_str())) {
221  close(fd_);
222  flock(fd_, LOCK_UN);
223  throw Exception("Connecting to URG laser failed: %s", ctrl_->what());
224  }
225 
226  ctrl_->setCaptureMode(AutoCapture);
227  device_info_ = get_device_info(ctrl_);
228 
229  if (device_info_.find("PROD") == device_info_.end()) {
230  close(fd_);
231  flock(fd_, LOCK_UN);
232  throw Exception("Failed to read product info for URG laser");
233  }
234 
235  logger->log_info(name(), "Using device file %s", cfg_device_.c_str());
236  std::map<std::string, std::string>::iterator di;
237  for (di = device_info_.begin(); di != device_info_.end(); ++di) {
238  logger->log_info(name(), "%s: %s", di->first.c_str(), di->second.c_str());
239  }
240 
241  scan_msec_ = ctrl_->scanMsec();
242  float distance_min = 0.;
243  float distance_max = 0.;
244 
245  try {
246  first_ray_ = config->get_uint((cfg_prefix_ + "first_ray").c_str());
247  last_ray_ = config->get_uint((cfg_prefix_ + "last_ray").c_str());
248  front_ray_ = config->get_uint((cfg_prefix_ + "front_ray").c_str());
249  slit_division_ = config->get_uint((cfg_prefix_ + "slit_division").c_str());
250  } catch (Exception &e) {
251  logger->log_info(name(), "No or incomplete config data, reading from device");
252  // Get data from device
253  RangeSensorParameter p = ctrl_->parameter();
254  first_ray_ = p.area_min;
255  last_ray_ = p.area_max;
256  front_ray_ = p.area_front;
257  slit_division_ = p.area_total;
258  distance_min = p.distance_min / 1000.;
259  distance_max = p.distance_max / 1000.;
260  }
261 
262  step_per_angle_ = slit_division_ / 360.;
263  angle_per_step_ = 360. / slit_division_;
264  angular_range_ = (last_ray_ - first_ray_) * angle_per_step_;
265 
266  logger->log_info(name(), "Time per scan: %li msec", scan_msec_);
267  logger->log_info(name(), "Rays range: %u..%u, front at %u", first_ray_, last_ray_, front_ray_);
268  logger->log_info(name(), "Slit Division: %u", slit_division_);
269  logger->log_info(name(), "Step/Angle: %f", step_per_angle_);
270  logger->log_info(name(), "Angle/Step: %f deg", angle_per_step_);
271  logger->log_info(name(), "Angular Range: %f deg", angular_range_);
272  logger->log_info(name(), "Min dist: %f m", distance_min);
273  logger->log_info(name(), "Max dist: %f m", distance_max);
274 
275  cfg_time_offset_ = 0.;
276  try {
277  float time_factor = config->get_float((cfg_prefix_ + "time_offset_scan_time_factor").c_str());
278  cfg_time_offset_ = (scan_msec_ / -1000.) * time_factor;
279  } catch (Exception &e) {
280  } // ignored, use default
281 
282  try {
283  cfg_time_offset_ += config->get_float((cfg_prefix_ + "time_offset").c_str());
284  } catch (Exception &e) {
285  } // ignored, use default
286 
287  // that should be 1000 really to convert msec -> usec. But empirically
288  // the results are slightly better with 990 as factor.
289  timer_ = new TimeWait(clock, scan_msec_ * 990);
290 
291  alloc_distances(number_of_values_);
292 
293  ctrl.release();
294 }
295 
296 void
298 {
299  free(_distances);
300  _distances = NULL;
301  delete timer_;
302 
303  ctrl_->stop();
304  delete ctrl_;
305 
306  close(fd_);
307  flock(fd_, LOCK_UN);
308 
309  logger->log_debug(name(), "Stopping laser");
310 }
311 
312 void
314 {
315  timer_->mark_start();
316 
317  std::vector<long> values;
318  int num_values = ctrl_->capture(values);
319  if (num_values > 0) {
320  //logger->log_debug(name(), "Captured %i values", num_values);
321  _data_mutex->lock();
322 
323  _new_data = true;
324  _timestamp->stamp();
325  *_timestamp += cfg_time_offset_;
326  for (unsigned int a = 0; a < 360; ++a) {
327  unsigned int front_idx = front_ray_ + roundf(a * step_per_angle_);
328  unsigned int idx = front_idx % slit_division_;
329  if ((idx >= first_ray_) && (idx <= last_ray_)) {
330  switch (values[idx]) // See the SCIP2.0 reference on page 12, Table 3
331  {
332  case 0: // Detected object is possibly at 22m
333  _distances[a] = std::numeric_limits<float>::quiet_NaN();
334  break;
335  case 1: // Reflected light has low intensity
336  _distances[a] = std::numeric_limits<float>::quiet_NaN();
337  break;
338  case 2: // Reflected light has low intensity
339  _distances[a] = std::numeric_limits<float>::quiet_NaN();
340  break;
341  case 6: // Others
342  _distances[a] = std::numeric_limits<float>::quiet_NaN();
343  break;
344  case 7: // Distance data on the preceding and succeeding steps have errors
345  _distances[a] = std::numeric_limits<float>::quiet_NaN();
346  break;
347  case 8: // Intensity difference of two waves
348  _distances[a] = std::numeric_limits<float>::quiet_NaN();
349  break;
350  case 9: // The same step had error in the last two scan
351  _distances[a] = std::numeric_limits<float>::quiet_NaN();
352  break;
353  case 10: // Others
354  _distances[a] = std::numeric_limits<float>::quiet_NaN();
355  break;
356  case 11: // Others
357  _distances[a] = std::numeric_limits<float>::quiet_NaN();
358  break;
359  case 12: // Others
360  _distances[a] = std::numeric_limits<float>::quiet_NaN();
361  break;
362  case 13: // Others
363  _distances[a] = std::numeric_limits<float>::quiet_NaN();
364  break;
365  case 14: // Others
366  _distances[a] = std::numeric_limits<float>::quiet_NaN();
367  break;
368  case 15: // Others
369  _distances[a] = std::numeric_limits<float>::quiet_NaN();
370  break;
371  case 16: // Others
372  _distances[a] = std::numeric_limits<float>::quiet_NaN();
373  break;
374  case 17: // Others
375  _distances[a] = std::numeric_limits<float>::quiet_NaN();
376  break;
377  case 18: // Error reading due to strong reflective object
378  _distances[a] = std::numeric_limits<float>::quiet_NaN();
379  break;
380  case 19: // Non-Measurable step
381  _distances[a] = std::numeric_limits<float>::quiet_NaN();
382  break;
383  default:
384  // div by 1000.f: mm -> m
385  _distances[a] = values[idx] / 1000.f;
386  }
387  }
388  }
389  _data_mutex->unlock();
390  //} else {
391  //logger->log_warn(name(), "No new scan available, ignoring");
392  }
393 
394  timer_->wait();
395 }
396 
397 std::map<std::string, std::string>
398 HokuyoUrgAcquisitionThread::get_device_info(qrk::UrgCtrl *ctrl)
399 {
400  std::map<std::string, std::string> device_info;
401 
402  std::vector<std::string> version_info;
403  if (ctrl->versionLines(version_info)) {
404  for (unsigned int i = 0; i < version_info.size(); ++i) {
405  std::string::size_type colon_idx = version_info[i].find(":");
406  std::string::size_type semi_colon_idx = version_info[i].find(";");
407  if ((colon_idx == std::string::npos) || (semi_colon_idx == std::string::npos)) {
408  logger->log_warn(name(),
409  "Could not understand version info string '%s'",
410  version_info[i].c_str());
411  } else {
412  std::string::size_type val_len = semi_colon_idx - colon_idx - 1;
413  std::string key = version_info[i].substr(0, colon_idx);
414  std::string value = version_info[i].substr(colon_idx + 1, val_len);
415  device_info[key] = value;
416  }
417  }
418  } else {
419  throw Exception("Failed retrieving version info: %s", ctrl->what());
420  }
421  return device_info;
422 }
HokuyoUrgAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
Definition: urg_aqt.cpp:63
virtual void loop()
Code to execute in the thread.
Definition: urg_aqt.cpp:313
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
Definition: urg_aqt.cpp:74
virtual void init()
Initialize the thread.
Definition: urg_aqt.cpp:85
virtual void finalize()
Finalize the thread.
Definition: urg_aqt.cpp:297
Laser acqusition thread.
float * _distances
Allocate a float array and copy your distance values measured in meters here.
fawkes::Mutex * _data_mutex
Lock while writing to distances or echoes array or marking new data.
bool _new_data
Set to true in your loop if new data is available.
void alloc_distances(unsigned int num_distances)
Allocate distances array.
unsigned int _distances_size
Assign this the size of the _distances array.
fawkes::Time * _timestamp
Time when the most recent data was received.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
Interface for configuration handling.
Definition: config.h:65
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:333
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
Time wait utility.
Definition: wait.h:33
void mark_start()
Mark start of loop.
Definition: wait.cpp:68
void wait()
Wait until minimum loop time has been reached.
Definition: wait.cpp:78
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
Fawkes library namespace.