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 <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 # ifdef __cplusplus
43 extern "C" {
44 # endif
45 # include <libudev.h>
46 # ifdef __cplusplus
47 }
48 # endif
49 #endif
50 
51 using namespace qrk;
52 using namespace fawkes;
53 
54 
55 
56 /** @class HokuyoUrgAcquisitionThread "urg_aqt.h"
57  * Laser acqusition thread for Hokuyo URG laser range finders.
58  * This thread fetches the data from the laser.
59  * @author Tim Niemueller
60  */
61 
62 
63 /** Constructor.
64  * @param cfg_name short name of configuration group
65  * @param cfg_prefix configuration path prefix
66  */
68  std::string &cfg_prefix)
69  : LaserAcquisitionThread("HokuyoUrgAcquisitionThread")
70 {
71  set_name("HokuyoURG(%s)", cfg_name.c_str());
72  __pre_init_done = false;
73  __cfg_name = cfg_name;
74  __cfg_prefix = cfg_prefix;
75 }
76 
77 
78 void
81 {
82  if (__pre_init_done) return;
83 
84  __number_of_values = _distances_size = 360;
85 
86  __pre_init_done = true;
87 }
88 
89 void
91 {
93 
94 #ifdef HAVE_LIBUDEV
95  try {
96  __cfg_device = config->get_string((__cfg_prefix + "device").c_str());
97  } catch (Exception &e) {
98  // check if bus/port numbers are given
99  try {
100  __cfg_device = "";
101  __cfg_serial = config->get_string((__cfg_prefix + "serial").c_str());
102 
103  // try to find device using udev
104  struct udev *udev;
105  struct udev_enumerate *enumerate;
106  struct udev_list_entry *devices, *dev_list_entry;
107  struct udev_device *dev, *usb_device;
108  udev = udev_new();
109  if (!udev) {
110  throw Exception("HokuyoURG: Failed to initialize udev for "
111  "device detection");
112  }
113 
114  enumerate = udev_enumerate_new(udev);
115  udev_enumerate_add_match_subsystem(enumerate, "tty");
116  udev_enumerate_scan_devices(enumerate);
117 
118  devices = udev_enumerate_get_list_entry(enumerate);
119  udev_list_entry_foreach(dev_list_entry, devices) {
120 
121  const char *path;
122 
123  path = udev_list_entry_get_name(dev_list_entry);
124  dev = udev_device_new_from_syspath(udev, path);
125 
126  usb_device = udev_device_get_parent_with_subsystem_devtype(dev, "usb",
127  "usb_device");
128  if (! dev || ! usb_device) continue;
129 
130  if ( (strcmp(udev_device_get_sysattr_value(usb_device,"manufacturer"),
131  "Hokuyo Data Flex for USB") == 0) &&
132  (strcmp(udev_device_get_sysattr_value(usb_device,"product"),
133  "URG-Series USB Driver") == 0) )
134  {
135 
136  const char *devpath = udev_device_get_devnode(dev);
137  int urgfd = open(devpath, 0, O_RDONLY);
138  if (urgfd == -1) {
139  logger->log_info(name(), "Failed to probe %s, cannot open file: %s",
140  devpath, strerror(errno));
141  continue;
142  }
143  if (flock(urgfd, LOCK_EX | LOCK_NB) != 0) {
144  logger->log_info(name(), "Failed to probe %s, cannot lock file: %s",
145  devpath, strerror(errno));
146  close(urgfd);
147  continue;
148  }
149  UrgCtrl probe_ctrl;
150  if ( ! probe_ctrl.connect(devpath) ) {
151  logger->log_info(name(), "Failed to probe %s: %s", devpath,
152  probe_ctrl.what());
153  flock(urgfd, LOCK_UN);
154  close(urgfd);
155  continue;
156  }
157 
158  std::map<std::string, std::string> devinfo;
159  try {
160  devinfo = get_device_info(&probe_ctrl);
161  } catch (Exception &e) {
162  logger->log_info(name(), "Failed to probe device info %s: %s",
163  devpath, e.what());
164  flock(urgfd, LOCK_UN);
165  close(urgfd);
166  continue;
167  }
168  flock(urgfd, LOCK_UN);
169  close(urgfd);
170 
171  if (devinfo["SERI"] == __cfg_serial) {
172  __cfg_device = devpath;
173 
174  logger->log_info(
175  name(), "Matching URG at %s (vendor: %s (%s), "
176  "product: %s (%s), serial %s)", devpath,
177  udev_device_get_sysattr_value(usb_device, "manufacturer"),
178  udev_device_get_sysattr_value(usb_device, "idVendor"),
179  udev_device_get_sysattr_value(usb_device, "product"),
180  udev_device_get_sysattr_value(usb_device, "idProduct"),
181  devinfo["SERI"].c_str());
182 
183  break;
184  } else {
185  logger->log_info(name(), "Non-matching URG with serial %s at %s",
186  devinfo["SERI"].c_str(), 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",
195  __cfg_serial.c_str());
196  }
197 
198  } catch (Exception &e2) {
199  e.append(e2);
200  throw e;
201  }
202  }
203 #else
204  __cfg_device = config->get_string((__cfg_prefix + "device").c_str());
205 #endif
206 
207  __ctrl = new UrgCtrl();
208 #if __cplusplus >= 201103L
209  std::unique_ptr<UrgCtrl> ctrl(__ctrl);
210 #else
211  std::auto_ptr<UrgCtrl> ctrl(__ctrl);
212 #endif
213  __fd = open(__cfg_device.c_str(), 0, O_RDONLY);
214  if (__fd == -1) {
215  throw Exception(errno, "Failed to open URG device %s", __cfg_device.c_str());
216  }
217  if (flock(__fd, LOCK_EX | LOCK_NB) != 0) {
218  close(__fd);
219  throw Exception("Failed to acquire lock for URG device %s", __cfg_device.c_str());
220  }
221  if ( ! __ctrl->connect(__cfg_device.c_str()) ) {
222  close(__fd);
223  flock(__fd, LOCK_UN);
224  throw Exception("Connecting to URG laser failed: %s", __ctrl->what());
225  }
226 
227  __ctrl->setCaptureMode(AutoCapture);
228  __device_info = get_device_info(__ctrl);
229 
230  if (__device_info.find("PROD") == __device_info.end()) {
231  close(__fd);
232  flock(__fd, LOCK_UN);
233  throw Exception("Failed to read product info for URG laser");
234  }
235 
236  logger->log_info(name(), "Using device file %s", __cfg_device.c_str());
237  std::map<std::string, std::string>::iterator di;
238  for (di = __device_info.begin(); di != __device_info.end(); ++di) {
239  logger->log_info(name(), "%s: %s", di->first.c_str(), di->second.c_str());
240  }
241 
242  __scan_msec = __ctrl->scanMsec();
243  float distance_min = 0.;
244  float distance_max = 0.;
245 
246  try {
247  __first_ray = config->get_uint((__cfg_prefix + "first_ray").c_str());
248  __last_ray = config->get_uint((__cfg_prefix + "last_ray").c_str());
249  __front_ray = config->get_uint((__cfg_prefix + "front_ray").c_str());
250  __slit_division = config->get_uint((__cfg_prefix + "slit_division").c_str());
251  } catch (Exception &e) {
252  logger->log_info(name(), "No or incomplete config data, reading from device");
253  // Get data from device
254  RangeSensorParameter p = __ctrl->parameter();
255  __first_ray = p.area_min;
256  __last_ray = p.area_max;
257  __front_ray = p.area_front;
258  __slit_division = p.area_total;
259  distance_min = p.distance_min / 1000.;
260  distance_max = p.distance_max / 1000.;
261  }
262 
263  __step_per_angle = __slit_division / 360.;
264  __angle_per_step = 360. / __slit_division;
265  __angular_range = (__last_ray - __first_ray) * __angle_per_step;
266 
267  logger->log_info(name(), "Time per scan: %li msec", __scan_msec);
268  logger->log_info(name(), "Rays range: %u..%u, front at %u",
269  __first_ray, __last_ray, __front_ray);
270  logger->log_info(name(), "Slit Division: %u", __slit_division);
271  logger->log_info(name(), "Step/Angle: %f", __step_per_angle);
272  logger->log_info(name(), "Angle/Step: %f deg", __angle_per_step);
273  logger->log_info(name(), "Angular Range: %f deg", __angular_range);
274  logger->log_info(name(), "Min dist: %f m", distance_min);
275  logger->log_info(name(), "Max dist: %f m", distance_max);
276 
277 
278  __cfg_time_offset = 0.;
279  try {
280  float time_factor =
281  config->get_float((__cfg_prefix + "time_offset_scan_time_factor").c_str());
282  __cfg_time_offset = (__scan_msec / -1000.) * time_factor;
283  } catch (Exception &e) {} // ignored, use default
284 
285  try {
286  __cfg_time_offset += config->get_float((__cfg_prefix + "time_offset").c_str());
287  } catch (Exception &e) {} // ignored, use default
288 
289  // that should be 1000 really to convert msec -> usec. But empirically
290  // the results are slightly better with 990 as factor.
291  __timer = new TimeWait(clock, __scan_msec * 990);
292 
293  alloc_distances(__number_of_values);
294 
295  ctrl.release();
296 }
297 
298 
299 void
301 {
302  free(_distances);
303  _distances = NULL;
304  delete __timer;
305 
306  __ctrl->stop();
307  delete __ctrl;
308 
309  close(__fd);
310  flock(__fd, LOCK_UN);
311 
312  logger->log_debug(name(), "Stopping laser");
313 }
314 
315 
316 void
318 {
319  __timer->mark_start();
320 
321  std::vector<long> values;
322  int num_values = __ctrl->capture(values);
323  if (num_values > 0) {
324  //logger->log_debug(name(), "Captured %i values", num_values);
325  _data_mutex->lock();
326 
327  _new_data = true;
328  _timestamp->stamp();
329  *_timestamp += __cfg_time_offset;
330  for (unsigned int a = 0; a < 360; ++a) {
331  unsigned int front_idx = __front_ray + roundf(a * __step_per_angle);
332  unsigned int idx = front_idx % __slit_division;
333  if ( (idx >= __first_ray) && (idx <= __last_ray) ) {
334  switch (values[idx]) // See the SCIP2.0 reference on page 12, Table 3
335  {
336  case 0: // Detected object is possibly at 22m
337  _distances[a] = std::numeric_limits<float>::quiet_NaN();
338  break;
339  case 1: // Reflected light has low intensity
340  _distances[a] = std::numeric_limits<float>::quiet_NaN();
341  break;
342  case 2: // Reflected light has low intensity
343  _distances[a] = std::numeric_limits<float>::quiet_NaN();
344  break;
345  case 6: // Others
346  _distances[a] = std::numeric_limits<float>::quiet_NaN();
347  break;
348  case 7: // Distance data on the preceding and succeeding steps have errors
349  _distances[a] = std::numeric_limits<float>::quiet_NaN();
350  break;
351  case 8: // Intensity difference of two waves
352  _distances[a] = std::numeric_limits<float>::quiet_NaN();
353  break;
354  case 9: // The same step had error in the last two scan
355  _distances[a] = std::numeric_limits<float>::quiet_NaN();
356  break;
357  case 10: // Others
358  _distances[a] = std::numeric_limits<float>::quiet_NaN();
359  break;
360  case 11: // Others
361  _distances[a] = std::numeric_limits<float>::quiet_NaN();
362  break;
363  case 12: // Others
364  _distances[a] = std::numeric_limits<float>::quiet_NaN();
365  break;
366  case 13: // Others
367  _distances[a] = std::numeric_limits<float>::quiet_NaN();
368  break;
369  case 14: // Others
370  _distances[a] = std::numeric_limits<float>::quiet_NaN();
371  break;
372  case 15: // Others
373  _distances[a] = std::numeric_limits<float>::quiet_NaN();
374  break;
375  case 16: // Others
376  _distances[a] = std::numeric_limits<float>::quiet_NaN();
377  break;
378  case 17: // Others
379  _distances[a] = std::numeric_limits<float>::quiet_NaN();
380  break;
381  case 18: // Error reading due to strong reflective object
382  _distances[a] = std::numeric_limits<float>::quiet_NaN();
383  break;
384  case 19: // Non-Measurable step
385  _distances[a] = std::numeric_limits<float>::quiet_NaN();
386  break;
387  default:
388  // div by 1000.f: mm -> m
389  _distances[a] = values[idx] / 1000.f;
390  }
391  }
392  }
393  _data_mutex->unlock();
394  //} else {
395  //logger->log_warn(name(), "No new scan available, ignoring");
396  }
397 
398  __timer->wait();
399 }
400 
401 std::map<std::string, std::string>
402  HokuyoUrgAcquisitionThread::get_device_info(qrk::UrgCtrl *ctrl)
403 {
404  std::map<std::string, std::string> device_info;
405 
406  std::vector<std::string> version_info;
407  if (ctrl->versionLines(version_info)) {
408  for (unsigned int i = 0; i < version_info.size(); ++i) {
409  std::string::size_type colon_idx = version_info[i].find(":");
410  std::string::size_type semi_colon_idx = version_info[i].find(";");
411  if ((colon_idx == std::string::npos) ||
412  (semi_colon_idx == std::string::npos)) {
413  logger->log_warn(name(), "Could not understand version info string '%s'",
414  version_info[i].c_str());
415  } else {
416  std::string::size_type val_len = semi_colon_idx - colon_idx - 1;
417  std::string key = version_info[i].substr(0, colon_idx);
418  std::string value = version_info[i].substr(colon_idx+1, val_len);
419  device_info[key] = value;
420  }
421  }
422  } else {
423  throw Exception("Failed retrieving version info: %s", ctrl->what());
424  }
425  return device_info;
426 }
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.
Fawkes library namespace.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:135
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
HokuyoUrgAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
Definition: urg_aqt.cpp:67
virtual void init()
Initialize the thread.
Definition: urg_aqt.cpp:90
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::Time * _timestamp
Time when the most recent data was received.
Definition: urg_aqt.h:31
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:317
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:761
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:300
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:79
const char * name() const
Get name of thread.
Definition: thread.h:95
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.
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
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
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:67
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
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