25 #include <core/threading/mutex.h> 27 #include <urg/RangeSensorParameter.h> 28 #include <urg/UrgCtrl.h> 29 #include <utils/time/wait.h> 64 std::string &cfg_prefix)
67 set_name(
"HokuyoURG(%s)", cfg_name.c_str());
68 pre_init_done_ =
false;
70 cfg_prefix_ = cfg_prefix;
81 pre_init_done_ =
true;
100 struct udev_enumerate * enumerate;
101 struct udev_list_entry *devices, *dev_list_entry;
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)
116 struct udev_device *dev, *usb_device;
118 path = udev_list_entry_get_name(dev_list_entry);
119 dev = udev_device_new_from_syspath(udev, path);
121 usb_device = udev_device_get_parent_with_subsystem_devtype(dev,
"usb",
"usb_device");
122 if (!dev || !usb_device)
125 if ((strcmp(udev_device_get_sysattr_value(usb_device,
"manufacturer"),
126 "Hokuyo Data Flex for USB")
128 && (strcmp(udev_device_get_sysattr_value(usb_device,
"product"),
129 "URG-Series USB Driver")
131 const char *devpath = udev_device_get_devnode(dev);
132 int urgfd = open(devpath, 0, O_RDONLY);
135 "Failed to probe %s, cannot open file: %s",
140 if (flock(urgfd, LOCK_EX | LOCK_NB) != 0) {
142 "Failed to probe %s, cannot lock file: %s",
149 if (!probe_ctrl.connect(devpath)) {
151 flock(urgfd, LOCK_UN);
156 std::map<std::string, std::string> devinfo;
158 devinfo = get_device_info(&probe_ctrl);
161 flock(urgfd, LOCK_UN);
165 flock(urgfd, LOCK_UN);
168 if (devinfo[
"SERI"] == cfg_serial_) {
169 cfg_device_ = devpath;
172 "Matching URG at %s (vendor: %s (%s), " 173 "product: %s (%s), serial %s)",
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());
184 "Non-matching URG with serial %s at %s",
185 devinfo[
"SERI"].c_str(),
190 udev_enumerate_unref(enumerate);
193 if (cfg_device_ ==
"") {
194 throw Exception(
"No Hokuyo URG with serial %s found", cfg_serial_.c_str());
206 ctrl_ =
new UrgCtrl();
207 #if __cplusplus >= 201103L 208 std::unique_ptr<UrgCtrl> ctrl(ctrl_);
210 std::auto_ptr<UrgCtrl> ctrl(ctrl_);
212 fd_ = open(cfg_device_.c_str(), 0, O_RDONLY);
214 throw Exception(errno,
"Failed to open URG device %s", cfg_device_.c_str());
216 if (flock(fd_, LOCK_EX | LOCK_NB) != 0) {
218 throw Exception(
"Failed to acquire lock for URG device %s", cfg_device_.c_str());
220 if (!ctrl_->connect(cfg_device_.c_str())) {
223 throw Exception(
"Connecting to URG laser failed: %s", ctrl_->what());
226 ctrl_->setCaptureMode(AutoCapture);
227 device_info_ = get_device_info(ctrl_);
229 if (device_info_.find(
"PROD") == device_info_.end()) {
232 throw Exception(
"Failed to read product info for URG laser");
236 std::map<std::string, std::string>::iterator di;
237 for (di = device_info_.begin(); di != device_info_.end(); ++di) {
241 scan_msec_ = ctrl_->scanMsec();
242 float distance_min = 0.;
243 float distance_max = 0.;
246 first_ray_ =
config->
get_uint((cfg_prefix_ +
"first_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());
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.;
262 step_per_angle_ = slit_division_ / 360.;
263 angle_per_step_ = 360. / slit_division_;
264 angular_range_ = (last_ray_ - first_ray_) * angle_per_step_;
267 logger->
log_info(
name(),
"Rays range: %u..%u, front at %u", first_ray_, last_ray_, front_ray_);
275 cfg_time_offset_ = 0.;
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;
283 cfg_time_offset_ +=
config->
get_float((cfg_prefix_ +
"time_offset").c_str());
317 std::vector<long> values;
318 int num_values = ctrl_->capture(values);
319 if (num_values > 0) {
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_)) {
333 _distances[a] = std::numeric_limits<float>::quiet_NaN();
336 _distances[a] = std::numeric_limits<float>::quiet_NaN();
339 _distances[a] = std::numeric_limits<float>::quiet_NaN();
342 _distances[a] = std::numeric_limits<float>::quiet_NaN();
345 _distances[a] = std::numeric_limits<float>::quiet_NaN();
348 _distances[a] = std::numeric_limits<float>::quiet_NaN();
351 _distances[a] = std::numeric_limits<float>::quiet_NaN();
354 _distances[a] = std::numeric_limits<float>::quiet_NaN();
357 _distances[a] = std::numeric_limits<float>::quiet_NaN();
360 _distances[a] = std::numeric_limits<float>::quiet_NaN();
363 _distances[a] = std::numeric_limits<float>::quiet_NaN();
366 _distances[a] = std::numeric_limits<float>::quiet_NaN();
369 _distances[a] = std::numeric_limits<float>::quiet_NaN();
372 _distances[a] = std::numeric_limits<float>::quiet_NaN();
375 _distances[a] = std::numeric_limits<float>::quiet_NaN();
378 _distances[a] = std::numeric_limits<float>::quiet_NaN();
381 _distances[a] = std::numeric_limits<float>::quiet_NaN();
397 std::map<std::string, std::string>
398 HokuyoUrgAcquisitionThread::get_device_info(qrk::UrgCtrl *ctrl)
400 std::map<std::string, std::string> device_info;
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());
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;
419 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.