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
42extern "C" {
43# endif
44# include <libudev.h>
45# ifdef __cplusplus
46}
47# endif
48#endif
49
50using namespace qrk;
51using 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
73void
75{
76 if (pre_init_done_)
77 return;
78
79 number_of_values_ = _distances_size = 360;
80
81 pre_init_done_ = true;
82}
83
84void
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) {
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) {
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
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 {
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
296void
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
312void
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 }
390 //} else {
391 //logger->log_warn(name(), "No new scan available, ignoring");
392 }
393
394 timer_->wait();
395}
396
397std::map<std::string, std::string>
398HokuyoUrgAcquisitionThread::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)) {
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:68
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 noexcept
Get primary string.
Definition: exception.cpp:639
void append(const char *format,...) noexcept
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.