Fawkes API  Fawkes Development Version
urg_gbx_aqt.cpp
1 
2 /***************************************************************************
3  * urg_gbx_aqt.cpp - Thread for Hokuyo URG using the Gearbox library
4  *
5  * Created: Fri Dec 04 20:47:50 2009 (at Frankfurt Airport)
6  * Copyright 2008-2009 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_gbx_aqt.h"
24 
25 #include <core/threading/mutex.h>
26 
27 #ifdef HAVE_URG_GBX_9_11
28 # include <hokuyo_aist/hokuyo_aist.h>
29 #else
30 # include <hokuyoaist/hokuyoaist.h>
31 #endif
32 #include <flexiport/flexiport.h>
33 
34 #include <cmath>
35 #include <cstdio>
36 #include <cstdlib>
37 #include <memory>
38 #include <string>
39 
40 #ifdef HAVE_URG_GBX_9_11
41 using namespace hokuyo_aist;
42 #else
43 using namespace hokuyoaist;
44 #endif
45 using namespace fawkes;
46 
47 /** @class HokuyoUrgGbxAcquisitionThread "urg_gbx_aqt.h"
48  * Laser acqusition thread for Hokuyo URG laser range finders.
49  * This thread fetches the data from the laser. This implementation uses
50  * the Gearbox library.
51  * @author Tim Niemueller
52  */
53 
54 /** Constructor.
55  * @param cfg_name short name of configuration group
56  * @param cfg_prefix configuration path prefix
57  */
59  std::string &cfg_prefix)
60 : LaserAcquisitionThread("HokuyoUrgGbxAcquisitionThread")
61 {
62  set_name("HokuyoURG_GBX(%s)", cfg_name.c_str());
63  pre_init_done_ = false;
64  cfg_name_ = cfg_name;
65  cfg_prefix_ = cfg_prefix;
66 }
67 
68 void
70 {
71  if (pre_init_done_)
72  return;
73 
74  number_of_values_ = _distances_size = 360;
75 
76  pre_init_done_ = true;
77 }
78 
79 void
81 {
83 
84  cfg_device_ = config->get_string((cfg_prefix_ + "device").c_str());
85 
86 #ifdef HAVE_URG_GBX_9_11
87  laser_ = new HokuyoLaser();
88 # if __cplusplus >= 201103L
89  std::unique_ptr<HokuyoLaser> laser(laser_);
90 # else
91  std::auto_ptr<HokuyoLaser> laser(laser_);
92 # endif
93 #else
94  laser_ = new Sensor();
95 # if __cplusplus >= 201103L
96  std::unique_ptr<Sensor> laser(laser_);
97 # else
98  std::auto_ptr<Sensor> laser(laser_);
99 # endif
100 #endif
101  std::string port_options = "type=serial,device=" + cfg_device_ + ",timeout=1";
102  try {
103 #ifdef HAVE_URG_GBX_9_11
104  laser_->Open(port_options);
105 #else
106  laser_->open(port_options);
107 #endif
108  } catch (flexiport::PortException &e) {
109  throw Exception("Connecting to URG laser failed: %s", e.what());
110  }
111 
112 #ifdef HAVE_URG_GBX_9_11
113  HokuyoSensorInfo info;
114  laser_->GetSensorInfo(&info);
115 
116  data_ = new HokuyoData();
117  first_ray_ = info.firstStep;
118  last_ray_ = info.lastStep;
119  front_ray_ = info.frontStep;
120 
121 #else
122  SensorInfo info;
123  laser_->get_sensor_info(info);
124  data_ = new ScanData();
125 
126  first_ray_ = info.first_step;
127  last_ray_ = info.last_step;
128  front_ray_ = info.front_step;
129 #endif
130 
131  slit_division_ = info.steps;
132  num_rays_ = last_ray_ - first_ray_;
133  front_idx_ = front_ray_ - first_ray_;
134 
135  step_per_angle_ = slit_division_ / 360.;
136  angle_per_step_ = 360. / slit_division_;
137  angular_range_ = (last_ray_ - first_ray_) * angle_per_step_;
138 
139  logger->log_info(name(), "VEND: %s", info.vendor.c_str());
140  logger->log_info(name(), "PROD: %s", info.product.c_str());
141  logger->log_info(name(), "FIRM: %s", info.firmware.c_str());
142  logger->log_info(name(), "PROT: %s", info.protocol.c_str());
143  logger->log_info(name(), "SERI: %s", info.serial.c_str());
144  logger->log_info(name(),
145  "Rays range: %u..%u, front at %u (idx %u), "
146  "%u rays total",
147  first_ray_,
148  last_ray_,
149  front_ray_,
150  front_idx_,
151  num_rays_);
152  logger->log_info(name(), "Slit Division: %u", slit_division_);
153  logger->log_info(name(), "Step/Angle: %f", step_per_angle_);
154  logger->log_info(name(), "Angle/Step: %f deg", angle_per_step_);
155  logger->log_info(name(), "Angular Range: %f deg", angular_range_);
156 
157  alloc_distances(number_of_values_);
158 #ifdef HAVE_URG_GBX_9_11
159  laser_->SetPower(true);
160 #else
161  laser_->set_power(true);
162 #endif
163 
164  laser.release();
165 }
166 
167 void
169 {
170  free(_distances);
171  _distances = NULL;
172 
173  logger->log_debug(name(), "Stopping laser");
174 #ifdef HAVE_URG_GBX_9_11
175  laser_->SetPower(false);
176 #else
177  laser_->set_power(false);
178 #endif
179  delete laser_;
180  delete data_;
181 }
182 
183 void
185 {
186  // static Time ref(clock);
187  // static Time now(clock);
188  // static unsigned int scans = 0;
189 
190  // now.stamp();
191  // if (now - &ref >= 1) {
192  // logger->log_debug(name(), "Current: %u scans/sec", scans);
193  // scans = 0;
194  // ref = now;
195  // } else {
196  // ++scans;
197  // }
198 
199  try {
200  // GetNewRanges is causes scans/sec to be halfed
201 #ifdef HAVE_URG_GBX_9_11
202  laser_->GetRanges(data_);
203  } catch (HokuyoError &he) {
204 #else
205  laser_->get_ranges(*data_);
206  } catch (BaseError &he) {
207 #endif
208  logger->log_warn(name(), "Failed to read data: %s", he.what());
209  return;
210  }
211 
212 #ifdef HAVE_URG_GBX_9_11
213  const uint32_t *ranges = data_->Ranges();
214 #else
215  const uint32_t *ranges = data_->ranges();
216 #endif
217 
218  _data_mutex->lock();
219 
220  _new_data = true;
221  _timestamp->stamp();
222  for (unsigned int a = 0; a < 360; ++a) {
223  unsigned int frontrel_idx = front_idx_ + roundf(a * step_per_angle_);
224  unsigned int idx = frontrel_idx % slit_division_;
225  if (idx <= num_rays_) {
226  // div by 1000.f: mm -> m
227  _distances[a] = ranges[idx] / 1000.f;
228  }
229  }
230  _data_mutex->unlock();
231 }
HokuyoUrgGbxAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
Definition: urg_gbx_aqt.cpp:58
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
Definition: urg_gbx_aqt.cpp:80
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
Definition: urg_gbx_aqt.cpp:69
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.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
Interface for configuration handling.
Definition: config.h:65
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
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 & stamp()
Set this time to the current time.
Definition: time.cpp:704
Fawkes library namespace.