25 #include <core/threading/mutex.h> 26 #include <utils/time/wait.h> 28 #include <urg/UrgCtrl.h> 29 #include <urg/RangeSensorParameter.h> 68 std::string &cfg_prefix)
71 set_name(
"HokuyoURG(%s)", cfg_name.c_str());
72 __pre_init_done =
false;
73 __cfg_name = cfg_name;
74 __cfg_prefix = cfg_prefix;
82 if (__pre_init_done)
return;
86 __pre_init_done =
true;
105 struct udev_enumerate *enumerate;
106 struct udev_list_entry *devices, *dev_list_entry;
107 struct udev_device *dev, *usb_device;
110 throw Exception(
"HokuyoURG: Failed to initialize udev for " 114 enumerate = udev_enumerate_new(udev);
115 udev_enumerate_add_match_subsystem(enumerate,
"tty");
116 udev_enumerate_scan_devices(enumerate);
118 devices = udev_enumerate_get_list_entry(enumerate);
119 udev_list_entry_foreach(dev_list_entry, devices) {
123 path = udev_list_entry_get_name(dev_list_entry);
124 dev = udev_device_new_from_syspath(udev, path);
126 usb_device = udev_device_get_parent_with_subsystem_devtype(dev,
"usb",
128 if (! dev || ! usb_device)
continue;
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) )
136 const char *devpath = udev_device_get_devnode(dev);
137 int urgfd = open(devpath, 0, O_RDONLY);
140 devpath, strerror(errno));
143 if (flock(urgfd, LOCK_EX | LOCK_NB) != 0) {
145 devpath, strerror(errno));
150 if ( ! probe_ctrl.connect(devpath) ) {
153 flock(urgfd, LOCK_UN);
158 std::map<std::string, std::string> devinfo;
160 devinfo = get_device_info(&probe_ctrl);
164 flock(urgfd, LOCK_UN);
168 flock(urgfd, LOCK_UN);
171 if (devinfo[
"SERI"] == __cfg_serial) {
172 __cfg_device = devpath;
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());
186 devinfo[
"SERI"].c_str(), devpath);
190 udev_enumerate_unref(enumerate);
193 if (__cfg_device ==
"") {
194 throw Exception(
"No Hokuyo URG with serial %s found",
195 __cfg_serial.c_str());
207 __ctrl =
new UrgCtrl();
208 #if __cplusplus >= 201103L 209 std::unique_ptr<UrgCtrl> ctrl(__ctrl);
211 std::auto_ptr<UrgCtrl> ctrl(__ctrl);
213 __fd = open(__cfg_device.c_str(), 0, O_RDONLY);
215 throw Exception(errno,
"Failed to open URG device %s", __cfg_device.c_str());
217 if (flock(__fd, LOCK_EX | LOCK_NB) != 0) {
219 throw Exception(
"Failed to acquire lock for URG device %s", __cfg_device.c_str());
221 if ( ! __ctrl->connect(__cfg_device.c_str()) ) {
223 flock(__fd, LOCK_UN);
224 throw Exception(
"Connecting to URG laser failed: %s", __ctrl->what());
227 __ctrl->setCaptureMode(AutoCapture);
228 __device_info = get_device_info(__ctrl);
230 if (__device_info.find(
"PROD") == __device_info.end()) {
232 flock(__fd, LOCK_UN);
233 throw Exception(
"Failed to read product info for URG laser");
237 std::map<std::string, std::string>::iterator di;
238 for (di = __device_info.begin(); di != __device_info.end(); ++di) {
242 __scan_msec = __ctrl->scanMsec();
243 float distance_min = 0.;
244 float distance_max = 0.;
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());
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.;
263 __step_per_angle = __slit_division / 360.;
264 __angle_per_step = 360. / __slit_division;
265 __angular_range = (__last_ray - __first_ray) * __angle_per_step;
269 __first_ray, __last_ray, __front_ray);
278 __cfg_time_offset = 0.;
281 config->
get_float((__cfg_prefix +
"time_offset_scan_time_factor").c_str());
282 __cfg_time_offset = (__scan_msec / -1000.) * time_factor;
286 __cfg_time_offset +=
config->
get_float((__cfg_prefix +
"time_offset").c_str());
310 flock(__fd, LOCK_UN);
321 std::vector<long> values;
322 int num_values = __ctrl->capture(values);
323 if (num_values > 0) {
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) ) {
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();
364 _distances[a] = std::numeric_limits<float>::quiet_NaN();
367 _distances[a] = std::numeric_limits<float>::quiet_NaN();
370 _distances[a] = std::numeric_limits<float>::quiet_NaN();
373 _distances[a] = std::numeric_limits<float>::quiet_NaN();
376 _distances[a] = std::numeric_limits<float>::quiet_NaN();
379 _distances[a] = std::numeric_limits<float>::quiet_NaN();
382 _distances[a] = std::numeric_limits<float>::quiet_NaN();
385 _distances[a] = std::numeric_limits<float>::quiet_NaN();
401 std::map<std::string, std::string>
402 HokuyoUrgAcquisitionThread::get_device_info(qrk::UrgCtrl *ctrl)
404 std::map<std::string, std::string> device_info;
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)) {
414 version_info[i].c_str());
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;
423 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.
virtual const char * what() const
Get primary string.
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::Time * _timestamp
Time when the most recent data was received.
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.
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.
const char * name() const
Get name of thread.
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.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void lock()
Lock this mutex.
Time & stamp()
Set this time to the current time.
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.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
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.