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