25 #include <core/threading/mutex.h>
26 #include <utils/time/wait.h>
28 #include <urg/UrgCtrl.h>
29 #include <urg/RangeSensorParameter.h>
62 std::string &cfg_prefix)
65 set_name(
"HokuyoURG(%s)", cfg_name.c_str());
66 __pre_init_done =
false;
67 __cfg_name = cfg_name;
68 __cfg_prefix = cfg_prefix;
76 if (__pre_init_done)
return;
80 __pre_init_done =
true;
99 struct udev_enumerate *enumerate;
100 struct udev_list_entry *devices, *dev_list_entry;
101 struct udev_device *dev, *usb_device;
104 throw Exception(
"HokuyoURG: Failed to initialize udev for "
108 enumerate = udev_enumerate_new(udev);
109 udev_enumerate_add_match_subsystem(enumerate,
"tty");
110 udev_enumerate_scan_devices(enumerate);
112 devices = udev_enumerate_get_list_entry(enumerate);
113 udev_list_entry_foreach(dev_list_entry, devices) {
117 path = udev_list_entry_get_name(dev_list_entry);
118 dev = udev_device_new_from_syspath(udev, path);
120 usb_device = udev_device_get_parent_with_subsystem_devtype(dev,
"usb",
122 if (! dev || ! usb_device)
continue;
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) )
130 const char *devpath = udev_device_get_devnode(dev);
131 int urgfd = open(devpath, 0, O_RDONLY);
134 devpath, strerror(errno));
137 if (flock(urgfd, LOCK_EX | LOCK_NB) != 0) {
139 devpath, strerror(errno));
144 if ( ! probe_ctrl.connect(devpath) ) {
147 flock(urgfd, LOCK_UN);
152 std::map<std::string, std::string> devinfo;
154 devinfo = get_device_info(&probe_ctrl);
158 flock(urgfd, LOCK_UN);
162 flock(urgfd, LOCK_UN);
165 if (devinfo[
"SERI"] == __cfg_serial) {
166 __cfg_device = devpath;
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());
180 devinfo[
"SERI"].c_str(), devpath);
184 udev_enumerate_unref(enumerate);
187 if (__cfg_device ==
"") {
188 throw Exception(
"No Hokuyo URG with serial %s found",
189 __cfg_serial.c_str());
201 __ctrl =
new UrgCtrl();
202 std::auto_ptr<UrgCtrl> ctrl(__ctrl);
203 __fd = open(__cfg_device.c_str(), 0, O_RDONLY);
205 throw Exception(errno,
"Failed to open URG device %s", __cfg_device.c_str());
207 if (flock(__fd, LOCK_EX | LOCK_NB) != 0) {
209 throw Exception(
"Failed to acquire lock for URG device %s", __cfg_device.c_str());
211 if ( ! __ctrl->connect(__cfg_device.c_str()) ) {
213 flock(__fd, LOCK_UN);
214 throw Exception(
"Connecting to URG laser failed: %s", __ctrl->what());
217 __ctrl->setCaptureMode(AutoCapture);
218 __device_info = get_device_info(__ctrl);
220 if (__device_info.find(
"PROD") == __device_info.end()) {
222 flock(__fd, LOCK_UN);
223 throw Exception(
"Failed to read product info for URG laser");
227 std::map<std::string, std::string>::iterator di;
228 for (di = __device_info.begin(); di != __device_info.end(); ++di) {
232 int scan_msec = __ctrl->scanMsec();
233 float distance_min = 0.;
234 float distance_max = 0.;
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());
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.;
253 __step_per_angle = __slit_division / 360.;
254 __angle_per_step = 360. / __slit_division;
255 __angular_range = (__last_ray - __first_ray) * __angle_per_step;
259 __first_ray, __last_ray, __front_ray);
288 flock(__fd, LOCK_UN);
299 std::vector<long> values;
300 int num_values = __ctrl->capture(values);
301 if (num_values > 0) {
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) ) {
313 _distances[a] = std::numeric_limits<float>::quiet_NaN();
316 _distances[a] = std::numeric_limits<float>::quiet_NaN();
319 _distances[a] = std::numeric_limits<float>::quiet_NaN();
322 _distances[a] = std::numeric_limits<float>::quiet_NaN();
325 _distances[a] = std::numeric_limits<float>::quiet_NaN();
328 _distances[a] = std::numeric_limits<float>::quiet_NaN();
331 _distances[a] = std::numeric_limits<float>::quiet_NaN();
334 _distances[a] = std::numeric_limits<float>::quiet_NaN();
337 _distances[a] = std::numeric_limits<float>::quiet_NaN();
340 _distances[a] = std::numeric_limits<float>::quiet_NaN();
343 _distances[a] = std::numeric_limits<float>::quiet_NaN();
346 _distances[a] = std::numeric_limits<float>::quiet_NaN();
349 _distances[a] = std::numeric_limits<float>::quiet_NaN();
352 _distances[a] = std::numeric_limits<float>::quiet_NaN();
355 _distances[a] = std::numeric_limits<float>::quiet_NaN();
358 _distances[a] = std::numeric_limits<float>::quiet_NaN();
361 _distances[a] = std::numeric_limits<float>::quiet_NaN();
377 std::map<std::string, std::string>
378 HokuyoUrgAcquisitionThread::get_device_info(qrk::UrgCtrl *ctrl)
380 std::map<std::string, std::string> device_info;
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)) {
390 version_info[i].c_str());
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;
399 throw Exception(
"Failed retrieving version info: %s", ctrl->what());
void wait()
Wait until minimum loop time has been reached.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Fawkes library namespace.
void unlock()
Unlock the mutex.
HokuyoUrgAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
virtual void init()
Initialize the thread.
Logger * logger
This is the Logger member used to access the logger.
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.
virtual void loop()
Code to execute in the thread.
void set_name(const char *format,...)
Set name of thread.
virtual const char * what() const
Get primary string.
Base class for exceptions in Fawkes.
unsigned int _distances_size
Assign this the size of the _distances array.
virtual void finalize()
Finalize the thread.
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.
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.
void mark_start()
Mark start of loop.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void lock()
Lock this mutex.
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.
Interface for configuration handling.
void append(const char *format,...)
Append messages to the message list.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.